ROTEU2QR converts a sequence of Euler angles to a real unit quaternion
Inputs:
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
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.
Outputs:
Q(4,...) output quaternion. Q is normalized to have magnitude 1 with
its first non-zero coefficient positive.
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 (including a replicated axis, z)
(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.
Inverse conversion: If m has length 3 with adjacent characters distinct,
then v_rotqr2eu(m,v_roteu2qr(m,e))=e.
Inverse rotation: qrmult(roteu2qr(m,e),roteu2qr(fliplr(m),-fliplr(e)))=+-[ 1 0 0 0]'0001 function q=v_roteu2qr(m,e) 0002 %ROTEU2QR converts a sequence of Euler angles to a real unit quaternion 0003 % Inputs: 0004 % 0005 % M a string of n characters from the set determining the order of rotation axes 0006 % as listed below. Note that the control characters 'rdoOaA' may occur anywhere in the string: 0007 % 'x','y','z' rotate around the given axis by the corresponding angle 0008 % given in e() 0009 % '1','2','3' 90 degree rotation around x,y or z axis; doesn't use a value from e() 0010 % '4','5','6' 180 degree rotation around x,y or z axis; doesn't use a value from e() 0011 % '7','8','9' 270 degree rotation around x,y or z axis; doesn't use a value from e() 0012 % 'r','d' all angles are given in radians or degrees [default='r'] 0013 % 'R','D' all angles are given in radians or degrees and are negated 0014 % 'o','O','a','A' selects whether to rotate the object or the coordinate axes and 0015 % whether the rotation axes remain fixed in space for consecutive 0016 % rotations (extrinsic) or else move with each rotation (intrinsic). 0017 % 'o' = object-extrinsic [default] 0018 % 'O' = object-intrinsic 0019 % 'a' = axes-extrinsic 0020 % 'A' = axes-intrinsic 0021 % 0022 % 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. 0023 % A positive rotation is clockwise if looking along the +ve axis away from the origin or anti-clockwise if 'R' or 'D' is given. 0024 % The x, y, z axes form a right-handed triple. 0025 % 0026 % Outputs: 0027 % 0028 % Q(4,...) output quaternion. Q is normalized to have magnitude 1 with 0029 % its first non-zero coefficient positive. 0030 % 0031 % The string M specifies the seqeunce of axes about which the rotations are performed. There are 12 0032 % possible 3-character sequences that avoid consecutive repetitions. These are 'Euler angles' if 0033 % there is a repeated axis or 'Tait-Bryan angles' if not. Common choices are: 0034 % (1) 'zxz' the most common Euler angle set (including a replicated axis, z) 0035 % (2) 'xyz' corresponds to 'roll, pitch, yaw' for an aeroplane heading in the x direction with y to 0036 % the right and z down. The intrinsic equivalent is 'Ozyx' corresponding to 'yaw, pitch, roll'. 0037 % (3) 'z1z1z' involves 5 rotations, in which all the non-fixed rotations are around the z axis. 0038 % 0039 % Inverse conversion: If m has length 3 with adjacent characters distinct, 0040 % then v_rotqr2eu(m,v_roteu2qr(m,e))=e. 0041 % 0042 % Inverse rotation: qrmult(roteu2qr(m,e),roteu2qr(fliplr(m),-fliplr(e)))=+-[ 1 0 0 0]' 0043 0044 % 0045 % Copyright (C) Mike Brookes 2007-2020 0046 % Version: $Id: v_roteu2qr.m 11260 2020-07-18 20:07:58Z dmb $ 0047 % 0048 % VOICEBOX is a MATLAB toolbox for speech processing. 0049 % Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html 0050 % 0051 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0052 % This program is free software; you can redistribute it and/or modify 0053 % it under the terms of the GNU General Public License as published by 0054 % the Free Software Foundation; either version 2 of the License, or 0055 % (at your option) any later version. 0056 % 0057 % This program is distributed in the hope that it will be useful, 0058 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0059 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0060 % GNU General Public License for more details. 0061 % 0062 % You can obtain a copy of the GNU General Public License from 0063 % http://www.gnu.org/copyleft/gpl.html or by writing to 0064 % Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. 0065 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0066 persistent y cb sb mch mvch 0067 if isempty(y) 0068 y=repmat([2 4 1 3 1 3 2 4; 3 2 1 4 1 4 3 2; 3 4 2 1 1 2 4 3],4,1); 0069 cb=cos([0 0 0 1 1 1 2 2 2 3 3 3]*pi/4); 0070 sb=sin([0 0 0 1 1 1 2 2 2 3 3 3]*pi/4); 0071 mch=''; % cached rotation code string 0072 mvch=[0;0;1;1;0;0;1]; 0073 end 0074 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0075 % Convert the m string 0076 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0077 if ischar(m) && strcmp(m,mch) % check if the rotation code string is cached 0078 mv=mvch; 0079 else 0080 mv=v_roteucode(m); % else decode the string 0081 mch=m; % and save the result in the cache for next time. 0082 mvch=mv; 0083 end 0084 ne=mv(2,end); % number of elements required in e per output 0085 if ne==0 0086 sz=[1 1]; % change sz to give a single output 0087 q=zeros(4,1); % space for single output quaternion 0088 else 0089 sz=size(e); 0090 if sz(1)==1 && numel(e)==ne % allow legacy call with row-vector input 0091 e=e(:); 0092 sz=size(e); 0093 else 0094 e=reshape(e,sz(1),[]); % put each set of angles in a separate column 0095 end 0096 q=zeros(4,size(e,2)); % space for output quaternions 0097 end 0098 q(1,:)=1; % initialize output quaternions to the value 1 0099 r=q; % space for temporary quaternion 0100 ef=0.5*mv(end-3); % signed euler angle scale factor include 0.5 factor 0101 nm=size(mv,2)-1; % number of rotations to do 0102 for i=1:nm 0103 mvi=mv(:,i); 0104 mi=mvi(1); 0105 x=y(mi,:); % index for x,y,or z axes 0106 if mi<4 0107 b=ef*e(mvi(2),:); % semi-rotation angle in radians 0108 c=cos(b); 0109 s=sin(b); 0110 else 0111 c=cb(mi); 0112 s=sb(mi); 0113 end 0114 r(x(1:2),:)=q(x(3:4),:); 0115 r(x(5:6),:)=-q(x(7:8),:); 0116 q=repmat(c,4,1).*q+repmat(s,4,1).*r; 0117 end 0118 q(1,:)=q(1,:)*mv(end); % invert rotation if necessary 0119 if ~nargout 0120 v_rotqr2ro(q(:,1)); % plot a cube 0121 else 0122 sz(1)=4; % each rotation needs 4 outputs 0123 q=reshape(q.*repmat(sign([8 4 2 1]*sign(q)),4,1),sz); % force leading coefficient to be positive and reshape 0124 end