Home > voicebox > rotqr2eu.m

# rotqr2eu

## PURPOSE

ROTQR2EQ converts a real unit quaternion into the corresponding euler angles

## SYNOPSIS

function e=rotqr2eu(m,q)

## DESCRIPTION

```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".```

## CROSS-REFERENCE INFORMATION

This function calls:
• atan2sc ATAN2SC sin and cosine of atan(y/x) [S,C,R,T]=(Y,X)
This function is called by:

## SOURCE CODE

```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.
0026 %
0027 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0028 %   This program is free software; you can redistribute it and/or modify
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```

Generated on Mon 06-Aug-2018 14:48:32 by m2html © 2003