ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
Inputs:
Q(4,...) Real-valued quaternion array (possibly unnormalized)
Outputs:
R(3,3,...) Rotation matrix array
Plots a diagram if no output specified
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.
Copyright (C) Mike Brookes 2007-2018
Version: $Id: v_rotqr2ro.m 10865 2018-09-21 17:22:45Z dmb $
VOICEBOX is a MATLAB toolbox for speech processing.
Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You can obtain a copy of the GNU General Public License from
http://www.gnu.org/copyleft/gpl.html or by writing to
Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 function r=v_rotqr2ro(q) 0002 %ROTQR2RO converts a real quaternion to a 3x3 rotation matrix 0003 % Inputs: 0004 % 0005 % Q(4,...) Real-valued quaternion array (possibly unnormalized) 0006 % 0007 % Outputs: 0008 % 0009 % R(3,3,...) Rotation matrix array 0010 % Plots a diagram if no output specified 0011 % 0012 % In the quaternion representation of a rotation, and q(1) = cos(t/2) 0013 % where t is the angle of rotation in the range 0 to 2pi 0014 % and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation 0015 % a positive rotation about [0 0 1] takes the X axis towards the Y axis. 0016 % 0017 % Copyright (C) Mike Brookes 2007-2018 0018 % Version: $Id: v_rotqr2ro.m 10865 2018-09-21 17:22:45Z dmb $ 0019 % 0020 % VOICEBOX is a MATLAB toolbox for speech processing. 0021 % Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html 0022 % 0023 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0024 % This program is free software; you can redistribute it and/or modify 0025 % it under the terms of the GNU General Public License as published by 0026 % the Free Software Foundation; either version 2 of the License, or 0027 % (at your option) any later version. 0028 % 0029 % This program is distributed in the hope that it will be useful, 0030 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0031 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0032 % GNU General Public License for more details. 0033 % 0034 % You can obtain a copy of the GNU General Public License from 0035 % http://www.gnu.org/copyleft/gpl.html or by writing to 0036 % Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. 0037 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0038 0039 persistent a b c d e f g h m 0040 if isempty(a) 0041 a=[1 5 9]; 0042 b=[11 16 6]; 0043 c=[16 6 11]; 0044 d=[4 8 3]; 0045 e=[10 15 14]; 0046 f=[4 2 3]; 0047 g=[2 6 7]; 0048 h=[1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4]'; 0049 m=[1 1 1 1 2 2 2 2 3 3 3 3 4 4 4 4]'; 0050 end 0051 sz=size(q); 0052 q=reshape(q,4,[]); % convert to 2D matrix 0053 nq =size(q,2); % number of quaternions to convert 0054 p=2*q(h,:).*q(m,:)./repmat(sum(q.^2,1),16,1); % force normalized and calculate quadratic terms 0055 r=zeros(9,nq); % space for nq rotation matrices 0056 r(a,:)=1-p(b,:)-p(c,:); 0057 r(d,:)=p(e,:)-p(f,:); 0058 r(g,:)=p(e,:)+p(f,:); 0059 r=reshape(r,[3 3 sz(2:end)]); 0060 if ~nargout 0061 % display rotated cube 0062 q1=q(:,1); 0063 q1=q1/sqrt(q1'*q1); % normalize the first quaternion 0064 clf('reset'); % clear current axis 0065 v0=[-1 1 1 -1 -1 1 1 -1; -1 -1 1 1 -1 -1 1 1; -1 -1 -1 -1 1 1 1 1]*0.5; % unrotated coordinates 0066 v=r(:,:,1)*v0; % use first elemnt of r to rotate 0067 fv=[4 1 5 8; 2 3 7 6; 1 2 6 5; 3 4 8 7; 2 1 4 3; 5 6 7 8]; % verices for each face 0068 fc=[0 1 1; 1 0 0; 1 0 1; 0 1 0; 1 1 0; 0 0 1]; % colours for faces 0069 xc={[25 46 46 25; 55 55 49 49]/100, 0070 [25 33 33 39 39 46 46 39 39 33 33 25; 55 55 63 63 55 55 49 49 42 42 49 49]/100, 0071 [48 56 62 69 76 66 76 69 62 56 48 59; 68 68 58 68 68 53 37 37 47 37 37 53]/100, 0072 [48 55 62 69 77 65 65 59 59; 68 68 55 68 68 50 37 37 50]/100, 0073 [50 74 74 57 74 74 50 50 67 50; 68 68 62 43 43 37 37 43 62 62]/100}; 0074 xf=[1 3; 2 3; 1 4; 2 4; 1 5; 2 5]; % characters to plot on each face 0075 nf=size(fv,1); % number of faces 0076 for i=1:6 0077 p(i)=patch(v(1,fv(i,:)),v(2,fv(i,:)),v(3,fv(i,:)),fc(i,:)); 0078 set(p(i),'FaceAlpha',0.65); 0079 k=1.001; % factor to move out labels slightly to get correct depth ordering 0080 for j=1:2 0081 xij=xc{xf(i,j)}; % relative coordinates of character vertices 0082 patch(k*(v(1,fv(i,1))+(v(1,fv(i,2))-v(1,fv(i,1)))*xij(1,:)+(v(1,fv(i,4))-v(1,fv(i,1)))*xij(2,:)), ... 0083 k*(v(2,fv(i,1))+(v(2,fv(i,2))-v(2,fv(i,1)))*xij(1,:)+(v(2,fv(i,4))-v(2,fv(i,1)))*xij(2,:)), ... 0084 k*(v(3,fv(i,1))+(v(3,fv(i,2))-v(3,fv(i,1)))*xij(1,:)+(v(3,fv(i,4))-v(3,fv(i,1)))*xij(2,:)),1-fc(i,:)); 0085 end 0086 end 0087 qa=q1(2:4); 0088 qm=max(abs(qa)); 0089 if qm>1e-6*abs(q1(1)) 0090 qa=qa/(qm/0.7); % scale so axis extends outside the cube 0091 hold on; 0092 plot3([1 -1]*qa(1),[1 -1]*qa(2),[1 -1]*qa(3),'-'); 0093 hold off 0094 end 0095 th=360/pi*acos(abs(q1(1))); 0096 xlabel('x axis'); 0097 ylabel('y axis'); 0098 zlabel('z axis'); 0099 q=q(:,1)*((2*(q(find(q1(:)~=0,1))>0)-1)); % force leading coefficient to be positive 0100 title(sprintf('%d^\\circ, qr'' = [%.2f,%.2f,%.2f,%.2f], eu_{xyzo}'' = [%d, %d, %d]^\\circ',round(th),q1(:),round(v_rotqr2eu('xyz',q1)*180/pi))); 0101 axis([-1 1 -1 1 -1 1 0 1]*sqrt(3)/2); 0102 axis equal 0103 grid on 0104 view(3); 0105 end 0106