Discover Euler angle vector from 3x3 matrix
Uses the conventions above.
Parameters: | M : array-like, shape (3,3) cy_thresh : None or scalar, optional
|
---|---|
Returns: | z : scalar y : scalar x : scalar
|
Notes
If there was no numerical error, the routine could be derived using Sympy expression for z then y then x rotation matrix, which is:
[ cos(y)*cos(z), -cos(y)*sin(z), sin(y)],
[cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
[sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z), cos(x)*cos(y)]
with the obvious derivations for z, y, and x
z = atan2(-r12, r11) y = asin(r13) x = atan2(-r23, r33)
Problems arise when cos(y) is close to zero, because both of:
z = atan2(cos(y)*sin(z), cos(y)*cos(z))
x = atan2(cos(y)*sin(x), cos(x)*cos(y))
will be close to atan2(0, 0), and highly unstable.
The cy fix for numerical instability below is from: Graphics Gems IV, Paul Heckbert (editor), Academic Press, 1994, ISBN: 0123361559. Specifically it comes from EulerAngles.c by Ken Shoemake, and deals with the case where cos(y) is close to zero:
See: http://www.graphicsgems.org/
The code appears to be licensed (from the website) as “can be used without restrictions”.