## Friday, February 18, 2011

### Converting rotation matrix to arbitrarily ordered Euler angles

There is enhanced version of this algorithm available in new post.

The Matrix and Quaternion FAQ presents the straightforward method of converting rotation matrix to Euler angles. Still, it has two shortcomings.
• There is discontinuity close to Z rotation of 90 degrees, where X and Y rotations suddenly jump from close to zero to close to 180 degrees.
• It only shows the way to get angles in XYZ order (i.e. M = Mx * My * Mz).
Arbitrarily ordered angles are extremely useful for skeletal animation to give joints more natural deformations. But the main motivation was actually writing libmiletos container for Poser figures (cr2 file format). These have always bone rotations in twist/bend/bend order, while twist is defined as the rotation of the main axis of the bone. And as Poser figures have different bending algorithms for different rotations, the angles cannot be easily reordered.
So I have written slightly enhanced version for libelea.

Stable decomposition of matrix to Euler angles

First the original code.
It is derived from expanded form of rotation matrix:
```|  CE      -CF       D   0 |
M = |  BDE+AF  -BDF+AE  -BC  0 |
|  0        0        0   1 |

A,B are the cosine and sine of the X-axis rotation axis,
C,D are the cosine and sine of the Y-axis rotation axis,
E,F are the cosine and sine of the Z-axis rotation axis.
```
From it the following algorithm can be derived (using row-major matrix layout, so it has to be transposed for column-major OpenGL matrices):
```angleY = asin (m);
C =  cos (angleY);
if (fabs (C) > 0.005) {
trx    =  m / C;
try    = -m  / C;
angleX = atan2 (try, trx);
trx    =  m / C;
try    = -m / C;
angleZ = atan2 (try, trx);
} else {
angleX = 0;
trx    =  m;
try    =  m;
angleZ = atan2 (try, trx);
}
```
The problem is that when angleY goes from slightly below 90 degrees to slightly above 90 degrees, C changes sign and suddenly angles X and Y jump to different quadrant.

The enhanced version from libelea (also written for row-major order matrices):
```aY = asin (m);
aY2 = M_PI_F - aY;
if (fabs (m) != 1) {
C = cos (aY);
C2 = cos (aY2);
trX = m / C;
trY = -m / C;
aX = atan2 (trY, trX);
aX2 = atan2 (-m / C2, m / C2);
trX = m / C;
trY = -m / C;
aZ = atan2 (trY, trX);
aZ2 = atan2 (-m / C2, m / C2);
} else {
aX = aX2 = 0;
trX = m;
trY = m;
aZ = aZ2 = atan2 (trY, trX);
}
if ((aX * aX + aY * aY + aZ * aZ) < (aX2 * aX2 + aY2 * aY2 + aZ2 * aZ2)) {
angleX = aX;
angleY = aY;
angleZ = aZ;
} else {
angleX = aX2;
angleY = aY2;
angleZ = aZ2;
}
```
The only trick here is to calculate both Y angles satisfying the formula
```sin (aY) = c
```
and subsequently two versions of other angles too. Then the actual set of angles is chosen such, that the squared sum of all angles is lesser. This behaves extremely well as far as I have tested it.

Decomposition of matrix to arbitrarily ordered Euler angles

The code from libelea (matrices are again row-major order).
The order of axes is given as an array of 3 integers, whose values denote axes (i.e. {0,1,2} is XYZ, {1,0,2} is YXZ and so on.
```// We need either quaternion or axis/angle pair
// of given rotation
// Go for quaternion now
Quaternionf q = m.getRotationQuaternion ();
float sign =
((order == (order + 1)) ||
(order == (order + 1))) ? 1.0f : -1.0f;
// Array of identity axis vectors
static Vector3f iv[] = {
Vector3fX, Vector3fY, Vector3fZ
};
// Compose matrix that transforms to reordered axes space
Matrix3x4f s;
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
s[4 * row + col] = iv[order[row]][col];
}
}
for (int col = 0; col < 3; col++) {
s[8 + col] = sign * s[8 + col];
}
// Transform quaternion
// As we transform the first 3 values anyways,
// we can as well use vector transformation method here
s.transformVector3InPlace (q);
// Compose new rotation matrix from transformed quaternion
Matrix3x4f rs;
rs.setRotation (q);
// Decompose new matrix to Euler angles
rs.getEulerAngles (angles);
// We have to switch sign, if order was left-hand
angles *= sign;
```
The idea is to transform matrix in such way, that the reordered rotation axes of original matrix become XYZ rotation axes of transformed matrix and then perform trivial decomposition.

Have fun!