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: This function is called by:

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 General Public License as published by
0025 %   the Free Software Foundation; either version 2 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 General Public License for more details.
0032 %
0033 %   You can obtain a copy of the GNU General Public License from
0034 %   http://www.gnu.org/copyleft/gpl.html or by writing to
0035 %   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
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

Generated by m2html © 2003