rotqr2eu

ROTQR2EQ converts a real unit quaternion into the corresponding euler angles
 Inputs: 

     M(1,3)   a string of 3 characters from the set {'x','y','z'}
              or, equivalently, a vector whose elements are all 1, 2 or 3
     Q(3,3)   3x3 rotation matrix

 Outputs:

     E(3,1)   3 euler angles

 The string M specifies the axes about which the rotations are performed.
 You cannot have the same axis in adjacent positions and so there are 12
 possibilities. Common ones are "ZXZ" and "ZYX".

0001 function e=rotqr2eu(m,q)
0002 %ROTQR2EQ converts a real unit quaternion into the corresponding euler angles
0003 % Inputs:
0004 %
0005 %     M(1,3)   a string of 3 characters from the set {'x','y','z'}
0006 %              or, equivalently, a vector whose elements are all 1, 2 or 3
0007 %     Q(3,3)   3x3 rotation matrix
0008 %
0009 % Outputs:
0010 %
0011 %     E(3,1)   3 euler angles
0012 %
0013 % The string M specifies the axes about which the rotations are performed.
0014 % You cannot have the same axis in adjacent positions and so there are 12
0015 % possibilities. Common ones are "ZXZ" and "ZYX".
0016 
0017 % Suggestions:
0018 %   (1) Might be quicker to convert to a matrix and do it in that domain
0019 
0020 %
0021 %      Copyright (C) Mike Brookes 2007
0022 %      Version: $Id: rotqr2eu.m 713 2011-10-16 14:45:43Z dmb $
0023 %
0024 %   VOICEBOX is a MATLAB toolbox for speech processing.
0025 %   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
0026 %
0027 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0028 %   This program is free software; you can redistribute it and/or modify
0029 %   it under the terms of the GNU General Public License as published by
0030 %   the Free Software Foundation; either version 2 of the License, or
0031 %   (at your option) any later version.
0032 %
0033 %   This program is distributed in the hope that it will be useful,
0034 %   but WITHOUT ANY WARRANTY; without even the implied warranty of
0035 %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0036 %   GNU General Public License for more details.
0037 %
0038 %   You can obtain a copy of the GNU General Public License from
0039 %   http://www.gnu.org/copyleft/gpl.html or by writing to
0040 %   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
0041 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0042 
0043 % test: ea=2*pi*(rand(3,1)-0.5);m='yxy';q=roteu2qr(m,ea); e=rotqr2eu(m,q);[[ea*180/pi; 0] [e*180/pi; 0] q roteu2qr(m,e)]
0044 % redundancy: If m(1)==m(3) then q= [a b c] and [a+-pi -b c+-pi] are equivalent. The output always has b>=0
0045 %             If m(1) ~=m(3) then q=[a b c] and [a+-pi pi-b c+-pi] are equivalent. The output always has |b|<=pi/2
0046 % all angles are in the range +-pi
0047 e=zeros(3,1);
0048 y=[2 4 1 3 1 3 2 4; 3 2 1 4 1 4 3 2; 3 4 2 1 1 2 4 3];
0049 if ischar(m)
0050     m=lower(m)-'w';
0051 end
0052 if any(abs(m-2)>1), error('Euler axis must be x,y or z'); end
0053 u=m(1)+1;
0054 v=m(2)+1;
0055 w=m(3)+1;
0056 % first we rotate around w to null element (v,u) with respect to element (!vw,u) of rotation matrix
0057 g=2*mod(u-v,3)-3;
0058 ss=(2*mod(v-w,3)-3)*(q(v)*q(u)+g*q(9-u-v)*q(1));
0059 if u==w                 % if u==w then (!vw,u) is off-diagonal
0060     cc=q(9-v-w)*q(u)-g*q(v+w-u)*q(1);
0061 else               % if u~=w then (!vw,u)=(u,u) is on diagonal
0062     cc=q(1)^2+q(u)^2-0.5;
0063 end
0064 [ss,cc,rr,t]=atan2sc(ss,cc);
0065 if cc>0
0066     c=sqrt(0.5*(1+cc));
0067     s=0.5*ss/c;
0068 else
0069     s=sqrt(0.5*(1-cc));
0070     c=0.5*ss/s;
0071 end
0072 % s=sin(t/2);
0073 % c=cos(t/2);
0074 x=y(w-1,:);
0075 r=zeros(4,1);
0076 r(x(1:2))=q(x(3:4));
0077 r(x(5:6))=-q(x(7:8));
0078 q2=c*q-s*r;
0079 % next we rotate around v to null element (!uv,u) with repect to element (u,u) of rotation matrix
0080 ss2=-g*q2(9-u-v)*q2(u)+q2(v)*q2(1);     % always off-diagonal
0081 cc2=q2(1)^2+q2(u)^2-0.5;     % always on-diagonal
0082 [s2,c2,rr,t2]=atan2sc(ss2,cc2);
0083 x2=y(v-1,:);
0084 r2=zeros(4,1);
0085 r2(x2(1:2))=q2(x2(3:4));
0086 r2(x2(5:6))=-q2(x2(7:8));
0087 q3=c2*q2-s2*r2;
0088 if q3(1)<0, q3=-q3; end
0089 t3=2*atan2(q3(u),q3(1));
0090 e(1)=t3;
0091 e(2)=t2;
0092 e(3)=t;
0093 if (u==w && t2<0) || (u~=w && abs(t2)>pi/2)  % remove redundancy
0094     mk=u~=w;
0095     e(2)=(2*mk-1)*t2;
0096     e=e-((2*(e>0)-1) .* [1; mk; 1])*pi;
0097 end
0098