c++ - 矩阵到 EulerAngles

Question

我正在尝试从旋转矩阵中提取欧拉角。我的约定:矩阵列主，坐标系右手，正角右手，旋转顺序YXZ(先航向，然后姿态，然后倾斜)

我找到了这个，但无法使用它，因为它们使用其他轴顺序:(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);
}

Answer 1

好的，我通过一些数学运算解决了这个问题。我拿了一张纸和一支笔，写下了 3 旋转矩阵(在我的例子中:X、Y、Z)。然后我按照我愿意旋转的顺序将它们相乘(在我的例子中:Y*X*Z)。

结果矩阵的其中一个值等于-sinB，即B 第二次旋转。您可以根据该值计算 B 旋转。如果继续查看矩阵，您还会注意到有 2 个值等于 sinA*cosB 和 cosA*cosB，这两个值的除法简化了 cosB 导致 sinA/cosA 与 tanA 相同，成为第一个旋转的 A。您可以计算该分区的 A 旋转。同样，您会注意到 sinC*cosB 和 cosC*cosB 值。

最后需要考虑cosB=0的情况，也就是B=90或者B=-90的情况，在这个万一你不能做我之前告诉过的除法，因为你将除以零!因此，在这种情况下，您考虑 B=+-90 C=0 并根据更简单的结果矩阵计算 A。

这就是我为 session 编写的代码!!

/**
*   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 );
}