V_ROTRO2EU converts a 3x3 rotation matrix into the corresponding euler angles
inverse of v_roteu2ro
M a string of n characters from the set determining the order of rotation axes
as listed below. Note that the control characters 'rdoOaA' may occur anywhere in the string:
'x','y','z' rotate around the given axis by the corresponding angle
given in e()
'1','2','3' 90 degree rotation around x,y or z axis; doesn't use a value from e()
'4','5','6' 180 degree rotation around x,y or z axis; doesn't use a value from e()
'7','8','9' 270 degree rotation around x,y or z axis; doesn't use a value from e()
'r','d' all angles are given in radians or degrees [default='r']
'R','D' all angles are given in radians or degrees and are negated
'o','O','a','A' selects whether to rotate the object or the coordinate axes and
whether the rotation axes remain fixed in space for consecutive
rotations (extrinsic) or else move with each rotation (intrinsic).
'o' = object-extrinsic [default]
'O' = object-intrinsic
'a' = axes-extrinsic
'A' = axes-intrinsic
R(3,3,...) Input rotation matrix (or array of matrices)
Outputs:
E(K,...) K Euler angles in radians (or degrees if 'd' or 'D' specified) per quaternion where K is the number of 'xyz' characters in M.
A positive rotation is clockwise if looking along the +ve axis away from the origin or anti-clockwise if 'R' or 'D' is given.
The x, y, z axes form a right-handed triple.
The string M specifies the seqeunce of axes about which the rotations are performed. There are 12
possible 3-character sequences that avoid consecutive repetitions. These are 'Euler angles' if
there is a repeated axis or 'Tait-Bryan angles' if not. Common choices are:
(1) 'zxz' the most common Euler angle set
(2) 'xyz' corresponds to 'roll, pitch, yaw' for an aeroplane heading in the x direction with y to
the right and z down. The intrinsic equivalent is 'Ozyx' corresponding to 'yaw, pitch, roll'.
(3) 'z1z1z' involves 5 rotations, in which all the non-fixed rotations are around the z axis.
The Euler angles are not, in general, unique. In particular:
(1) v_roteu2ro('zxz',[a b c]) = v_roteu2ro('zxz',[a+pi -b c+pi])
(2) v_roteu2ro('xyz',[a b c]) = v_roteu2ro('xyz',[a+pi pi-b c+pi])0001 function e=v_rotro2eu(m,r) 0002 %V_ROTRO2EU converts a 3x3 rotation matrix into the corresponding euler angles 0003 % inverse of v_roteu2ro 0004 % M a string of n characters from the set determining the order of rotation axes 0005 % as listed below. Note that the control characters 'rdoOaA' may occur anywhere in the string: 0006 % 'x','y','z' rotate around the given axis by the corresponding angle 0007 % given in e() 0008 % '1','2','3' 90 degree rotation around x,y or z axis; doesn't use a value from e() 0009 % '4','5','6' 180 degree rotation around x,y or z axis; doesn't use a value from e() 0010 % '7','8','9' 270 degree rotation around x,y or z axis; doesn't use a value from e() 0011 % 'r','d' all angles are given in radians or degrees [default='r'] 0012 % 'R','D' all angles are given in radians or degrees and are negated 0013 % 'o','O','a','A' selects whether to rotate the object or the coordinate axes and 0014 % whether the rotation axes remain fixed in space for consecutive 0015 % rotations (extrinsic) or else move with each rotation (intrinsic). 0016 % 'o' = object-extrinsic [default] 0017 % 'O' = object-intrinsic 0018 % 'a' = axes-extrinsic 0019 % 'A' = axes-intrinsic 0020 % 0021 % R(3,3,...) Input rotation matrix (or array of matrices) 0022 % 0023 % Outputs: 0024 % 0025 % E(K,...) K Euler angles in radians (or degrees if 'd' or 'D' specified) per quaternion where K is the number of 'xyz' characters in M. 0026 % A positive rotation is clockwise if looking along the +ve axis away from the origin or anti-clockwise if 'R' or 'D' is given. 0027 % The x, y, z axes form a right-handed triple. 0028 % 0029 % The string M specifies the seqeunce of axes about which the rotations are performed. There are 12 0030 % possible 3-character sequences that avoid consecutive repetitions. These are 'Euler angles' if 0031 % there is a repeated axis or 'Tait-Bryan angles' if not. Common choices are: 0032 % (1) 'zxz' the most common Euler angle set 0033 % (2) 'xyz' corresponds to 'roll, pitch, yaw' for an aeroplane heading in the x direction with y to 0034 % the right and z down. The intrinsic equivalent is 'Ozyx' corresponding to 'yaw, pitch, roll'. 0035 % (3) 'z1z1z' involves 5 rotations, in which all the non-fixed rotations are around the z axis. 0036 % 0037 % The Euler angles are not, in general, unique. In particular: 0038 % (1) v_roteu2ro('zxz',[a b c]) = v_roteu2ro('zxz',[a+pi -b c+pi]) 0039 % (2) v_roteu2ro('xyz',[a b c]) = v_roteu2ro('xyz',[a+pi pi-b c+pi]) 0040 0041 % Copyright (C) Mike Brookes 2007-2020 0042 % Version: $Id: v_rotro2eu.m 11260 2020-07-18 20:07:58Z dmb $ 0043 % 0044 % VOICEBOX is a MATLAB toolbox for speech processing. 0045 % Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html 0046 % 0047 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0048 % This program is free software; you can redistribute it and/or modify 0049 % it under the terms of the GNU General Public License as published by 0050 % the Free Software Foundation; either version 2 of the License, or 0051 % (at your option) any later version. 0052 % 0053 % This program is distributed in the hope that it will be useful, 0054 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0055 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0056 % GNU General Public License for more details. 0057 % 0058 % You can obtain a copy of the GNU General Public License from 0059 % http://www.gnu.org/copyleft/gpl.html or by writing to 0060 % Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. 0061 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0062 persistent mch mvch rtr rtci rtsi scai w6 th6 x6 pmw 0063 if isempty(mvch) 0064 mch=''; % cached rotation code string 0065 mvch=[0;0;1;1;0;0;1]; 0066 rtr=[1 4 7 2 5 8 3 6 9]; % indices to transpose a vectorized 3x3 matrix 0067 rtci=[2 3 5 6 8 9; 3 1 6 4 9 7; 1 2 4 5 7 8]'; 0068 rtsi=[3 2 6 5 9 8; 1 3 4 6 7 9; 2 1 5 4 8 7]'; 0069 scai=[0 0 0 1; 0 0 0 2; 0 0 0 3; 1 -1 0 1; 1 -1 0 2; 1 -1 0 3; 0 0 -1 1; 0 0 -1 2; 0 0 -1 3; -1 1 0 1; -1 1 0 2; -1 1 0 3]'; % [sin; -sin; cos; xyz] for fixed rotations 0070 w6=ones(6,1); % 0071 th6=3*w6; 0072 x6=[2 1 2 1 2 1]'; % Index for sin components 0073 pmw=[1; -1]; 0074 end 0075 0076 if ~nargout 0077 v_rotro2qr(r); 0078 else 0079 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0080 % Convert the m string 0081 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0082 if ischar(m) && strcmp(m,mch) % check if the rotation code string is cached 0083 mv=mvch; 0084 else 0085 mv=v_roteucode(m); % else decode the string 0086 mch=m; % and save the result in the cache for next time. 0087 mvch=mv; 0088 end 0089 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0090 % Now calculate euler angles 0091 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0092 nm=size(mv,2)-1; % number of rotation codes 0093 sz=size(r); 0094 r=reshape(r,9,[]); % vectorize the rotation matrices 0095 nr=size(r,2); % number of rotation matrices 0096 if mv(end)<0 0097 r=r(rtr,:); % transpose rotation matrix 0098 end 0099 e=zeros(mv(2,end),nr); % initialize array of euler angles 0100 ef=mv(end-3); 0101 for i=nm:-1:1 % process rotations in reverse order 0102 mvi=mv(:,i); 0103 mi=mvi(1); 0104 if mi<=3 % rotation around x,y or z 0105 if mvi(6)~=0 % skip if this rotation is redundant 0106 [si,ci,ri,ti]=v_atan2sc(mvi(7)*r(mvi(4),:),mvi(7)*r(mvi(5),:)); 0107 e(mv(2,i),:)=ti*mvi(6)/ef; % save the euler angle 0108 si=mvi(6)*pmw*si; % make -si available and correct sign 0109 r(rtci(:,mi),:)=ci(w6,:).*r(rtci(:,mi),:)-si(x6,:).*r(rtsi(:,mi),:); % apply reverse rotation 0110 end 0111 else % fixed rotation 0112 ai=scai(4,mi); % axis of rotation: 1, 2 or 3 0113 r(rtci(:,ai),:)=scai(th6,mi).*r(rtci(:,ai),:)-scai(x6,mi).*r(rtsi(:,ai),:); % apply reverse rotation 0114 end 0115 end 0116 if nr>1 % if there was >1 input matrix 0117 e=reshape(e,[size(e,1) sz(3:end)]); % restore the shape of the Euler angle array 0118 end 0119 end