calc_opq_from_R_Kraus

% calculate Euler angles from Rotation matrix 
 following PCV Sect. 8.1.3.3 (8.23) ff

 Usage
   [omega,phi,kappa] = calc_R_from_opq_kraus(R)
 
   R - double 3x3, 3D rotation matrix
 
   omega,phi,kappa - Euler angles, radiant

 R = R_3(kappa)*R_2(phi)*R_1(omega)
   = [...
     cos(phi)*cos(kappa), ...
     -cos(phi)*sin(kappa), ...
     sin(phi); ... 
     sin(omega)*sin(phi)*cos(kappa)+cos(omega)*sin(kappa), ...
     -sin(omega)*sin(phi)*sin(kappa)+cos(omega)*cos(kappa), ...
     -sin(omega)*cos(phi); ...
     -cos(omega)*sin(phi)*cos(kappa)+sin(omega)*sin(kappa), ...
     cos(omega)*sin(phi)*sin(kappa)+sin(omega)*cos(kappa), ...
     cos(omega)*cos(phi)];

 o      = omega <= pi
 p      = phi
 q      = kappa

 Wolfgang Förstner
 wfoerstn@uni-bonn.de 

 See also calc_R_from_opq_kraus, calc_r_phi_from_R, calc_Rot_ab, calc_Rot_q,
 calc_Rot_r, calc_Rot_rod, calc_Rot_rp, calc_Mq, calc_Mq_comm

0001 %% calculate Euler angles from Rotation matrix
0002 % following PCV Sect. 8.1.3.3 (8.23) ff
0003 %
0004 % Usage
0005 %   [omega,phi,kappa] = calc_R_from_opq_kraus(R)
0006 %
0007 %   R - double 3x3, 3D rotation matrix
0008 %
0009 %   omega,phi,kappa - Euler angles, radiant
0010 %
0011 % R = R_3(kappa)*R_2(phi)*R_1(omega)
0012 %   = [...
0013 %     cos(phi)*cos(kappa), ...
0014 %     -cos(phi)*sin(kappa), ...
0015 %     sin(phi); ...
0016 %     sin(omega)*sin(phi)*cos(kappa)+cos(omega)*sin(kappa), ...
0017 %     -sin(omega)*sin(phi)*sin(kappa)+cos(omega)*cos(kappa), ...
0018 %     -sin(omega)*cos(phi); ...
0019 %     -cos(omega)*sin(phi)*cos(kappa)+sin(omega)*sin(kappa), ...
0020 %     cos(omega)*sin(phi)*sin(kappa)+sin(omega)*cos(kappa), ...
0021 %     cos(omega)*cos(phi)];
0022 %
0023 % o      = omega <= pi
0024 % p      = phi
0025 % q      = kappa
0026 %
0027 % Wolfgang Förstner
0028 % wfoerstn@uni-bonn.de
0029 %
0030 % See also calc_R_from_opq_kraus, calc_r_phi_from_R, calc_Rot_ab, calc_Rot_q,
0031 % calc_Rot_r, calc_Rot_rod, calc_Rot_rp, calc_Mq, calc_Mq_comm
0032 
0033 function [o,p,q] = calc_opq_from_R_Kraus(R)
0034 
0035 q = mod(atan2(-R(1,2),R(1,1))+2*pi,2*pi);
0036 o = mod(atan2(-R(2,3),R(3,3))+2*pi,2*pi);
0037 p = mod(atan2(R(1,3),-R(1,2)/sin(q))+2*pi,2*pi);
0038 
0039 if  o > pi
0040     q = q-pi;
0041     o = o-pi;
0042     p = mod(pi-p+2*pi,2*pi);
0043 end