rotqr2ro

ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
 Inputs:

     Q(4,1)   real-valued quaternion (possibly unnormalized)

 Outputs:

     R(3,3)   Input rotation matrix
              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
      Version: $Id: rotqr2ro.m 5659 2015-02-02 20:21:49Z 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=rotqr2ro(q)
0002 %ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
0003 % Inputs:
0004 %
0005 %     Q(4,1)   real-valued quaternion (possibly unnormalized)
0006 %
0007 % Outputs:
0008 %
0009 %     R(3,3)   Input rotation matrix
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
0018 %      Version: $Id: rotqr2ro.m 5659 2015-02-02 20:21:49Z 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
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 end
0049 p=2*(q*q.')/(q.'*q);            % force normalized
0050 r=zeros(3,3);
0051 r(a)=1-p(b)-p(c);
0052 r(d)=p(e)-p(f);
0053 r(g)=p(e)+p(f);
0054 if ~nargout
0055     % display rotated cube
0056     clf('reset'); % clear current axis
0057     %     vv=[0,0,0;1,0,0;0,1,0;0,0,1]*r';  % pyramid
0058     %     ff=[1 2 4; 2 1 3; 3 1 4; 4 2 3];
0059     %     cc=[0 1 0; 0 0 1; 1 0 0; 1 1 0];
0060     vv=[0,0,0;1,0,0;0,1,0;0,0,1;0 1 1; 1 0 1; 1 1 0; 1 1 1]*r';    % cube
0061     ff=[1 2 6 4; 2 7 8 6; 7 3 5 8; 4 5 3 1; 3 7 2 1; 6 8 5 4];
0062     %     cc=[0 1 0; 1 0 0; 0 1 0; 1 0 0; 0 0 1; 0 0 1];
0063     cc=[1 0 1 0 2 2]';
0064     pa=patch('Vertices',vv,'Faces',ff,'FaceVertexCData',cc,'FaceColor','Flat');
0065     colormap([1 0 0; 0 1 0; 0 0 1]);
0066     xlabel('x axis');
0067     ylabel('y axis');
0068     zlabel('z axis');
0069     title(sprintf('qr = [%.2f, %.2f, %.2f, %.2f]''    initial xyz=0 are rgb',q))
0070     axis([-1 1 -1 1 -1 1 0 1]*sqrt(3));
0071     grid on
0072     view(3);
0073     axis equal;
0074 end
0075