矩阵到EulerAngles [英] Matrix to EulerAngles
问题描述
我试图从旋转矩阵中提取欧拉角。
我的convetions:
矩阵列主,坐标系右旋,正角右旋,旋转顺序YXZ(第一个标题,然后态度,然后银行)
I'm trying to extract euler angles from a rotation matrix. My convetions: Matrix column-major, Coordinate System right-handed, Positive Angle right-handed, Rotation Order YXZ (first heading, then attitude, then bank)
我发现这个,但不能使用它,因为他们使用其他轴顺序:(http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm)
I've found this, but couldn't use it because they use other axes orders: (http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm)
/** this conversion uses conventions as described on page:
* http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
* Coordinate System: right hand
* Positive angle: right hand
* Order of euler angles: heading first, then attitude, then bank
* matrix row column ordering:
* [m00 m01 m02]
* [m10 m11 m12]
* [m20 m21 m22]*/
public final void rotate(matrix m) {
// Assuming the angles are in radians.
if (m.m10 > 0.998) { // singularity at north pole
heading = Math.atan2(m.m02,m.m22);
attitude = Math.PI/2;
bank = 0;
return;
}
if (m.m10 < -0.998) { // singularity at south pole
heading = Math.atan2(m.m02,m.m22);
attitude = -Math.PI/2;
bank = 0;
return;
}
heading = Math.atan2(-m.m20,m.m00);
bank = Math.atan2(-m.m12,m.m11);
attitude = Math.asin(m.m10);
}
推荐答案
一些数学。我拿了一个papper和一支钢笔,写了3个旋转矩阵(在我的例子:X,Y,Z)。然后我按照我愿意旋转的顺序乘以它们(在我的例子中:Y * X * Z)。
Ok, I solve this doing some math. I took a papper and a pen, and wrote the 3 rotation matrix (in my case: X,Y,Z). And then I multiplied them in the order I was willing to rotate (in my case: Y*X*Z).
结果矩阵的值之一等于 -sinB ,第二轮为 B 。您可以从该值计算 B 旋转。如果继续查看矩阵,您还会注意到有两个等于 sinA * cosB 和 cosA * cosB 的值,这两个值的除法简化了 cosB ,导致 sinA / cosA 与 tanA 相同,即 A 第一个旋转。您可以计算该部门的 A 轮换。同样,您会注意到 sinC * cosB 和 cosC * cosB 值。
The resulting matrix has one of its values equal to -sinB, being B the second rotation. You can calculate the B rotation from that value. If you continue looking at the matrix, you will also notice there are 2 values equal to sinA*cosB and cosA*cosB, the division of this two values simplifies the cosB resulting in sinA/cosA that is the same as tanA, being A the first rotation. You can calculate the A rotation from that division. Similarly, you will notice sinC*cosB and cosC*cosB values.
最后, cosB = 0 这是 B = 90 或 B = -90 的情况,在这种情况下,之前,因为你会除以零!因此,在这种情况下,您会考虑 B = + - 90 C = 0 ,并从更简单的结果矩阵计算 A 。
Finally, you need to consider the case where cosB=0 this is when B=90 or B=-90, in this case you CAN'T make the division I told before, because you will be dividing by zero! So in this case you consider B=+-90 C=0 and you calculate A from the much more simple resulting matrix.
这是我为conventinons写的代码。
So this is the code I wrote for my conventinons!!
/**
* Matrix column-major
* Coordinate System right-handed
* Positive Angle right-handed
* Rotation Order YXZ (first heading, then attitude, then bank)
* [m00 m01 m02]
* [m10 m11 m12]
* [m20 m21 m22]
*/
public final void rotate(matrix m) {
// Assuming the angles are in radians.
if ( m.m12 > 0.998 || m.m12 < -0.998 ) { // singularity at south or north pole
heading = Math.atan2( -m.m20, m.m00 );
bank = 0;
} else {
heading = Math.atan2( m.m02, m.m22 );
bank = Math.atan2( m.m10, m.m11 );
}
attitude = Math.asin( m.m12 );
}
这篇关于矩阵到EulerAngles的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!