rotation3dAxisAndAngle

ROTATION3DAXISANDANGLE Determine axis and angle of a 3D rotation matrix.

   [AXIS, ANGLE] = rotation3dAxisAndAngle(MAT)
   Where MAT is a 4-by-4 matrix representing a rotation, computes the
   rotation axis (containing the points that remain invariant under the
   rotation), and the rotation angle around that axis.
   AXIS has the format [DX DY DZ], constrained to unity, and ANGLE is the
   rotation angle in radians.

   Note: this method use eigen vector extraction. It would be more precise
   to use quaternions, see:
   http://www.mathworks.cn/matlabcentral/newsreader/view_thread/160945

   
   Example
     origin = [1 2 3];
     direction = [4 5 6];
     line = [origin direction];
     angle = pi/3;
     rot = createRotation3dLineAngle(line, angle);
     [axis angle2] = rotation3dAxisAndAngle(rot);
     angle2
     angle2 =
           1.0472

   See also
   transforms3d, vectors3d, angles3d, eulerAnglesToRotation3d

0001 function [axis, theta] = rotation3dAxisAndAngle(mat)
0002 %ROTATION3DAXISANDANGLE Determine axis and angle of a 3D rotation matrix.
0003 %
0004 %   [AXIS, ANGLE] = rotation3dAxisAndAngle(MAT)
0005 %   Where MAT is a 4-by-4 matrix representing a rotation, computes the
0006 %   rotation axis (containing the points that remain invariant under the
0007 %   rotation), and the rotation angle around that axis.
0008 %   AXIS has the format [DX DY DZ], constrained to unity, and ANGLE is the
0009 %   rotation angle in radians.
0010 %
0011 %   Note: this method use eigen vector extraction. It would be more precise
0012 %   to use quaternions, see:
0013 %   http://www.mathworks.cn/matlabcentral/newsreader/view_thread/160945
0014 %
0015 %
0016 %   Example
0017 %     origin = [1 2 3];
0018 %     direction = [4 5 6];
0019 %     line = [origin direction];
0020 %     angle = pi/3;
0021 %     rot = createRotation3dLineAngle(line, angle);
0022 %     [axis angle2] = rotation3dAxisAndAngle(rot);
0023 %     angle2
0024 %     angle2 =
0025 %           1.0472
0026 %
0027 %   See also
0028 %   transforms3d, vectors3d, angles3d, eulerAnglesToRotation3d
0029 %
0030 
0031 % ------
0032 % Author: David Legland
0033 % e-mail: david.legland@inra.fr
0034 % Created: 2010-08-11,    using Matlab 7.9.0.529 (R2009b)
0035 % Copyright 2010 INRA - Cepia Software Platform.
0036 
0037 % extract the linear part of the rotation matrix
0038 A = mat(1:3, 1:3);
0039 
0040 % extract eigen values and eigen vectors
0041 [V, D] = eig(A - eye(3));
0042 
0043 % we need the eigen vector corresponding to eigenvalue==1
0044 [dummy, ind] = min(abs(diag(D)-1)); %#ok<ASGLU>
0045 
0046 % extract corresponding eigen vector
0047 vector = V(:, ind)';
0048 
0049 % compute rotation angle
0050 t = [A(3,2)-A(2,3) , A(1,3)-A(3,1) , A(2,1)-A(1,2)];
0051 theta = atan2(dot(t, vector), trace(A)-1);
0052 
0053 % If angle is negative, invert both angle and vector direction
0054 if theta<0
0055     theta  = -theta; 
0056     vector = -vector; 
0057 end
0058 
0059 % try to get a point on the line
0060 % seems to work, but not sure about stability
0061 [V, D] = eig(mat-eye(4)); %#ok<ASGLU>
0062 origin = V(1:3,4)'/V(4, 4);
0063 
0064 % create line corresponding to rotation axis
0065 axis = [origin vector];
0066