ROTEU2QR converts a sequence of Euler angles to a real unit quaternion
Inputs:
M(1,n) a string of n characters from the set {'x','y','z'}
or, equivalently, a vector whose elements are 1, 2, or 3
T(n,1) n rotation angles. A positive rotation is clockwise if
looking along the axis away from the origin.
Outputs:
Q(4,1) output quaternion. Q is normalized to have magnitude 1 with
its first non-zero coefficient positive.
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". A positive rotation is clockwise
if looking along the axis away from the origin; thus a rotation of +pi/2
around Z rotates [1 0 0]' to [0 1 0]'.
Inverse conversion: If m has length 3 with adjacent characters distinct,
then rotqr2eu(m,roteu2qr(m,t))=t.
Inverse rotation: qrmult(roteu2qr(m,t),roteu2qr(fliplr(m),-fliplr(t)))=+-[ 1 0 0 0]'0001 function q=roteu2qr(m,t) 0002 %ROTEU2QR converts a sequence of Euler angles to a real unit quaternion 0003 % Inputs: 0004 % 0005 % M(1,n) a string of n characters from the set {'x','y','z'} 0006 % or, equivalently, a vector whose elements are 1, 2, or 3 0007 % T(n,1) n rotation angles. A positive rotation is clockwise if 0008 % looking along the axis away from the origin. 0009 % 0010 % Outputs: 0011 % 0012 % Q(4,1) output quaternion. Q is normalized to have magnitude 1 with 0013 % its first non-zero coefficient positive. 0014 % 0015 % The string M specifies the axes about which the rotations are performed. 0016 % You cannot have the same axis in adjacent positions and so there are 12 0017 % possibilities. Common ones are "ZXZ" and "ZYX". A positive rotation is clockwise 0018 % if looking along the axis away from the origin; thus a rotation of +pi/2 0019 % around Z rotates [1 0 0]' to [0 1 0]'. 0020 % 0021 % Inverse conversion: If m has length 3 with adjacent characters distinct, 0022 % then rotqr2eu(m,roteu2qr(m,t))=t. 0023 % 0024 % Inverse rotation: qrmult(roteu2qr(m,t),roteu2qr(fliplr(m),-fliplr(t)))=+-[ 1 0 0 0]' 0025 0026 % 0027 % Copyright (C) Mike Brookes 2007-2012 0028 % Version: $Id: roteu2qr.m 2171 2012-07-12 07:33:03Z 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 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 % m consists of a sequence of axes e.g. 'zxy' 0050 % and t gives the rotation angles in radians 0051 q=[1 0 0 0]'; 0052 if ischar(m) 0053 m=lower(m)-'w'; 0054 end 0055 if any(abs(m-2)>1), error('Euler axis must be x,y or z'); end 0056 for i=1:length(m) 0057 x=y(m(i),:); 0058 b=0.5*t(i); 0059 c=cos(b); 0060 s=sin(b); 0061 r=zeros(4,1); 0062 r(x(1:2))=q(x(3:4)); 0063 r(x(5:6))=-q(x(7:8)); 0064 q=c*q+s*r; 0065 end 0066 q=q*(2*(q(find(q~=0,1))>0)-1); % force leading coefficient to be positive