%INVEULER   Inverse Euler
% Inverse of Euler transform. Function returns 2 possible arrays of 3
% Euler angles, given a 4x4 transformation matrix T
%
% Syntax:  [euler1, euler2] = inveuler(T)
%
% Inputs:
%    T - transformation matrix
%
% Outputs:
%    euler1 - the 1st solution
%    euler1 - the 2nd solution

% Author: Travis Hydzik
% Last revision: 19 October 2004

function [euler1, euler2] = inveuler(T)

phi = [ atan2(T(2,3), T(1,3))
        atan2(T(2,3), T(1,3)) + pi ];
theta = atan2( T(2,3)*sin(phi) + T(1,3)*cos(phi), T(3,3));
psi   = atan2(-T(1,1)*sin(phi) + T(2,1)*cos(phi), -T(1,2)*sin(phi) + T(2,2)*cos(phi));
 
euler1 = [phi(1) theta(1) psi(1)];
euler2 = [phi(2) theta(2) psi(2)];