rotro2eu

ROTRO2EQ converts a 3x3 rotation matrix into the corresponding euler angles
 Inputs: 

     M(1,3)   a string of 3 characters from the set {'x','y','z'} e.g. "zxz" or "zyx"
              or, equivalently, a vector whose elements are 1, 2 or 3
     RO(3,3)  3x3 rotation matrix

 Outputs:

     E(3,1)   3 euler angles in the range +-pi. A positive rotation is clockwise
               if looking along the axis away from the origin.

 The string M specifies the axes (fixed in space) about which the rotations of
 an object are performed. You cannot have the same axis in adjacent positions
 and so there are 12 possibilities. Common ones are "ZXZ" and "ZYX".
 If you want the axes to move with the object, you should reverse the
 ordering of both "m" and "e".

 There is some reduncancy in euler angle values:
  (i)  If m(1)==m(3) then e=[a b c] and e=[a+-pi -b c+-pi] are equivalent.
       The output of this routine will always have b>=0;
  (ii) If m(1)~=m(3) then e=[a b c] and e=[a+-pi pi-b c+-pi] are equivalent.
       The output of this routine will always have |b|<=pi/2

0001 function e=rotro2eu(m,ro)
0002 %ROTRO2EQ converts a 3x3 rotation matrix into the corresponding euler angles
0003 % Inputs:
0004 %
0005 %     M(1,3)   a string of 3 characters from the set {'x','y','z'} e.g. "zxz" or "zyx"
0006 %              or, equivalently, a vector whose elements are 1, 2 or 3
0007 %     RO(3,3)  3x3 rotation matrix
0008 %
0009 % Outputs:
0010 %
0011 %     E(3,1)   3 euler angles in the range +-pi. A positive rotation is clockwise
0012 %               if looking along the axis away from the origin.
0013 %
0014 % The string M specifies the axes (fixed in space) about which the rotations of
0015 % an object are performed. You cannot have the same axis in adjacent positions
0016 % and so there are 12 possibilities. Common ones are "ZXZ" and "ZYX".
0017 % If you want the axes to move with the object, you should reverse the
0018 % ordering of both "m" and "e".
0019 %
0020 % There is some reduncancy in euler angle values:
0021 %  (i)  If m(1)==m(3) then e=[a b c] and e=[a+-pi -b c+-pi] are equivalent.
0022 %       The output of this routine will always have b>=0;
0023 %  (ii) If m(1)~=m(3) then e=[a b c] and e=[a+-pi pi-b c+-pi] are equivalent.
0024 %       The output of this routine will always have |b|<=pi/2
0025 
0026 %
0027 %      Copyright (C) Mike Brookes 2007
0028 %      Version: $Id: rotro2eu.m 2187 2012-07-20 13:45:35Z dmb $
0029 %
0030 %   VOICEBOX is a MATLAB toolbox for speech processing.
0031 %   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
0032 %
0033 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0034 %   This program is free software; you can redistribute it and/or modify
0035 %   it under the terms of the GNU General Public License as published by
0036 %   the Free Software Foundation; either version 2 of the License, or
0037 %   (at your option) any later version.
0038 %
0039 %   This program is distributed in the hope that it will be useful,
0040 %   but WITHOUT ANY WARRANTY; without even the implied warranty of
0041 %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0042 %   GNU General Public License for more details.
0043 %
0044 %   You can obtain a copy of the GNU General Public License from
0045 %   http://www.gnu.org/copyleft/gpl.html or by writing to
0046 %   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
0047 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0048 
0049 e=zeros(3,1);
0050 if ischar(m)
0051     m=lower(m)-'w';
0052 end
0053 if numel(m)~=3 | any(abs(m-2)>1), error('Euler axis must be an x,y or z triplet'); end
0054 u=m(1);
0055 v=m(2);
0056 w=m(3);
0057 if sum(m==v)>1, error('Consecutive Euler axes must differ'); end
0058 % first we rotate around w to null element (v,u) with respect to element (!vw,u) of rotation matrix
0059 g=2*mod(u-v,3)-3;   % +1 if v follows u or -1 if u follows v
0060 h=2*mod(v-w,3)-3;   % +1 if w follows v or -1 if v follows w
0061 [s,c,r,e(3)]=atan2sc(h*ro(v,u),ro(6-v-w,u));
0062 r2=ro;
0063 ix=1+mod(w+(0:1),3);
0064 r2(ix,:)=[c s; -s c]*ro(ix,:);
0065 % next we rotate around v to null element (!uv,u) with repect to element (u,u) of rotation matrix
0066 e(2)=atan2(-g*r2(6-u-v,u),r2(u,u));
0067 % finally we rotate around u to null element (v,!uv) with respect to element (!uv,!uv) = element (v,v)
0068 e(1)=atan2(-g*r2(v,6-u-v),r2(v,v));
0069 if (u==w && e(2)<0) || (u~=w && abs(e(2))>pi/2)  % remove redundancy
0070     mk=u~=w;
0071     e(2)=(2*mk-1)*e(2);
0072     e=e-((2*(e>0)-1) .* [1; mk; 1])*pi;
0073 end
0074