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

          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 Lesser General Public License as published by
       the Free Software Foundation; either version 3 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 Lesser General Public License for more details.

       You can obtain a copy of the GNU Lesser General Public License from
       https://www.gnu.org/licenses/ .

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

## 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.
    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 Lesser General Public License as published by
    0026 %   the Free Software Foundation; either version 3 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 Lesser General Public License for more details.
    0033 %
    0034 %   You can obtain a copy of the GNU Lesser General Public License from
    0035 %   https://www.gnu.org/licenses/ .
    0036 %
    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