Maths - Conversion Matrix to Quaternion

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 ;
}

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

if (m00 > m11)&(m00 > m22)) { 
   S = sqrt( 1.0 + m00 - m11 - m22 ) * 2; // S=4*qx 
   qw = (m12 - m21) / S;
   qx = 0.25 * S;
   qy = (m01 + m10) / S; 
   qz = (m02 + m20) / S; 
} else if (m11 > m22)) { 
   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 { 
   S = sqrt( 1.0 + m22 - m00 - m11 ) * 2; // S=4*qz
   qw = (m01 - m10) / S;
   qx = (m02 + m20) / S; 
   qy = (m12 + m21) / S; 
   qz = 0.25 * S;
} 

C++ Code (kindly sent to me by Angel)

There is also code from minorlogic at the end of this page. This includes code for both normalised and unnormalised quaternions.

inline void CalculateRotation( Quaternion& q ) const {
  float trace = a[0][0] + a[1][1] + a[2][2] + 1.0f;
  if( trace > M_EPSILON ) {
    float s = 0.5f / sqrtf(trace);
    q.w = 0.25f / s;
    q.x = ( a[2][1] - a[1][2] ) * s;
    q.y = ( a[0][2] - a[2][0] ) * s;
    q.z = ( a[1][0] - a[0][1] ) * s;
  } else {
    if ( a[0][0] > a[1][1] && a[0][0] > a[2][2] ) {
      float s = 2.0f * sqrtf( 1.0f + a[0][0] - a[1][1] - a[2][2]);
      q.w = (a[1][2] - a[2][1] ) / s;
      q.x = 0.25f * s;
      q.y = (a[0][1] + a[1][0] ) / s;
      q.z = (a[0][2] + a[2][0] ) / s;
    } else if (a[1][1] > a[2][2]) {
      float s = 2.0f * sqrtf( 1.0f + a[1][1] - a[0][0] - a[2][2]);
      q.w = (a[0][2] - a[2][0] ) / s;
      q.x = (a[0][1] + a[1][0] ) / s;
      q.y = 0.25f * s;
      q.z = (a[1][2] + a[2][1] ) / s;
    } else {
      float s = 2.0f * sqrtf( 1.0f + a[2][2] - a[0][0] - a[1][1] );
      q.w = (a[0][1] - a[1][0] ) / s;
      q.x = (a[0][2] + a[2][0] ) / s;
      q.y = (a[1][2] + a[2][1] ) / s;
      q.z = 0.25f * s;
    }
  }
}

Alternative Method

Christian has suggested an alternative algorithm and he makes a convincing argument that this is more efficient here.
 
quaternion.w = sqrt( max( 0, 1 + m00 + m11 + m22 ) ) / 2; 
quaternion.x = sqrt( max( 0, 1 + m00 - m11 - m22 ) ) / 2; 
quaternion.y = sqrt( max( 0, 1 - m00 + m11 - m22 ) ) / 2; 
quaternion.z = sqrt( max( 0, 1 - m00 - m11 + m22 ) ) / 2; 
Q.x = _copysign( Q.x, m21 - m12 ) 
Q.y = _copysign( Q.y, m02 - m20 ) 
Q.z = _copysign( Q.z, m10 - m01 ) 

• The max( 0, ... ) is just a safeguard against rounding error.
• copysign takes the sign from the second term and sets the sign of the first without altering the magnitude, I don't know of a java equivalent.

Scaling

Since it is possible for both matrices and quaternions to represent scaling in addition to rotation, then it would be possible to include this in the convertion. In most cases this is not necessary, as we only want to represent rotations, but if you do need to include a scale factor then you might want to try this method suggested by Christian.

Of course, this will mean that the matrix is no longer orthogonal and that the quaternion is no longer normalised.

We first calculate absQ2 which is the cube root of the volume spanned by the matrix axes. 
 
absQ2 = det( matrix )^(1/3) 
 
quaternion.w = sqrt( max( 0, absQ2 + m00 + m11 + m22 ) ) / 2;  
 
...etc 

Rounding Errors

When comparing different methods of doing this conversion I think we should consider sensitivity to rounding errors. In other words, the matrix contains redundant information so, althrough a matrix may initially be orthogonal, if we then do some rotation operations rounding errors may de-orthogonalise the matrix, if this happens which method is most likely to cancel out the errors? 
 
I don't know how best to tackle this? say, for instance, rounding errors introduce an error 'delta e' to one element of the matrix, but we don't know which element has the error. Which method is most likely to cancel out this error? Which method is most likely to produce a normalised quaternion?

Derivation of Equations

Quaternion multiplication and orthogonal matrix multiplication can both be used to represent rotation. If a quaternion is represented by qw + i qx + j qy + k qz , then the equivalent matrix, to represent the same rotation, is:

This assumes that the quaternion is normalised (qw² + qx² + qy² + qz² =1) and that the matrix is orthogonal.

This page discusses the equivalence of quaternion multiplication and orthogonal matrix multiplication.

So this gives the following equations:

• m00 = 1 - 2*qy² - 2*qz²
• m01 = 2*qx*qy - 2*qz*qw
• m02 = 2*qx*qz + 2*qy*qw
• m10 = 2*qx*qy + 2*qz*qw
• m11 = 1 - 2*qx² - 2*qz²
• m12 = 2*qy*qz - 2*qx*qw
• m20 = 2*qx*qz - 2*qy*qw
• m21 = 2*qy*qz + 2*qx*qw
• m22 = 1 - 2*qx² - 2*qy²

This contains more information than we need, so we need to choose a method which is less sensitive to matrix not being orthogonal.

If the quaternion is normalised: qx² + qy² + qz²+ qw² = 1

so,

qw² = 1 - qx² - qy² - qz²

multiply both sides by 4 gives,

4 * qw² = 4 - 4*qx² - 4*qy² - 4*qz²

4 * qw² = 1 + 1 - 2*qy² - 2*qz² + 1 - 2*qx² - 2*qz² + 1 - 2*qx² - 2*qy²

4 * qw² = 1 + m00 + m11 + m22

so qw= sqrt (1 + m00 + m11 + m22) /2

the sum of the diagonal elements is known as the trace (Tr), this gives,

qw= sqrt (1 + Tr) /2

this method can only be used if we are taking the square root of a positive number ie,

1 + Tr <= 0

Is this always the case for orthogonal matrices ???

So we have an equation for qw, to get the other values we can use:

m21 - m12 = 2*qy*qz - 2*qx*qw - 2*qy*qz - 2*qx*qw

m21 - m12 = 4 *qx*qw

therefore:

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

now calculate qy

m02 - m20 = 2*qx*qz + 2*qy*qw - 2*qx*qz + 2*qy*qw

m02 - m20 = 4*qy*qw

therefore:

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

now calculate qz,

m10 - m01 = 2*qx*qy + 2*qz*qw - 2*qx*qy + 2*qz*qw

m10 - m01 = 4*qz*qw

therefore:

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

so summarising the results:

• qw= sqrt (1 + m00 + m11 + m22) /2
• qx = (m21 - m12)/( 4 *qw)
• qy = (m02 - m20)/( 4 *qw)
• qz = (m10 - m01)/( 4 *qw)

Some other results:

Issues

This page assumes:

• the matrix is orthoginal
• trace = m00 + m11 + m22 + 1 > 0 (the matrix is special orthogonal which cannot represent a reflection component)

If these conditions are satisfied then the resulting quaternion should be normalised.

Rotation about a point other than origin

Quaternions and 3x3 matrices alone can only represent rotations about the origin. But if we include a 3D vector with the quaternion we can use this to represent the point about which we are rotating. Also if we use a 4x4 matrix then this can hold a translation (as explained here) and therefore can specify a rotation about a point.

The following code generates a 3D vector (representing the centre of rotation) from the 4x4 matrix. The theory is given here.

Example

we take the 90 degree rotation from this:

to this:

As shown here the matrix for this rotation is:

So using the above result:

qw= sqrt (1 + m00 + m11 + m22) /2 = sqrt (2)/2 = 1.4142/2 = 0.7071
qx = (m21 - m12)/( 4 *qw) = 2/(4 * 0.7071) = 1/ 1.4142 = 0.7071
qy = (m02 - m20)/( 4 *qw) = 0
qz = (m10 - m01)/( 4 *qw) = 0

This gives the quaternion 0.7071 + i 0.7071

As you can see here, this gives the result that we are looking for.

Angle Calculator and Further examples

I have put a java applet here which allows the values to be entered and the converted values shown along with a graphical representation of the orientation.

Also further examples in 90 degree steps here