rotqr2ax

ROTQR2AX converts a real quaternion to the corresponding rotation axis and angle
 Inputs: 

     Q(4,1)   real-valued quaternion (with magnitude = 1)

 Outputs:

     A(3,1)   Unit vector in the direction of the rotation axis.
     T        Rotation angle in radians (in range 0 to 2pi)

 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: rotqr2ax.m 713 2011-10-16 14:45:43Z 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 [a,t]=rotqr2ax(q)
0002 %ROTQR2AX converts a real quaternion to the corresponding rotation axis and angle
0003 % Inputs:
0004 %
0005 %     Q(4,1)   real-valued quaternion (with magnitude = 1)
0006 %
0007 % Outputs:
0008 %
0009 %     A(3,1)   Unit vector in the direction of the rotation axis.
0010 %     T        Rotation angle in radians (in range 0 to 2pi)
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: rotqr2ax.m 713 2011-10-16 14:45:43Z 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 a=q(2:4);
0039 m=sqrt(a'*a);
0040 t=2*atan2(m,q(1));      % avoids problems if unnormalized
0041 if m>0
0042     a=a/m;
0043 else
0044     a=[0 0 1]';
0045 end
0046