v_rotqr2ro

PURPOSE

ROTQR2RO converts a real quaternion to a 3x3 rotation matrix

SYNOPSIS

function r=v_rotqr2ro(q)

DESCRIPTION

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

Version: \$Id: v_rotqr2ro.m 10865 2018-09-21 17:22:45Z dmb \$

VOICEBOX is a MATLAB toolbox for speech processing.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This program is free software; you can redistribute it and/or modify
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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%```

CROSS-REFERENCE INFORMATION

This function calls:
• v_rotqr2eu V_ROTQR2EQ converts a real unit quaternion into the corresponding euler angles
This function is called by:
• v_rectifyhomog V_RECTIFYHOMOG Apply rectifying homographies to an image set
• v_rotax2qr V_ROTQR2AX converts a rotation axis and angle to the corresponding real quaternion
• v_roteu2qr ROTEU2QR converts a sequence of Euler angles to a real unit quaternion
• v_roteu2ro V_ROTEU2QR converts a sequence of Euler angles to a rotation matrix
• v_rotmc2qc V_ROTMC2QC converts a matrix of complex quaternion matrices to a matrix of complex quaternion vectors
• v_rotmr2qr V_ROTMR2QR converts a matrix of real quaternion matrices to quaternion vectors
• v_rotpl2ro V_ROTPL2RO find matrix to rotate in the plane containing u and v r=[u,v,t]
• v_rotqc2mc V_ROTQC2MC converts a matrix of complex quaternion vectors to quaternion matrices
• v_rotqc2qr V_ROTQC2QR converts a matrix of complex quaternion row vectors into real form
• v_rotqr2ax V_ROTQR2AX converts a real quaternion to the corresponding rotation axis and angle
• v_rotqr2eu V_ROTQR2EQ converts a real unit quaternion into the corresponding euler angles
• v_rotqr2mr V_ROTQR2MR converts a matrix of real quaternion vectors to quaternion matrices
• v_rotqr2qc V_ROTQR2QC converts a matrix of real quaternion vectors into complex form
• v_rotqrvec V_ROTQRVEC applies a quaternion rotation ot a vector array y=[q,x]
• v_rotro2pl V_ROTRO2PL find the plane and rotation angle of a rotation matrix [u,v,t]=r
• v_rotro2qr V_ROTRO2QR converts a 3x3 rotation matrix to a real quaternion

SOURCE CODE

```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.
0022 %
0023 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0024 %   This program is free software; you can redistribute it and/or modify
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```