% Function computes equivalent axis of rotation and angle change
% given a rotation transformation matrix T
%
% input:  T     - 4x4 homogeneous rotation matrix
%
% output: axis  - Unit 3 vector describing the axis of rotation.
%         theta - The angle of rotation (radians)   

function [axis, theta] = invrotaxis(T)

% calculate axis vector

     i = [1,0,0];
     j = [0,1,0];
     k = [0,0,1];
     i_new = T(1:3,1)';
     j_new = T(1:3,2)';
     k_new = T(1:3,3)';

     c(1,1:3) = cross((i_new - i), (j_new - j));
     c(2,1:3) = cross((j_new - j), (k_new - k));
     c(3,1:3) = cross((k_new - k), (i_new - i));
     c

     [M, I] = max([norm(c(1,:)), norm(c(2,:)), norm(c(3,:))]);

     axis(1:3) = c(I,:);

% calculate rotation angle

     c2(1,1:3) = cross(axis, [1,0,0]);
     c2(2,1:3) = cross(axis, [0,1,0]);
     c2(3,1:3) = cross(axis, [0,0,1]);

     [M, I] = max([norm(c2(1,:)), norm(c2(2,:)), norm(c2(3,:))]);

     base = [0, 0, 0];
     base(I) = 1
     axis;

     e = cross(axis, base);
     f = cross(axis,T(1:3,I)); 

     theta = atan2(norm(cross(e,f)), e*f');
     if cross(e,f)~=0
     theta = theta*sign(cross(e,f)*axis');
     end
