Thread Subject: angle from rotation matrix

Hi,

Any one knows how to extract the rotational axis and the
angle from a 3-by-3 rotation matrix? Thanks very much.
I've googled, but haven't got the luck for the solution.

"Pinpress " <nospam__@yahoo.com> wrote in message
<fjvbqk$gbt$1@fred.mathworks.com>...
> Hi,
>
> Any one knows how to extract the rotational axis and the
> angle from a 3-by-3 rotation matrix? Thanks very much.
> I've googled, but haven't got the luck for the solution.


http://mathworld.wolfram.com/RotationMatrix.html
http://en.wikipedia.org/wiki/Rotation_matrix

On Sat, 15 Dec 2007 01:49:08 +0000 (UTC), "Pinpress "
<nospam__@yahoo.com> wrote:

>Hi,
>
>Any one knows how to extract the rotational axis and the
>angle from a 3-by-3 rotation matrix? Thanks very much.
>I've googled, but haven't got the luck for the solution.

One method is to convert the rotation matrix (i.e., direction cosine
matrix) into a quaternion first, and then you can easily pick off the
angle and axis directly from the quaternion elements. I have listed
three routines below.

dc2quat.m --> Converts a direction cosine matrix into a quaternion.

quat2dc.m --> Converts a quaternion into a direction cosine matrix.

angleaxis.m --> Calculates rotation angle and axis of input (dc matrix
or quaternion)

The algorithm for converting a direction cosine matrix into a
quaternion is not mine .. I believe it is based on a technical journal
article by A. R. Klumpp.

James Tursa

------------------------------------------------------------------------

function q = dc2quat(dc)
% dc2quat quaternion direction cosine matrix angle axis
%**********************************************************************
%
% dc2quat calculates the quaternion corresponding to a direction
% cosine matrix. I believe this is based on the algorithm
% given by A. R. Klumpp, "Singularity-Free Extraction of a
% Quaternion from a Direction-Cosine Matrix," Journal of
% Spacecraft and Rockets, Vol. 13, Dec. 1976, pp. 754-755.
% Assumes input dc is orthonormal.
%
% Input: dc = 3x3 direction cosine matrix
%
% Output: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Programmer: James Tursa
%
%**********************************************************************

q = [0 0 0 0];
tr = dc(1,1) + dc(2,2) + dc(3,3);
ii = 0;
nn = 0;
q(1) = tr;
for kk=1:3
    if( dc(kk,kk) > q(1) )
        ii = kk;
        q(1) = dc(kk,kk);
    end
end

tr = sqrt(1 + 2*q(1) - tr);

order = [2 3 1];
for mm=1:3
    kk = order(mm);
    nn = nn + 1;
    jj = 6 - nn - kk;
    x = ii * (nn - ii);
    if( x == 0)
        q(1) = (dc(jj,kk) - dc(kk,jj)) / tr;
        q(nn+1) = q(1);
    else
        q(jj+kk-ii+1) = (dc(jj,kk) + dc(kk,jj)) / tr;
    end
end

if( ii == 0 )
    q(1) = tr;
else
    q(ii+1) = tr;
end
q(2:4) = -q(2:4);
if( q(1) == 0 )
    q = 0.5 * q;
else
    q = 0.5 * sign(q(1)) * q;
end

%\
% MAKES QUATERNION A POSITIVE ROTATION
%/

if( q(1) <= 0 )
    q = -q;
end

%\
% NORMALIZE QUATERNION (KEEPS ROUTINE STABLE)
%/

q = q / norm(q);

return

end


------------------------------------------------------------------------

function dc = quat2dc(q)
% quat2dc quaternion direction cosine matrix angle axis
%*******************************************************************
%
% quat2dc calculates the dirction cosine matrix corresponding to a
% quaternion. Assumes input quaternion is normalized.
%
% Input: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Output: dc = 3x3 direction cosine matrix
%
% Programmer: James Tursa
%
%*******************************************************************

q11 = q(1)^2;
q12 = q(1)*q(2);
q13 = q(1)*q(3);
q14 = q(1)*q(4);
q22 = q(2)^2;
q23 = q(2)*q(3);
q24 = q(2)*q(4);
q33 = q(3)^2;
q34 = q(3)*q(4);
q44 = q(4)^2;

dc = zeros(3);
dc(1,1) = q11 + q22 - q33 - q44;
dc(2,1) = 2 * (q23 - q14);
dc(3,1) = 2 * (q24 + q13);
dc(1,2) = 2 * (q23 + q14);
dc(2,2) = q11 - q22 + q33 - q44;
dc(3,2) = 2 * (q34 - q12);
dc(1,3) = 2 * (q24 - q13);
dc(2,3) = 2 * (q34 + q12);
dc(3,3) = q11 - q22 - q33 + q44;

return
end

------------------------------------------------------------------------

function [a v] = angleaxis(x)
% angleaxis quaternion direction cosine matrix angle axis
%*******************************************************************
%
% angleaxis calculates the rotation angle and rotation axis of the
% input quaternion or direction cosine matrix.
%
% Input: x = quaternion, x(1) = scalar, x(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
% Assumes x is normalized.
%
% or
%
% x = direction cosine matrix.
% Assumes x is orthonormalized.
%
% Output: a = rotation angle (radians)
% v = rotation axis (1x3 unit vector)
%
% Programmer: James Tursa
%
%*******************************************************************

if( numel(x) == 9 )
    q = dc2quat(x);
else
    q = x;
end
a = 2 * acos(q(1));
if( nargout == 2 )
    if( a == 0 )
        v = [1 0 0];
    else
        v = q(2:4)/sin(a/2);
    end
end

return
end

------------------------------------------------------------------------

"Pinpress " <nospam__@yahoo.com> wrote in message <fjvbqk$gbt
$1@fred.mathworks.com>...
> Hi,
>
> Any one knows how to extract the rotational axis and the
> angle from a 3-by-3 rotation matrix? Thanks very much.
> I've googled, but haven't got the luck for the solution.
-------
  If matrix A is a 3 x 3 rotation matrix about the origin, then it must be a real
orthogonal (unitary) matrix (that is, its transpose must be equal to its
inverse), and its determinant must equal +1. Since any point on the axis of
rotation obviously remains fixed, a unit vector along this axis must
necessarily be an eigenvector of A with eigenvalue 1, and there clearly can be
no other real eigenvectors or eigenvalues. Hence, we can determine the axis
of rotation by selecting that unique eigenvector, w, which has an eigenvalue
of 1. It can be shown, 1) that the trace of A must equal 1+2*cos(theta) where
theta is the counterclockwise angle of rotation, and 2) that the dot product of
the vector

 t = [A(3,2)-A(2,3),A(1,3)-A(3,1),A(2,1)-A(1,2)]

with w must be 2*sin(theta), which means that sin(theta) and cos(theta) can
be computed. We can then use matlab's 'atan2' function to determine theta
itself from these values. Note that the signs of both w and theta can be
reversed and still correspond to the same rotation matrix, so the convention
has been adopted below that the positive value of theta will always be
selected and the sign of w adjusted accordingly if necessary.

  The following then is the needed matlab computation:

 [V,D] = eig(A);
 [ignore,ix] = min(abs(diag(D)-1));
 w = V(:,ix);
 t = [A(3,2)-A(2,3),A(1,3)-A(3,1),A(2,1)-A(1,2)];
 theta = atan2(t*w,trace(A)-1);
 if theta<0, theta = -theta; w = -w; end

The quantities w and theta will be a unit vector along the rotation axis and
the counterclockwise rotation about it, respectively. Note that this
computation does not work unless A is a valid rotation matrix.

Roger Stafford

"Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid>
wrote in message <fk0cpd$g0v$1@fred.mathworks.com>...

> The following then is the needed matlab computation:
>
> [V,D] = eig(A);
> [ignore,ix] = min(abs(diag(D)-1));
> w = V(:,ix);
> t = [A(3,2)-A(2,3),A(1,3)-A(3,1),A(2,1)-A(1,2)];
> theta = atan2(t*w,trace(A)-1);
> if theta<0, theta = -theta; w = -w; end
>

One can use either eig() or direct calculation from A:

%%%% First method

D=eig(A);
[dummy k]=max(abs(D-1));
theta=angle(D(k))


%%%%% Second method, returned angle in [0,pi]

c=(trace(A)-1);
s=norm([A(3,2)-A(2,3);...
        A(1,3)-A(3,1); ...
        A(2,1)-A(1,2)]);
theta=atan2(s,c) % better than using acos or asin

%%%%%%%%%%

Of course, the sign of theta is arbitrary chosen. It must be
compatible with the +/- direction of the rotation vector as
Roger pointed out.

Bruno

Sorry, I forgot to give detail on calculation of the
rotation vector:

c=(trace(A)-1);
u=[A(3,2)-A(2,3);...
   A(1,3)-A(3,1); ...
   A(2,1)-A(1,2)];
s=norm(u);
theta=atan2(s,c)

if s>0
    u=u/s
else
    warning('A close to identity, arbitrary result');
    u=[1 0 0]
end

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0rjr$rig
$1@fred.mathworks.com>...
> Sorry, I forgot to give detail on calculation of the
> rotation vector:
>
> c=(trace(A)-1);
> u=[A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)];
> s=norm(u);
> theta=atan2(s,c)
>
> if s>0
> u=u/s
> else
> warning('A close to identity, arbitrary result');
> u=[1 0 0]
> end
-------
  Your second method is better than mine, Bruno. I hadn't noticed that the
call to 'eig' could be eliminated altogether by using the fact that the vector,

 [A(3,2)-A(2,3);A(1,3)-A(3,1);A(2,1)-A(1,2)],

must necessarily lie along the axis of rotation.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0rjr$rig
$1@fred.mathworks.com>...
> Sorry, I forgot to give detail on calculation of the
> rotation vector:
>
> c=(trace(A)-1);
> u=[A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)];
> s=norm(u);
> theta=atan2(s,c)
>
> if s>0
> u=u/s
> else
> warning('A close to identity, arbitrary result');
> u=[1 0 0]
> end
-------
  Your second method is better than mine, Bruno. I hadn't noticed that the
call to 'eig' could be eliminated altogether by using the fact that the vector,

 [A(3,2)-A(2,3);A(1,3)-A(3,1);A(2,1)-A(1,2)],

must necessarily lie along the axis of rotation.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
---------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to accuracy of the rotation axis when the rotation angle theta is
near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
--------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to the accuracy of the rotation axis when the rotation angle theta
is near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
--------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to the accuracy of the rotation axis when the rotation angle theta
is near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
--------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to the accuracy of the rotation axis when the rotation angle theta
is near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
--------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to the accuracy of the rotation axis when the rotation angle theta
is near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
---------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to accuracy of the rotation axis when the rotation angle theta is
near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
$1@fred.mathworks.com>...
> One can use either eig() or direct calculation from A:
>
> %%%% First method
>
> D=eig(A);
> [dummy k]=max(abs(D-1));
> theta=angle(D(k))
>
> %%%%% Second method, returned angle in [0,pi]
>
> c=(trace(A)-1);
> s=norm([A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)]);
> theta=atan2(s,c) % better than using acos or asin
>
> Bruno
---------
Hello again, Bruno.

  It has occurred to me that the second method of yours would not be robust
with respect to accuracy of the rotation axis when the rotation angle theta is
near pi. In that case all the elements of your quantity u, computed from
matrix A, would necessarily be very small differences of typically large
quantities and subsequent division by norm(u) would yield excessive roundoff
errors in the normalized u. If norm(u) were zero in this case, the answer
[1,0,0] would be completely wrong, in general.

  I have not been able to think of an alternative method of doing the problem
in a robust manner, except by performing eigenvector analysis on it, as in
your first method, and in mine. In your first method it looks as though you
would still need to compute u from the A matrix and take its dot product with
the eigenvector whose eigenvalue is 1 in order to determine whether or not
that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
where theta has been determined from one of the complex eigenvalues. If
not, the eigenvector should be reversed in direction, or the sign of theta
reversed, so that the rotation axis unit vector would match the rotation angle.

Roger Stafford

On Mon, 17 Dec 2007 07:47:11 +0000 (UTC), "Roger Stafford"
<ellieandrogerxyzzy@mindspring.com.invalid> wrote:

>"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
>$1@fred.mathworks.com>...
>> One can use either eig() or direct calculation from A:
>>
>> %%%% First method
>>
>> D=eig(A);
>> [dummy k]=max(abs(D-1));
>> theta=angle(D(k))
>>
>> %%%%% Second method, returned angle in [0,pi]
>>
>> c=(trace(A)-1);
>> s=norm([A(3,2)-A(2,3);...
>> A(1,3)-A(3,1); ...
>> A(2,1)-A(1,2)]);
>> theta=atan2(s,c) % better than using acos or asin
>>
>> Bruno
>---------
>Hello again, Bruno.
>
> It has occurred to me that the second method of yours would not be robust
>with respect to accuracy of the rotation axis when the rotation angle theta is
>near pi. In that case all the elements of your quantity u, computed from
>matrix A, would necessarily be very small differences of typically large
>quantities and subsequent division by norm(u) would yield excessive roundoff
>errors in the normalized u. If norm(u) were zero in this case, the answer
>[1,0,0] would be completely wrong, in general.
>
> I have not been able to think of an alternative method of doing the problem
>in a robust manner, except by performing eigenvector analysis on it, as in
>your first method, and in mine. In your first method it looks as though you
>would still need to compute u from the A matrix and take its dot product with
>the eigenvector whose eigenvalue is 1 in order to determine whether or not
>that dot product is equal to, and therefore has the same sign as, 2*sin(theta),
>where theta has been determined from one of the complex eigenvalues. If
>not, the eigenvector should be reversed in direction, or the sign of theta
>reversed, so that the rotation axis unit vector would match the rotation angle.
>
>Roger Stafford

For the particular case of theta near pi (and for cases near 0 for
that matter), you essentially get a skew symmetric matrix for the
direction cosine matrix, so the small quantities that are differenced
in the formation of u are of opposite sign. So you don't get a
cancellation error in the subtraction ... the values end up adding in
magnitude. It appears to be robust in a couple of tests I have run.
Here is the code I used:

ang = pi - 1e-14
rx = [1 0 0;0 cos(ang) sin(ang);0 -sin(ang) cos(ang)];
%rx = eye(3);
ry = [cos(ang) 0 -sin(ang);0 1 0;sin(ang) 0 cos(ang)];
%ry = eye(3);
rz = [cos(ang) sin(ang) 0;-sin(ang) cos(ang) 0; 0 0 1];
%rz = eye(3);
A = rx*ry*rz

c=(trace(A)-1);
uq=[A(3,2)-A(2,3);...
   A(1,3)-A(3,1); ...
   A(2,1)-A(1,2)]';
s=norm(uq);
theta=atan2(s,c)

if s>0
    u=uq/s
else
    warning('A close to identity, arbitrary result');
    u=[1 0 0]
end

I ran with each x, y, and z rotation individually,and also all three
together. For example, all three together gives the output:

ang =

   3.14159265358978

A =

   1.00000000000000 -0.00000000000001 -0.00000000000001
   0.00000000000001 1.00000000000000 -0.00000000000001
   0.00000000000001 0.00000000000001 1.00000000000000

theta =

    1.790337176247374e-014

u =

   0.57735026918962 -0.57735026918963 0.57735026918962

The individual cases seems to work just fine, and the all-three case
shown above seems to work just fine also. If you consider the original
angle used and the difference from pi we actually got and then
multiply this by sqrt(3) you would expect to get something close to
theta, and we do:

>> sqrt(3)*(pi - ang)

ans =

    1.769125671472879e-014

Not sure how close to expect these to match, but at least they are
close.

Also, you would expect to get a rotation vector equally spaced from
the x, y, and z axes, and we do.

I also ran with a small ang near zero and got similar results. Then I
compared with the quaternion code I posted earlier (using atan2
instead of acos ... thanks Bruno) and got exactly the same results
(except that the quaternion code is opposite sense in rotation angle).
I believe the quaternion code I posted is based on a robust peer
reviewed method (although the code I posted was just a quick
conversion from some Fortran code I happened to have and not written
well in MATLAB syntax), so I am quite encouraged that the two give the
same answer.

James Tursa

James Tursa <aclassyguywithaknotac@hotmail.com> wrote in message
<2shcm3pkforks50k3jjkjo0bdkioir9jq2@4ax.com>...
> For the particular case of theta near pi (and for cases near 0 ........
-------
Hello James,

Your example with three successive rotations, each near pi, ends up with a
net rotation through a very small angle, theta = 1.7903e-014, and your A is
close to the identity matrix. In fact, if you do the three successive rotations
with 'ang' exactly equal to pi, you get precisely the identity transformation, as
one can see on paper and pencil. This is not the case I was worried about! It
is when the value of theta for the final transformation turns out to be near pi
that there will be accuracy problems.

  I have run an example to demonstrate this. The general expression for a
rotation matrix A about the rotation axis with unit vector v = [p;q;r] and
rotation angle w is:

 A = [...
 p^2+cos(w)*(q^2+r^2),(1-cos(w))*p*q-sin(w)*r,(1-cos(w))*r*p+sin(w)*q;
 (1-cos(w))*p*q+sin(w)*r,q^2+cos(w)*(r^2+p^2),(1-cos(w))*q*r-sin(w)*p;
 (1-cos(w))*r*p-sin(w)*q,(1-cos(w))*q*r+sin(w)*p,r^2+cos(w)*(p^2+q^2)];

If we choose the following values for w and v:

 w = pi-(1e-14);
 v = [1.1;1.2;1.3]; v = v/norm(v);
 p = v(1); q = v(2); r = v(3);

this gives:

 A =
  -0.44239631336406 0.60829493087557 0.65898617511521
   0.60829493087558 -0.33640552995392 0.71889400921658
   0.65898617511520 0.71889400921660 -0.22119815668203

As you can see, when diagonally opposite elements of A are subtracted, we
have the much-feared subtraction-of-large-quantities-with-small-
differences situation that can produce very large relative round-off errors.

  Specifically, Bruno's second method yields:

 u =
   0.52590972519131
   0.57957398286389
   0.62250538900196

which is markedly different from the original value, v, along the true axis of
rotation:

 v =
   0.52801689681105
   0.57601843288478
   0.62401996895852

The components of u are off from those in v in the third decimal place, which
can hardly be considered robust computation.

Roger Stafford

Roger,

Thank you or pointing out the accuracy problem when
theta~pi. Here is the change I made that work better. Though
I don't think the issue when theta~0 could be overcome.

Bruno


c=(trace(A)-1);
u=[A(3,2)-A(2,3);...
   A(1,3)-A(3,1); ...
   A(2,1)-A(1,2)];
s=norm(u);
theta=atan2(s,c)

if abs(2-c)>s % <- Use diagonal terms to estimate u
    signu=sign(u);
    signu(signu==0)=1; % we don't want the nil case
    u=sqrt((2*diag(A)-c)/(2-c)).*signu
elseif s~=0 % <- use off-diagonal terms to estimate u
    u=u/s
else
    warning('A close to identity');
    u=[1 0 0]
end

"Roger Stafford"
<ellieandrogerxyzzy@mindspring.com.invalid> wrote in
message <fk6eis$ra6$1@fred.mathworks.com>...
> :
> This is not the case I was worried about! It
> is when the value of theta for the final transformation
turns out to be near pi
> that there will be accuracy problems.
>
> I have run an example to demonstrate this.
> :
> Specifically, Bruno's second method yields:
>
> u =
> 0.52590972519131
> 0.57957398286389
> 0.62250538900196
>
> which is markedly different from the original value, v,
along the true axis of
> rotation:
>
> v =
> 0.52801689681105
> 0.57601843288478
> 0.62401996895852
>
> The components of u are off from those in v in the third
decimal place, which
> can hardly be considered robust computation.
>
> Roger Stafford

Got it! Thanks. I should have realized this from your
earlier post.

I would still point out that the quaternion code (with the
atan2 modification) that I posted earlier is robust for
small angles and produces the same result as the eig()
function on your example A matrix. Maybe I will spend the
effort to code this up with better MATLAB syntax.

James Tursa

"Bruno Luong" <b.luong@fogale.fr> wrote in message
<fk6ief$3kq$1@fred.mathworks.com>...
> Roger,
>
> Thank you or pointing out the accuracy problem when
> theta~pi. Here is the change I made that work better.
Though
> I don't think the issue when theta~0 could be overcome.
>
> Bruno
>
>
> c=(trace(A)-1);
> u=[A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)];
> s=norm(u);
> theta=atan2(s,c)
>
> if abs(2-c)>s % <- Use diagonal terms to estimate u
> signu=sign(u);
> signu(signu==0)=1; % we don't want the nil case
> u=sqrt((2*diag(A)-c)/(2-c)).*signu
> elseif s~=0 % <- use off-diagonal terms to estimate u
> u=u/s
> else
> warning('A close to identity');
> u=[1 0 0]
> end

Hello Bruno,

I think that if you continue to improve your method, i.e.
looking for the best combination of A elements to use to
form u depending on circimstances, etc., this effort will
eventually lead you to an algorithm that essentially
accomplishes ths same thing as the direction cosine matrix
to quaternion code that I posted earlier. This code looks
for the best numerical way to combine the A elements to
form the quaternion, and of course the rotation axis then
falls out naturally from the quaternion elements. Again I
would point out that this is not my original algorithm.

James Tursa

"James Tursa" <aclassyguywithaknotac@hotmail.com> wrote in
message <fk6q3s$dns$1@fred.mathworks.com>...
This code looks
> for the best numerical way to combine the A elements to
> form the quaternion.

James, I have not take a look at your code (need too revise
my quaternion lesson first, that was long ago), but could
you please elaborate (or give a hint) why it is the "best" ?

Thanks.

Bruno

PS: in the mean time, I'm trying to improve mine, still. ;-)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


c=(trace(A)-1);
u=[A(3,2)-A(2,3);...
   A(1,3)-A(3,1); ...
   A(2,1)-A(1,2)];
s=norm(u);
theta=atan2(s,c)

if c<0 % <- Use diagonal terms to estimate u
    signu=sign(u);
    signu(signu==0)=1; % we don't want the nil case
    u2=(2*diag(A)-c)/(2-c);
    u=sqrt(u2.*(u2>0)).*signu
elseif s~=0 % <- use off-diagonal terms to estimate u
    u=u/s
else
    warning('Rotation matrix close to identity');
    u=[1 0 0]
end

"Bruno Luong" <b.luong@fogale.fr> wrote in message <fk6ief$3kq
$1@fred.mathworks.com>...
> Roger,
>
> Thank you or pointing out the accuracy problem when
> theta~pi. Here is the change I made that work better. Though
> I don't think the issue when theta~0 could be overcome.
>
> Bruno
>
> c=(trace(A)-1);
> u=[A(3,2)-A(2,3);...
> A(1,3)-A(3,1); ...
> A(2,1)-A(1,2)];
> s=norm(u);
> theta=atan2(s,c)
>
> if abs(2-c)>s % <- Use diagonal terms to estimate u
> signu=sign(u);
> signu(signu==0)=1; % we don't want the nil case
> u=sqrt((2*diag(A)-c)/(2-c)).*signu
> elseif s~=0 % <- use off-diagonal terms to estimate u
> u=u/s
> else
> warning('A close to identity');
> u=[1 0 0]
> end
----------
  That's certainly an improvement over the former method, Bruno. However,
there are still some circumstances where it makes errors in the rotation axis
direction much larger than with the eigenvector method. This happens when
the rotation angle is greater than pi/2 and when the rotation axis lies almost
in the plane of a pair of coordinate axes. The rotation angle, on the other
hand, is determined just as accurately as that found with complex eigenvalue
angles.

  For example, set

 p = .00000001234567;
 q = .65432101234567;
 r = sqrt(1-p^2-q^2); % = 0.75621690856720

for the three direction cosines of a unit vector along the rotation axis and

 w = .7*pi;

for the rotation angle. The result with your new method is:

 u =
   0.00000000836198
   0.65432101234567
   0.75621690856720

where the first component is off in the ninth decimal place. Doing it with
matlab's 'eig' function gives a fully accurate value.

  This latter fact shows that the information is actually present in matrix A but
that the new method is still not managing to fully extract it. I suspect that a
method which is as robust as the eigenvector method would have to make
use of the sums of the matrix elements, A(3,2)+A(2,3), A(1,3)+A(3,1), and A
(2,1)+A(1,2), as well as their differences.

  If we go to the extreme case of a rotation angle exactly equal to pi, then A
would be a symmetric matrix, and it is clear that we need to use more than
just the three diagonal elements of A to determine the signs of the elements
of u. Of course, it is true in this case that the rotation axis can only be
determined as a line, not the direction along the line, since rotating by pi in
either the clockwise or counterclockwise directions about a line achieve the
same result. Nevertheless, of the eight possible sign combinations of
elements of u, we ought to be able to reduce it to only two possibilities, but
off-diagonal matrix elements would surely need to be taken into account.

Roger Stafford

On Mon, 17 Dec 2007 23:07:18 +0000 (UTC), "Bruno Luong"
<brunoluong@yahoo.com> wrote:

>"James Tursa" <aclassyguywithaknotac@hotmail.com> wrote in
>message <fk6q3s$dns$1@fred.mathworks.com>...
>This code looks
>> for the best numerical way to combine the A elements to
>> form the quaternion.
>
>James, I have not take a look at your code (need too revise
>my quaternion lesson first, that was long ago), but could
>you please elaborate (or give a hint) why it is the "best" ?
>
>Thanks.
>
>Bruno
>
>PS: in the mean time, I'm trying to improve mine, still. ;-)

Bruno,

After a little bit of research, I found the original Jounal Article
here:

http://pdf.aiaa.org/jaPreview/JSR/1978/PVJAPRE57311.pdf

The algorithm is due to Richard A. Spurrier, not Klumpp as I
originally thought (Klumpp is a reference). Perhaps "best" was not
necessarily appropriate to describe the algorithm. But the algorithm
*is* robust for small angles and angles near pi. The MATLAB code I
posted earlier was a quick translation of some Fortran code I happened
to have, which in turn is based on the algorithm in the Spurrier
article. This direct translation of this Fortran code is not the best
looking MATLAB code, obviously, so I would not recommend you use it
for reference. The algorithm in the journal article itself would be
much better for this purpose. I should probably code this up in MATLAB
and post it to the Mathworks File Exchange. To understand the article
in the same terms as your algorithm, use the following:

q0 = cos(theta/2)
[q1 q2 q3] = sin(theta/2) * u

(caution: rotation sense might be opposite of your algorithm)

And also the quaternion --> direction cosine matrix algorithm is
useful to refer to as well:

q11 = q(1)^2;
q12 = q(1)*q(2);
q13 = q(1)*q(3);
q14 = q(1)*q(4);
q22 = q(2)^2;
q23 = q(2)*q(3);
q24 = q(2)*q(4);
q33 = q(3)^2;
q34 = q(3)*q(4);
q44 = q(4)^2;

dc = zeros(3);
dc(1,1) = q11 + q22 - q33 - q44;
dc(2,1) = 2 * (q23 - q14);
dc(3,1) = 2 * (q24 + q13);
dc(1,2) = 2 * (q23 + q14);
dc(2,2) = q11 - q22 + q33 - q44;
dc(3,2) = 2 * (q34 - q12);
dc(1,3) = 2 * (q24 - q13);
dc(2,3) = 2 * (q34 + q12);
dc(3,3) = q11 - q22 - q33 + q44;

This indexing is offset by 1 from the article, i.e.

q(1) = q0
q(2) = q1
q(3) = q2
q(4) = q3

Quaternions used to be pretty much confined to aerospace and
mechanical engineering for practical applications and quantum
mechanics as well, but lately they seem to have also taken hold in
computer graphics and animation because of their numerical advantages
over direction cosine matrices when working with rotations,
particularly when it comes to interpolating (Google "slerp") and
renormalizing.

James Tursa

On Tue, 18 Dec 2007 04:38:39 +0000 (UTC), "Roger Stafford"
<ellieandrogerxyzzy@mindspring.com.invalid> wrote:

> I suspect that a
>method which is as robust as the eigenvector method would have to make
>use of the sums of the matrix elements, A(3,2)+A(2,3), A(1,3)+A(3,1), and A
>(2,1)+A(1,2), as well as their differences.
>

The Spurrier quaternion method does this. See my reply to Bruno.

James Tursa

Hello James and Roger,

I quite appreciate various comments.

Roger I also believe there is a way to extract most of the
information in the matrix, and eig must do the job pretty well.

James, I'll take a closer look of quaternion literature.

Long ago I have done some proton spin NMR calculation, where
rotation matrix is presented. I were using my own sauce
pretty close to the one I posted above (too lazy to dig out
the old code, so here I do the exercise again). I work
decently on my application, even with 9 digits limitation. I
try to avoid using cumbersum tool like eig() because of speed.

Best,

Bruno

Still improved direct method for precision (close to the
optimal?)

Bruno

%%%%%%%%%%%%%%%%%%%%%%%

d=diag(A);
t=sum(d);
c=t-1;
uR=[A(3,2)-A(2,3);...
    A(1,3)-A(3,1); ...
    A(2,1)-A(1,2)];
uR2=uR.^2;
s2=sum(uR2);
a=(3-t)+s2;
if a>0
    theta=atan2(sqrt(s2),c)
    % do not mess with parenthesis
    u2=(sum((A+A'-t*I).^2,2)+((4*d-2*t)+1))+uR2;
    absu=sqrt(u2.*(u2>0))/sqrt(a);
    signu=sign(uR);
    signu(signu==0)=1; % we don't want the nil case
    u=absu.*signu;
    u=u/norm(u)
else
    warning('Rotation matrix close to indentity');
    u=[1 0 0]'
    theta=0
end

"Bruno Luong" <brunoluong@yahoo.com> wrote in message
> u2=(sum((A+A'-t*I).^2,2)+((4*d-2*t)+1))+uR2;

Opps, "I" is *of course* the Identity matrix. Let's try it
again.

Bruno

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d=diag(A);
t=sum(d);
c=t-1;
uR=[A(3,2)-A(2,3);...
    A(1,3)-A(3,1); ...
    A(2,1)-A(1,2)];
uR2=uR.^2;
s2=sum(uR2);
a=(3-t)+s2;
if a>0
    theta=atan2(sqrt(s2),c)
    % Mess parenthesis at your own risk...
    u2=(sum((A+A'-t*eye(3)).^2,2)+((4*d-2*t)+1))+uR2;
    absu=sqrt(u2.*(u2>0))/sqrt(a);
    signu=sign(uR);
    signu(signu==0)=1; % we don't want the nil case
    u=absu.*signu;
    u=u/norm(u)
else
    warning('Rotation matrix close to indentity');
    u=[1 0 0]'
    theta=0
end

OK, I have made a small study to evaluate the methods. A
million random runs are done for three method:
- Direct method (D)
- MATLAB Eigen value method (E)
- James's quaternion (Q).

Next, I compute the median and mean of the L^2 errors
(100000 runs) in the rotation direction for three methods,
and here is the result:

Median results:
- D: 0.1241E-15
- E: 0.2077E-15
- Q: 0.1360E-15

Mean results:
- D: 0.6487088853611E-13
- E: 0.59115297358E-15
- Q: 0.274215765605692E-11

Conclusion:
- (E) is amazingly very stable, even in the difficult
situation where angle~0. It clearly outperforms the other
two methods n this case.
- (D) is less stable in difficult situation, but perform
overall 40% better than (E) for easy situation.
- (Q): Same remark as with (D). However it's (relatively)
poorly performed when angle close to 0.

Bellow is the code if you want to try it:

Bruno

%%%%%%%%%%%%%%%%%%%%%%%%

ntest=100000;

error=zeros(ntest,3);
for n=1:ntest
    theta=2*pi*(rand()-0.5);
    
    u=randn(3,1); u=u/norm(u);

    I=eye(3);
    R=[0 -u(3) u(2);
        u(3) 0 -u(1);
        -u(2) u(1) 0];
    T=u*u';
    A=cos(theta)*I + (1-cos(theta))*T + sin(theta)*R;

    d=diag(A);
    t=sum(d);
    uR=[A(3,2)-A(2,3);...
        A(1,3)-A(3,1); ...
        A(2,1)-A(1,2)];
    uR2=uR.^2;
    s2=sum(uR2);
    a=(3-t)+s2;
    if a>0
        theta=atan2(sqrt(s2),t-1);
        u2=(sum((A+A'-t*eye(3)).^2,2)+((4*d-2*t)+1))+uR2;
        absu=sqrt(u2.*(u2>0))/sqrt(a);
        signu=sign(uR);
        signu(signu==0)=1; % we don't want the nil case
        udir=absu.*signu;
        udir=udir/norm(udir);
    else
        warning('Rotation matrix close to indentity');
        udir=[1 0 0]';
        theta=0;
    end

    [P D]=eig(A);
    [dummy k]=min(diag(D)-1);
    ueig=P(:,k);
    
    [a v] = angleaxis(dc2quat(A));
    uqua=v';
    
    error(n,1)=min(norm(u-udir),norm(u+udir));
    error(n,2)=min(norm(u-ueig),norm(u+ueig));
    error(n,3)=min(norm(u-uqua),norm(u+uqua));
    
% if (error(n,1)>4e-12)
% return;
% end
    
end

median(error,1)
mean(error,1)

semilogy(error);
legend('Direct','Eigen','Quaternion');

end

Hi Bruno,

I am unable to reproduce your results. Every time I run it the
quaternion code results are the lowest errors of the three methods.
Could you double check your posted code to make sure it is the same
code that produced your posted results? Thanks.

James Tursa

Hi James,

Here is the complete code:

Run it on another computer:
median(D,E,Q): 1.241267e-016 2.077037e-016 1.359740e-016
  mean(D,E,Q): 9.230310e-014 6.622440e-016 1.184177e-011

Bruno

function error=test
%%%%%%%%%%%%%%%%%%%%%%%%

ntest=100000;

error=zeros(ntest,3);
for n=1:ntest
    theta=2*pi*(rand()-0.5);
    
    u=randn(3,1); u=u/norm(u);

    I=eye(3);
    R=[ 0 -u(3) u(2);
        u(3) 0 -u(1);
       -u(2) u(1) 0];
    T=u*u';
    A=cos(theta)*I + (1-cos(theta))*T + sin(theta)*R;

    [thetadir udir]=DirectMethod(A);
    [thetaeig ueig]=EigenMethod(A);
    [thetaqua uqua]=QuaternionMethod(A);

    error(n,1)=min(norm(u-udir),norm(u+udir));
    error(n,2)=min(norm(u-ueig),norm(u+ueig));
    error(n,3)=min(norm(u-uqua),norm(u+uqua));
    
end

median(error,1);
mean(error,1);

figure(1);
semilogy(error);
sres=arrayfun(@(k) ['median=' num2str(median(error(:,k)))
'/' ...
                    'mean=' num2str(mean(error(:,k)))], ...
                    (1:3), 'UniformOutput', false);
legend(['(D) ' sres{1}], ...
       ['(E) ' sres{2}], ...
       ['(Q) ' sres{3}]);

end

function [theta u]=DirectMethod(A)

    d=diag(A);
    t=sum(d);
    uR=[A(3,2)-A(2,3);...
        A(1,3)-A(3,1); ...
        A(2,1)-A(1,2)];
    uR2=uR.^2;
    s2=sum(uR2);
    a=(3-t)+s2;
    if a>0
        theta=atan2(sqrt(s2),t-1);
        u2=(sum((A+A'-t*eye(3)).^2,2)+((4*d-2*t)+1))+uR2;
        absu=sqrt(u2.*(u2>0))/sqrt(a);
        signu=sign(uR);
        signu(signu==0)=1; % we don't want the nil case
        u=absu.*signu;
        u=u/norm(u);
    else
        warning('Rotation matrix close to indentity');
        u=[1 0 0]';
        theta=0;
    end
end

function [theta u]=EigenMethod(A)
    [P D]=eig(A);
    d=diag(D);
    [dummy k]=min(d-1);
    u=P(:,k);
    [dummy k]=max(abs(d-1));
    theta=angle(d(k));
    t = [A(3,2)-A(2,3);A(1,3)-A(3,1);A(2,1)-A(1,2)];
    if any(sign(t).*sign(u)==-sign(sin(theta)))
        theta=-theta;
    end
end

function [theta u]=QuaternionMethod(A)
    [theta v] = angleaxis(dc2quat(A));
    u=v';
end

function q = dc2quat(dc)
% dc2quat quaternion direction cosine matrix angle axis
%**********************************************************************
%
% dc2quat calculates the quaternion corresponding to a direction
% cosine matrix. I believe this is based on the algorithm
% given by A. R. Klumpp, "Singularity-Free Extraction of a
% Quaternion from a Direction-Cosine Matrix," Journal of
% Spacecraft and Rockets, Vol. 13, Dec. 1976, pp. 754-755.
% Assumes input dc is orthonormal.
%
% Input: dc = 3x3 direction cosine matrix
%
% Output: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Programmer: James Tursa
%
%**********************************************************************

q = [0 0 0 0];
tr = dc(1,1) + dc(2,2) + dc(3,3);
ii = 0;
nn = 0;
q(1) = tr;
for kk=1:3
    if( dc(kk,kk) > q(1) )
        ii = kk;
        q(1) = dc(kk,kk);
    end
end

tr = sqrt(1 + 2*q(1) - tr);

order = [2 3 1];
for mm=1:3
    kk = order(mm);
    nn = nn + 1;
    jj = 6 - nn - kk;
    x = ii * (nn - ii);
    if( x == 0)
        q(1) = (dc(jj,kk) - dc(kk,jj)) / tr;
        q(nn+1) = q(1);
    else
        q(jj+kk-ii+1) = (dc(jj,kk) + dc(kk,jj)) / tr;
    end
end

if( ii == 0 )
    q(1) = tr;
else
    q(ii+1) = tr;
end
q(2:4) = -q(2:4);
if( q(1) == 0 )
    q = 0.5 * q;
else
    q = 0.5 * sign(q(1)) * q;
end

%\
% MAKES QUATERNION A POSITIVE ROTATION
%/

if( q(1) <= 0 )
    q = -q;
end

%\
% NORMALIZE QUATERNION (KEEPS ROUTINE STABLE)
%/

q = q / norm(q);

return

end

function dc = quat2dc(q)
% quat2dc quaternion direction cosine matrix angle axis
%*******************************************************************
%
% quat2dc calculates the dirction cosine matrix
corresponding to a
% quaternion. Assumes input quaternion is normalized.
%
% Input: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Output: dc = 3x3 direction cosine matrix
%
% Programmer: James Tursa
%
%*******************************************************************

q11 = q(1)^2;
q12 = q(1)*q(2);
q13 = q(1)*q(3);
q14 = q(1)*q(4);
q22 = q(2)^2;
q23 = q(2)*q(3);
q24 = q(2)*q(4);
q33 = q(3)^2;
q34 = q(3)*q(4);
q44 = q(4)^2;

dc = zeros(3);
dc(1,1) = q11 + q22 - q33 - q44;
dc(2,1) = 2 * (q23 - q14);
dc(3,1) = 2 * (q24 + q13);
dc(1,2) = 2 * (q23 + q14);
dc(2,2) = q11 - q22 + q33 - q44;
dc(3,2) = 2 * (q34 - q12);
dc(1,3) = 2 * (q24 - q13);
dc(2,3) = 2 * (q34 + q12);
dc(3,3) = q11 - q22 - q33 + q44;

return
end

function [a v] = angleaxis(x)
% angleaxis quaternion direction cosine matrix angle axis
%*******************************************************************
%
% angleaxis calculates the rotation angle and rotation axis
of the
% input quaternion or direction cosine matrix.
%
% Input: x = quaternion, x(1) = scalar, x(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
% Assumes x is normalized.
%
% or
%
% x = direction cosine matrix.
% Assumes x is orthonormalized.
%
% Output: a = rotation angle (radians)
% v = rotation axis (1x3 unit vector)
%
% Programmer: James Tursa
%
%*******************************************************************

if( numel(x) == 9 )
    q = dc2quat(x);
else
    q = x;
end
a = 2 * acos(q(1));
if( nargout == 2 )
    if( a == 0 )
        v = [1 0 0];
    else
        v = q(2:4)/sin(a/2);
    end
end

return
end

Hi Bruno,

Totally my fault. Although I mentioned in one of my earlier posts that
I changed angleaxis.m to use atan2 instead of acos, I never posted the
changed code. Here it is. That is the reason for the poor performance
at small angles ... it had nothing to do with the quaternion
conversion code itself.

James Tursa

function [a v] = angleaxis(x)
%*******************************************************************
%
% anglevec calculates the rotation angle and rotation axis of the
% input quaternion or direction cosine matrix.
%
% Input: x = quaternion, x(1) = scalar, x(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
% Assumes x is normalized.
%
% or
%
% x = direction cosine matrix.
% Assumes x is orthonormalized.
%
% Output: a = rotation angle (radians)
% v = rotation axis (1x3 unit vector)
%
% Programmer: James Tursa
%
%*******************************************************************

if( numel(x) == 9 )
    q = dc2quat(x);
else
    q = x;
end
n = norm(q(2:4));
a = 2*atan2(n,q(1));
if( n == 0 )
    v = [1 0 0];
else
    v = q(2:4)/n;
end

return
end

James Tursa <aclassyguywithaknotac@hotmail.com> wrote in
message <n48lm3p7479cc4r5rp943rd6khgji5j5gb@4ax.com>...
>
> Hi Bruno,
>
> Totally my fault. Although I mentioned in one of my
earlier posts that
> I changed angleaxis.m to use atan2 instead of acos, I
never posted the
> changed code. Here it is. That is the reason for the poor
performance
> at small angles ... it had nothing to do with the quaternion
> conversion code itself.
>
> James Tursa
>

Thank you James,

Now the quaternion shows it superiority:

Median (D,E,Q): 1.241267e-016 2.077037e-016 1.110223e-016
Mean (D,E,Q): 9.652586e-014 7.196230e-016 9.524508e-017

What is surprising is the Mean is even better than the median.

Bruno

"Bruno Luong" <b.luong@fogale.fr> wrote in news:fke8ss$p7n$1
@fred.mathworks.com:

>
> Now the quaternion shows it superiority:

"Quaternions came from Hamilton after his really good work had been done;
and though beautifully ingenious, have been an unmixed
evil to those who have touched them in any way.'' --- Lord Kelvin

--
Scott
Reverse name to reply

The paper 'Rotations in space and orthogonal matrices' by Kraines, 1991 issue of The College Mathematics Journal, is one I found simple and helpful. He shows the axis of rotation is the span of the eigenvector with eigenvalue 1 and that tr(Q)=1+2cos(theta), where Q is the rotation matrix.

Roger's message (below) mentions "It can be shown, 1) that the trace of A must equal 1+2*cos(theta) where theta is the counterclockwise angle of rotation, ...". I'm hoping that smeone can share how to show this with either linear albegra, Lie Theory or both. Thanks, John.

"Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <fk0cpd$g0v$1@fred.mathworks.com>...
> "Pinpress " <nospam__@yahoo.com> wrote in message <fjvbqk$gbt
> $1@fred.mathworks.com>...
> > Hi,
> >
> > Any one knows how to extract the rotational axis and the
> > angle from a 3-by-3 rotation matrix? Thanks very much.
> > I've googled, but haven't got the luck for the solution.
> -------
> If matrix A is a 3 x 3 rotation matrix about the origin, then it must be a real
> orthogonal (unitary) matrix (that is, its transpose must be equal to its
> inverse), and its determinant must equal +1. Since any point on the axis of
> rotation obviously remains fixed, a unit vector along this axis must
> necessarily be an eigenvector of A with eigenvalue 1, and there clearly can be
> no other real eigenvectors or eigenvalues. Hence, we can determine the axis
> of rotation by selecting that unique eigenvector, w, which has an eigenvalue
> of 1. It can be shown, 1) that the trace of A must equal 1+2*cos(theta) where
> theta is the counterclockwise angle of rotation, and 2) that the dot product of
> the vector
>
> t = [A(3,2)-A(2,3),A(1,3)-A(3,1),A(2,1)-A(1,2)]
>
> with w must be 2*sin(theta), which means that sin(theta) and cos(theta) can
> be computed. We can then use matlab's 'atan2' function to determine theta
> itself from these values. Note that the signs of both w and theta can be
> reversed and still correspond to the same rotation matrix, so the convention
> has been adopted below that the positive value of theta will always be
> selected and the sign of w adjusted accordingly if necessary.
>
> The following then is the needed matlab computation:
>
> [V,D] = eig(A);
> [ignore,ix] = min(abs(diag(D)-1));
> w = V(:,ix);
> t = [A(3,2)-A(2,3),A(1,3)-A(3,1),A(2,1)-A(1,2)];
> theta = atan2(t*w,trace(A)-1);
> if theta<0, theta = -theta; w = -w; end
>
> The quantities w and theta will be a unit vector along the rotation axis and
> the counterclockwise rotation about it, respectively. Note that this
> computation does not work unless A is a valid rotation matrix.
>
> Roger Stafford
>

"John_old Craighead" <7jwc2@queensu.ca> wrote in message <hjaf4r$4ro$1@fred.mathworks.com>...
> Roger's message (below) mentions "It can be shown, 1) that the trace of A must equal 1+2*cos(theta) where theta is the counterclockwise angle of rotation, ...". I'm hoping that smeone can share how to show this with either linear albegra, Lie Theory or both. Thanks, John.

Here is the general idea:

Euler has showed that linear map represented by a (3 x 3) A such that A'*A has always a fixed-point:
http://en.wikipedia.org/wiki/Euler%27s_rotation_theorem#Matrix_proof

Next, in the basis that contains the fixed-axis, the matrix is reduced to

[1 0 0
 0 cos -sin
0 sin cos ]

The trace is then 1 + 2*cos.

Bruno

"Pinpress " <nospam__@yahoo.com> wrote in message <fjvbqk$gbt$1@fred.mathworks.com>...
> Hi,
>
> Any one knows how to extract the rotational axis and the
> angle from a 3-by-3 rotation matrix? Thanks very much.
> I've googled, but haven't got the luck for the solution.

I am working on the attitude and orbit determination and control system on a satellite for a design competition for the Air Force. http://www.vs.afrl.af.mil/UNP/

I am given and x y and z components from an instrumental reading. I have to find the angle between those readings and the body fixed frame because the spacecraft is tumbling. I plan on using a 1-2-3 rotation matrices to find the angles. I might have to use the work you all have done and work it out three times for each axis. So if anyone has any help I will be more than happy.
Thank you.