# v_rotro2qr

## PURPOSE

V_ROTRO2QR converts a 3x3 rotation matrix to a real quaternion

## SYNOPSIS

function q=v_rotro2qr(r)

## DESCRIPTION

    V_ROTRO2QR converts a 3x3 rotation matrix to a real quaternion
     Inputs:

         R(3,3,...)   Input rotation matrix

     Outputs:

         Q(4,...)   normalized real-valued quaternion

     In the quaternion representation of a rotation, and q(1) = cos(t/2)
     where t is the angle of rotation in the range 0 to 2pi
     and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
     a positive rotation about [0 0 1] takes the X axis towards the Y axis.

## CROSS-REFERENCE INFORMATION

This function calls:

- v_rotqr2ro ROTQR2RO converts a real quaternion to a 3x3 rotation matrix

This function is called by:

- v_rectifyhomog V_RECTIFYHOMOG Apply rectifying homographies to an image set
- v_rotlu2ro V_ROTLU2RO converts look and up directions to a rotation matrix
- v_rotpl2ro V_ROTPL2RO find matrix to rotate in the plane containing u and v r=[u,v,t]
- v_rotro2eu V_ROTRO2EU converts a 3x3 rotation matrix into the corresponding euler angles
- v_rotro2lu V_ROTRO2QR converts a 3x3 rotation matrix to look and up directions
- v_rotro2pl V_ROTRO2PL find the plane and rotation angle of a rotation matrix [u,v,t]=r

## SOURCE CODE

    0001 function q=v_rotro2qr(r)
    0002 %V_ROTRO2QR converts a 3x3 rotation matrix to a real quaternion
    0003 % Inputs:
    0004 %
    0005 %     R(3,3,...)   Input rotation matrix
    0006 %
    0007 % Outputs:
    0008 %
    0009 %     Q(4,...)   normalized real-valued quaternion
    0010 %
    0011 % In the quaternion representation of a rotation, and q(1) = cos(t/2)
    0012 % where t is the angle of rotation in the range 0 to 2pi
    0013 % and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
    0014 % a positive rotation about [0 0 1] takes the X axis towards the Y axis.
    0015
    0016 %      Copyright (C) Mike Brookes 2007-2018
    0017 %      Version: $Id: v_rotro2qr.m 10865 2018-09-21 17:22:45Z dmb $
    0018 %
    0019 %   VOICEBOX is a MATLAB toolbox for speech processing.
    0020 %   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
    0021 %
    0022 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    0023 %   This program is free software; you can redistribute it and/or modify
    0024 %   it under the terms of the GNU Lesser General Public License as published by
    0025 %   the Free Software Foundation; either version 3 of the License, or
    0026 %   (at your option) any later version.
    0027 %
    0028 %   This program is distributed in the hope that it will be useful,
    0029 %   but WITHOUT ANY WARRANTY; without even the implied warranty of
    0030 %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    0031 %   GNU Lesser General Public License for more details.
    0032 %
    0033 %   You can obtain a copy of the GNU Lesser General Public License from
    0034 %   https://www.gnu.org/licenses/ .
    0035 %
    0036 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    0037
    0038 % in the comments below, t is the rotation angle, a is the rotation axis
    0039 sz=size(r);
    0040 r=reshape(r,9,[]); % make 2-dimensional
    0041 q=zeros(4,size(r,2));
    0042 d = 1+r(1,:)+r(5,:)+r(9,:);     % 2(1+cos(t)) = 4(cos(t/2))^2 = 4 q(1)^2
    0043 mm=d>1; % mask for rotation angles < 120 degrees
    0044 if any(mm)                       % for rotation angles less than 120 degrees
    0045     s = sqrt(d(mm))*2;            % 4 cos(t/2) = 2 sin(t)/sin(t/2)
    0046     q(2,mm) = (r(6,mm)-r(8,mm))./s;
    0047     q(3,mm) = (r(7,mm)-r(3,mm))./s;
    0048     q(4,mm) = (r(2,mm)-r(4,mm))./s;
    0049     q(1,mm) = 0.25*s;            % cos(t/2)
    0050 end
    0051 if any(~mm)
    0052     m=(r(1,:)>r(5,:)) & (r(1,:)>r(9,:)) & ~mm;
    0053     if any(m)
    0054         s  = sqrt( 1.0+r(1,m)-r(5,m)-r(9,m))*2;  % s>=2 always: s=4 a(1) sin(t/2) = 2 a(2) (1-cos(t))/sin(t/2)
    0055         q(2,m) = 0.25*s;
    0056         q(3,m) = (r(2,m)+r(4,m))./s;
    0057         q(4,m) = (r(7,m)+r(3,m))./s;
    0058         q(1,m) = (r(6,m)-r(8,m))./s;
    0059         mm=mm|m;
    0060     end
    0061     m=(r(5,:)>r(9,:)) & ~mm;
    0062     if any(m)
    0063         s  = sqrt( 1.0+r(5,m)-r(1,m)-r(9,m))*2; % s>=2 always: s=4 a(2) sin(t/2)
    0064         q(2,m) = (r(2,m)+r(4,m))./s;
    0065         q(3,m) = 0.25*s;
    0066         q(4,m) = (r(6,m)+r(8,m))./s;
    0067         q(1,m) = (r(7,m)-r(3,m))./s;
    0068         mm=mm|m;
    0069     end
    0070     if any(~mm)
    0071         m=~mm;
    0072         s  = sqrt( 1.0+r(9,m)-r(1,m)-r(5,m))*2;  % s>=2 always: s=4 a(3) sin(t/2)
    0073         q(2,m) = (r(7,m)+r(3,m))./s;
    0074         q(3,m) = (r(6,m)+r(8,m))./s;
    0075         q(4,m) = 0.25*s;
    0076         q(1,m) = (r(2,m)-r(4,m))./s;
    0077     end
    0078 end
    0079 if max(abs(sum(q.^2,1)-1))>1e-8; % check normalization
    0080     error('Input to rotro2qr must be a rotation matrix with det(r)=+1');
    0081 end
    0082 if length(sz)<3
    0083     sz=[4 1];
    0084 else
    0085     sz=[4 sz(3:end)];
    0086 end
    0087 q=reshape(q.*repmat(sign([8 4 2 1]*sign(q)),4,1),sz); % force leading coefficient to be positive and reshape
    0088 if ~nargout
    0089     v_rotqr2ro(q); % plot a cube
    0090 end