% rotation axis and rotation angle from rotation matrix R see PCV Sect. 8.1.4.2, p. 331 ff Usage: [r,phi]= calc_r_phi_from_R(R) R - double 3x3, 3D rotation matrix r - double 3x1, normalized rotation axis phi - double, rotation angle, radiant Wolfgang Förstner 10/2011 wfoerstn@uni-bonn.de See also calc_R_from_opq_kraus, calc_opq_from_R_Kraus, calc_Rot_ab, calc_Rot_q, calc_Rot_r, calc_Rot_rod, calc_Rot_rp, calc_Mq, calc_Mq_comm
0001 %% rotation axis and rotation angle from rotation matrix R 0002 % see PCV Sect. 8.1.4.2, p. 331 ff 0003 % 0004 % Usage: 0005 % [r,phi]= calc_r_phi_from_R(R) 0006 % 0007 % R - double 3x3, 3D rotation matrix 0008 % 0009 % r - double 3x1, normalized rotation axis 0010 % phi - double, rotation angle, radiant 0011 % 0012 % Wolfgang Förstner 10/2011 0013 % wfoerstn@uni-bonn.de 0014 % 0015 % See also calc_R_from_opq_kraus, calc_opq_from_R_Kraus, calc_Rot_ab, calc_Rot_q, 0016 % calc_Rot_r, calc_Rot_rod, calc_Rot_rp, calc_Mq, calc_Mq_comm 0017 0018 function [r,p]= calc_r_phi_from_R(R) 0019 0020 tr = trace(R); 0021 0022 % vector a which will lead to the rotation axis below, PCV Eq.(8.33) 0023 a = -[R(2,3)-R(3,2),R(3,1)-R(1,3),R(1,2)-R(2,1)]'; 0024 an = norm(a); 0025 0026 % rotation angle, PCV Eq.(8.34) 0027 phi = atan2(an,tr-1); 0028 0029 % three cases of rotation axis 0030 if abs(phi) < eps 0031 % 0° 0032 r = [1,0,0]'; 0033 p = 0; 0034 elseif abs(phi-pi) < eps 0035 % 180° 0036 D = (R+eye(3)); 0037 [~,i] = max([D(1,1),D(2,2),D(3,3)]); 0038 ru = D(:,i); 0039 r = ru/norm(ru); 0040 p = pi; 0041 else 0042 % general case 0043 r = a/an; 0044 p = phi; 0045 end 0046