sustaining_gazes/matlab_version/PDM_helpers/Rot2Euler.m

13 lines
393 B
Mathematica
Raw Normal View History

2016-04-28 21:40:36 +02:00
function [euler] = Rot2Euler(R)
q0 = sqrt( 1 + R(1,1) + R(2,2) + R(3,3) ) / 2;
q1 = (R(3,2) - R(2,3)) / (4*q0) ;
q2 = (R(1,3) - R(3,1)) / (4*q0) ;
q3 = (R(2,1) - R(1,2)) / (4*q0) ;
yaw = asin(2*(q0*q2 + q1*q3));
pitch= atan2(2*(q0*q1-q2*q3), q0*q0-q1*q1-q2*q2+q3*q3);
roll = atan2(2*(q0*q3-q1*q2), q0*q0+q1*q1-q2*q2-q3*q3);
euler = [pitch, yaw, roll];