Commit e9470ec2 authored by Jean-Yves TINEVEZ's avatar Jean-Yves TINEVEZ
Browse files

Finally found a good way to go to meaningful Euler angles from rotation matrix.

and back.
parent aea446a3
function R = euleurZXZ2rot( E )
%% EULERZYZ2ROT Convert euler ZX'Z' angles to rotation matrix.
% See https://www.geometrictools.com/Documentation/EulerAngles.pdf to have
% a final answer to this madness.
alpha = E( 1 );
beta = E( 2 );
gamma = E( 3 );
c1 = cos( alpha );
s1 = sin( alpha );
c2 = cos( beta );
s2 = sin( beta );
c3 = cos( gamma );
s3 = sin( gamma );
r11 = c1 * c3 - c2 * s1 * s3;
r12 = - c1 * s3 - c2 * c3 * s1;
r13 = s1 * s2;
r21 = c3 * s1 + c1 * c2 * s3;
r22 = c1 * c2 * c3 - s1 * s3;
r23 = - c1 * s2;
r31 = s2 * s3;
r32 = c3 * s2;
r33 = c2;
R = [
r11 r12 r13
r21 r22 r23
r31 r32 r33 ];
end
\ No newline at end of file
...@@ -12,7 +12,7 @@ function [ f, E ] = fit_ellipse( o ) ...@@ -12,7 +12,7 @@ function [ f, E ] = fit_ellipse( o )
% Fit a plane to these points. % Fit a plane to these points.
[ ~, ~, v ] = svd( p ); [ ~, ~, v ] = svd( p );
E = to_euler( v ); E = rot2eulerZXZ( v );
% Rotate the points into the principal axes frame. % Rotate the points into the principal axes frame.
p = p * v; p = p * v;
...@@ -24,26 +24,6 @@ function [ f, E ] = fit_ellipse( o ) ...@@ -24,26 +24,6 @@ function [ f, E ] = fit_ellipse( o )
%% Subfunctions %% Subfunctions
function E = to_euler( R )
if R( 1, 3 ) == 1 || R( 1, 3 ) == -1
E3 = 0;
dlta = atan2(R(1,2),R(1,3));
if R(1,3) == -1
E2 = pi/2;
E1 = E3 + dlta;
else
E2 = -pi/2;
E1 = -E3 + dlta;
end
else
E2 = - asin( R( 1, 3 ) );
E1 = atan2( R( 2, 3 ) / cos( E2 ), R( 3, 3 ) / cos( E2 ) );
E3 = atan2( R( 1, 2 ) / cos( E2 ), R( 1, 1 ) / cos( E2 ) );
end
E = [ E1, E2, E3 ];
end
function f = quadratic_to_cartesian( A ) function f = quadratic_to_cartesian( A )
% Equations taken from Wolfram website. % Equations taken from Wolfram website.
......
function[E,E_deg]=rot2eulerZXZ(R)
%ROT2EULERZXZ Convert rotation matrix to ZX'Z' Euler angles.
r13 = R( 1, 3 );
r31 = R( 3, 1 );
r22 = R( 2, 2 );
r23 = R( 2, 3 );
r32 = R( 3, 2 );
r33 = R( 3, 3 );
if r22 < +1
if r22 > -1
thetaX = acos( r33 );
thetaZ0 = atan2( r13, -r23 );
thetaZ1 = atan2( r31, r32 );
else
thetaX = pi;
thetaZ0 = -atan2( -r12, r11 );
thetaZ1 = 0;
end
else
thetaX = 0;
thetaZ0 = atan2( -r12, r11 );
thetaZ1 = 0;
end
E = double( [ thetaZ0, thetaX, thetaZ1 ] );
E_deg = rad2deg( E );
end
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment