## Equations

For a pure rotation, that is where:

- the matrix is orthogonal
- the matrix is special orthogonal which gives additional condition: det(matrix)= +1

Then the matrix can be converted to a quaternion using this basic form:

qw= √(1 + m00 + m11 + m22) /2

qx = (m21 - m12)/( 4 *qw)

qy = (m02 - m20)/( 4 *qw)

qz = (m10 - m01)/( 4 *qw)

But this is only accurate under certain conditions and to get an algorithm that is accurate under all circumstances we need to check the trace as below:

## Java Code

### Simple Code

public final void set(Matrix4f m1) { w = Math.sqrt(1.0 + m1.m00 + m1.m11 + m1.m22) / 2.0; double w4 = (4.0 * w); x = (m1.m21 - m1.m12) / w4 ; y = (m1.m02 - m1.m20) / w4 ; z = (m1.m10 - m1.m01) / w4 ; }

The above should work assuming that the function inside the square root is positive, that is,

1 + Tr < 0

where Tr = the trace which is the sum of the leading diagonal terms.

For instance the following matrix represents a 180° rotation about the y axis:

-1 | 0 | 0 |

0 | 1 | 0 |

0 | 0 | -1 |

In this case the above algorithm wont work because 1 + m00 + m11 + m22 = 0 which gives w=0 and so will cause division by zero.

Another example is a rotation of heading= 180° and attitude = 90°.

0 | 1 | 0 |

1 | 0 | 0 |

0 | 0 | -1 |

Even if the value of qw is very small it may produce big numerical errors when dividing. So it is better to use the following:

1) Calculate the trace(the sum of the diagonal elements) of the matrix T from
the equation:

T = 4 - 4*qx^{2} - 4*qy^{2} - 4*qz^{2}

= 4( 1 -qx^{2} - qy^{2} - qz^{2} )

= m00 + m11 + m22 + 1

If the trace of the matrix is greater than zero, then the result is:

S = 0.5 / sqrt(T) W = 0.25 / S X = ( m21 - m12 ) * S Y = ( m02 - m20 ) * S Z = ( m10 - m01 ) * S

If the trace of the matrix is less than or equal to zero then identify which major diagonal element has the greatest value.

### Code

float tr = m00 + m11 + m22 if (tr > 0) { float S = sqrt(tr+1.0) * 2; // S=4*qw qw = 0.25 * S; qx = (m21 - m12) / S; qy = (m02 - m20) / S; qz = (m10 - m01) / S; } else if ((m00 > m11)&(m00 > m22)) { float S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx qw = (m21 - m12) / S; qx = 0.25 * S; qy = (m01 + m10) / S; qz = (m02 + m20) / S; } else if (m11 > m22) { float S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy qw = (m02 - m20) / S; qx = (m01 + m10) / S; qy = 0.25 * S; qz = (m12 + m21) / S; } else { float S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz qw = (m10 - m01) / S; qx = (m02 + m20) / S; qy = (m12 + m21) / S; qz = 0.25 * S; }

We need to be sure this code is resilient to:

- Avoid division by zero - We need to be sure that S is never zero even with possible floating point errors or de-orthogonalised matrix input.
- Avoid square root of a negative number. - We need to be sure that the tr value chosen is never negative even with possible floating point errors or de-orthogonalised matrix input.

- Accuracy of dividing by (and square rooting) a very small number.

with floating point numbers, dividing small numbers by small numbers should be reasonably accurate but at the extreme it would loose accuracy. - Resilient to a de-orthogonalised matrix

For a discussion of the stability of this code see this forum discussion that I had with Ethan.

## Issues

(1) This page assumes that the input matrix represents a pure rotation otherwise the resulting quaternion will not be valid. This implies that the matrix must be special orthogonal, that is both:

- the matrix is orthogonal
- In addition the matrix is special orthogonal (pure rotation without reflection component)

If these conditions are satisfied then the resulting quaternion should be normalised (unit length).

(2) Both matrices and quaternions avoid the singularities and discontinuities involved with rotation in 3 dimensions by adding extra dimensions. This has the effect that different values could represent the same rotation, for example quaternion q and -q represent the same rotation. It is therefore possible that, if we are converting a rotation sequence, our output may jump between these equivalent forms. This could cause problems where subsequent operations such as differentiation is done on this data. So programmers need to be aware of this issue.

For a more detailed discussuion of these issues see this forum discussion that I had with Tim.