Sunday, February 13, 2011

How to get quaternion from rotation matrix

For now probably the whole generation of programmers has learned the basics of projective geometry from the famous Matrix and Quaternion FAQ. I myself have consulted it numerous times while writing Khayyam (or more precisely Elea support library). But experimenting with the 3D transformations I found that while theoretically correct, the two most esoteric methods of the FAQ give suboptimal results. Namely - converting the rotation matrix to quaternion or Euler angles becomes unstable for certain rotations and - at least for Euler angles - does not give the most intuitive solution. So decided to write slightly enhanced variants.

The original code from FAQ:
T = 1 + mat[0] + mat[5] + mat[10]
if ( T > 0.00000001 ) {
    S = sqrt(T) * 2;
    X = ( mat[9] - mat[6] ) / S;
    Y = ( mat[2] - mat[8] ) / S;
    Z = ( mat[4] - mat[1] ) / S;
    W = 0.25 * S;
} if ( mat[0] > mat[5] && mat[0] > mat[10] ) {
    S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
    X = 0.25 * S;
    Y = (mat[4] + mat[1] ) / S;
    Z = (mat[2] + mat[8] ) / S;
    W = (mat[9] - mat[6] ) / S;
} else if ( mat[5] > mat[10] ) {
    S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
    X = (mat[4] + mat[1] ) / S;
    Y = 0.25 * S;
    Z = (mat[9] + mat[6] ) / S;
    W = (mat[2] - mat[8] ) / S;
} else {
    S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
    X = (mat[2] + mat[8] ) / S;
    Y = (mat[9] + mat[6] ) / S;
    Z = 0.25 * S;
    W = (mat[4] - mat[1] ) / S;
}
The problem lies in how the calculation method is switched, depending on the comparison of certain matrix elements. If, for two sequential rotations, the matrix values happen to be only slightly different, but fall into different calculation paths, the resulting quaternions will be very different and thus interpolating between them results in sudden twists. This should not happen, if your matrices are perfectly orthogonal - but if they come into existence as the result of many multiplications and (especially) inversions chances are good that they are not. Now add into the mix rotation angles close to 180 degrees, and the there will be distortions.

Stable variant from libelea (please notice, that while usually libelea has column-major order, packed 3x4 matrices, as used here, have row-major order. So to port the code to OpenGL matrices you have to transpose matrix):
/* Get normalized transformed unit vectors of rotated space */
Vector3f axx(Vector3f(m[0], m[4], m[8]).normalize ());
Vector3f axy(Vector3f(m[1], m[5], m[9]).normalize ());
Vector3f axz(Vector3f(m[2], m[6], m[10]).normalize ());

/* Calculate transposition vectors */
/* I.e. how the unit vector endpoints will be transformed */
Vector3f tx = axx - Vector3fX;
Vector3f ty = axy - Vector3fY;
Vector3f tz = axz - Vector3fZ;

/* Find two biggest transpositions */
f32 txl2 = tx.length2 ();
f32 tyl2 = ty.length2 ();
f32 tzl2 = tz.length2 ();
Vector3f *a = &tx;
Vector3f *b = &ty;
if ((txl2 < tyl2) && (txl2 < tzl2)) {
    a = &ty;
    b = &tz;
} else if ((tyl2 < txl2) && (tyl2 < tzl2)) {
    a = &tz;
    b = &tx;
}

/* Get axis (cross product of two biggest transpositions)*/
Vector3f ax(*a * *b);
if (!ax.length2 ()) {
    /* Zero rotation */
    return Quaternionf0;
}
ax.normalizeSelf ();

/* Get vector perpendicular to axis and transform it */
Vector3f s(a->normalize ());
Vector3f t(m.transformVector3 (s));
t.normalizeSelf ();

/* Find rotation angle (between vector and its transform) */
Vector3f s_t(s * t);
f32 e = Vector3f::scalarProduct (s_t, ax);
if (e < 0) {
    ax = -ax;
}
f32 theta = Vector3f::angle (s, t);
f32 sint2 = sinf (theta / 2);
f32 cost2 = cosf (theta / 2);
return Quaternionf(ax[X] * sint2, ax[Y] * sint2, ax[Z] * sint2, cost2);

The idea is, that any vector perpendicular to axis will be transformed to vector also perpendicular to axis. So we can get axis as the cross product between such vector and its transform.
Now, as you can see, this is much slower, involving the vector normalization, cross products and trigonometric functions. But hopefully you do not have to do this very often - I mostly only needed it for importing foreign animation formats and compacting rotation matrices for serializing.
I have used this method quite extensively and it has excellent stability.

Next time I'll show, how to avoid sudden instabilities if converting rotation matrix to Euler angles.

No comments:

Post a Comment