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.
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