Problem with Euler angles from YZX Rotation Matrix - math

Question

I've gotten stuck getting my euler angles out my rotation matrix.
My conventions are:
Left-handed (x right, z back, y up)
YZX
Left handed angle rotation
My rotation matrix is built up from Euler angles like (from my code):
var xRotationMatrix = $M([
[1, 0, 0, 0],
[0, cx, -sx, 0],
[0, sx, cx, 0],
[0, 0, 0, 1]
]);
var yRotationMatrix = $M([
[ cy, 0, sy, 0],
[ 0, 1, 0, 0],
[-sy, 0, cy, 0],
[ 0, 0, 0, 1]
]);
var zRotationMatrix = $M([
[cz, -sz, 0, 0],
[sz, cz, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]
]);
Which results in a final rotation matrix as:
R(YZX) = | cy.cz, -cy.sz.cx + sy.sx, cy.sz.sx + sy.cx, 0|
| sz, cz.cx, -cz.sx, 0|
|-sy.cz, sy.sz.cx + cy.sx, -sy.sz.sx + cy.cx, 0|
| 0, 0, 0, 1|
I'm calculating my euler angles back from this matrix using this code:
this.anglesFromMatrix = function(m) {
var y = 0, x = 0, z = 0;
if (m.e(2, 1) > 0.999) {
y = Math.atan2(m.e(1, 3), m.e(3, 3));
z = Math.PI / 2;
x = 0;
} else if (m.e(2, 1) < -0.999) {
y = Math.atan2(m.e(1, 3), m.e(3, 3));
z = -Math.PI / 2;
x = 0;
} else {
y = Math.atan2(-m.e(3, 1), -m.e(1, 1));
x = Math.atan2(-m.e(2, 3), m.e(2, 2));
z = Math.asin(m.e(2, 1));
}
return {theta: this.deg(x), phi: this.deg(y), psi: this.deg(z)};
};
I've done the maths backwards and forwards a few times, but I can't see what's wrong. Any help would hugely appreciated.

Answer 1

Your matrix and euler angles aren't consistent. It looks like you should be using
y = Math.atan2(-m.e(3, 1), m.e(1, 1));
instead of
y = Math.atan2(-m.e(3, 1), -m.e(1, 1));
for the general case (the else branch).
I said "looks like" because -- what language is this? I'm assuming you have the indexing correct for this language. Are you sure about atan2? There is no single convention for atan2. In some programming languages the sine term is the first argument, in others, the cosine term is the first argument.

Answer 2

The last and most important branch of the anglesFromMatrix function has a small sign error but otherwise works correctly. Use
y = Math.atan2(-m.e(3, 1), m.e(1, 1))
since only m.e(3, 1) of m.e(1, 1) = cy.cz and m.e(3, 1) = -sy.cz should be inverted. I haven't checked the other branches for errors.
Beware that since sz = m.e(2, 1) has two solutions, the angles (x, y, z) used to construct the matrix m might not be the same as the angles (rx, ry, rz) returned by anglesFromMatrix(m). Instead we can test that the matrix rm constructed from (rx, ry, rz) does indeed equal m.

Answer 3

I worked on this problem extensively to come up with the correct angles for a given matrix. The problem in the math comes from the inability to determine a precise value for the SIN since -SIN(x) = SIN(-x) and this will affect the other values of the matrix. The solution I came up with comes up with two equally valid solutions out of eight possible solutions. I used a standard Z . Y . X matrix form but it should be adaptable to any matrix. Start by findng the three angles from: X = atan(m32,m33): Y = -asin(m31) : Z = atan(m21,m11) : Then create angles X' = -sign(X)*PI+X : Y'= sign(Y)*PI-Y : Z = -sign(Z)*pi+Z . Using these angles create eight set of angle groups : XYZ : X'YZ : XYZ' : X'YZ' : X'Y'Z' : XY'Z' : X'Y'Z : XY'Z
Use these set to create the eight corresponding matrixes. Then do a sum of the difference between the unknown matrix and each matrix. This is a sum of each element of the unknown minus the same element of the test matrix. After doing this, two of the sums will be zero and those matrixes will represent the solution angles to the original matrix. This works for all possible angle combinations including 0's. As 0's are introduced, more of the eight test matrixes become valid. At 0,0,0 they all become idenity matrixes!
Hope this helps, it worked very well for my application.
Bruce
update
After finding problems with Y = -90 or 90 degrees in the solution above. I came up with this solution that seems to reproduce the matrix at all values!
X = if(or(m31=1,m31=-1),0,atan(m33+1e-24,m32))
Y = -asin(m31)
Z = if(or(m31=1,m31=-1),-atan2(m22,m12),atan2(m11+1e-24,m21))
I went the long way around to find this solution, but it wa very enlightening :o)
Hope this helps!
Bruce