矩阵到欧拉角

我正在尝试从旋转矩阵中提取欧拉角。 我的约定： 矩阵列主、坐标系右手、正角右手、旋转顺序 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);
}

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

结果矩阵的其中一个值等于-sinB, being B第二轮旋转。您可以计算B从该值开始旋转。如果您继续查看该矩阵，您还会注意到有 2 个值等于sinA*cosB and cosA*cosB，这两个值的除法简化了cosB导致正弦A/正弦A那是一样的tanA, being A第一次旋转。您可以计算A从该部门轮换。同样，你会注意到正弦*余弦B and 余弦C*余弦B values.

最后，您需要考虑以下情况cosB=0这是当B=90 or B=-90，在这种情况下，你不能进行我之前告诉过的除法，因为你将被零除！所以在这种情况下你考虑B=+-90 C=0你计算A从更简单的结果矩阵。

这就是我为我的约定编写的代码！

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