%INVROTAXIS Inverse Rotation
% Function computes equivalent axis of rotation and angle change
% given a rotation transformation matrix T
%
% Syntax:  [axis, theta] = invrotaxis(T)
%
% Inputs:
%    T - 4x4 homogeneous rotation matrix
%
% Outputs:
%    axis - Unit 3 vector describing the axis of rotation.
%    theta - The angle of rotation (radians) 
%
% Other m-files required: sgn.m
%
% See also: ROT, ROTX, ROTY, ROTZ

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

function [axis, theta] = invrotaxis(T)

    T  = T(1:3,1:3);
    
	i = [ 1 0 0 ]';
	j = [ 0 1 0 ]';
	k = [ 0 0 1 ]';
	iNew = T*i;
	jNew = T*j;
	kNew = T*k;
    
    iAxis = cross(jNew - j, kNew - k);
    jAxis = cross(kNew - k, iNew - i);
    kAxis = cross(iNew - i, jNew - j);
    
	% [ i j k ]
    axes = [ i j k ]
    axesNew = [ iAxis jAxis kAxis ];
    
	[y,s] = max([norm(iAxis) norm(jAxis) norm(kAxis)]);
	axis = axesNew(:,s);
    axis = axis/norm(axis);

	% find  axis that is most perpendicular to the rotation axis
    [y,s] = min(abs([norm(axis'*i) norm(axis'*j) norm(axis'*k)]));
    axisPerp = axes(:,s);
    
	e = cross(axis, axisPerp);   
	f = cross(axis, T*axisPerp);  
	
	theta = atan2(norm(cross(e,f)), e'*f);
	theta = theta*sgn(cross(e,f)'*axis);  % correct sign for direction of axis
    
	if abs(theta) < 0.5
        % [ i j k ]
        axis = [ j'*kNew k'*iNew i'*jNew ]';
        theta = norm(axis);
        axis = axis/theta;
	end