欧拉到矩阵以及矩阵到欧拉的转换

我正在尝试使用 .NET/C# 将欧拉角描述的 3D 旋转转换为矩阵，然后再转换回来。我的约定是：

• 左手系统（x 向右，y 向上，z 向前）
• 旋转顺序：绕 y 航向、绕 x 俯仰、绕 z 倾斜
• 使用左手定则旋转为正（拇指指向+无穷大）

我的试用是：

欧拉到矩阵（为了简化，我删除了 x,y,z 翻译部分）

Matrix3D matrix = new Matrix3D() {
    M11 =   cosH * cosB - sinH * sinP * sinB,
    M12 = - sinB * cosP,
    M13 =   sinH * cosB + cosH * sinP * sinB,
    M21 =   cosH * sinB + sinH * sinP * cosB,
    M22 =   cosB * cosP,
    M23 =   sinB * sinH - cosH * sinP * cosB,
    M31 = - sinH * cosP,
    M32 = - sinP,
    M33 =   cosH * cosP,
};

矩阵到欧拉

const double RD_TO_DEG = 180 / Math.PI;            
double h, p, b; // angles in degrees

// extract pitch
double sinP = -matrix.M23;            
if (sinP >= 1) {
    p = 90; }       // pole
else if (sinP <= -1) {
    p = -90; } // pole
else {
    p = Math.Asin(sinP) * RD_TO_DEG; }             

// extract heading and bank
if (sinP < -0.9999 || sinP > 0.9999) { // account for small angle errors
    h = Math.Atan2(-matrix.M31, matrix.M11) * RD_TO_DEG;
    b = 0; }
else {
    h = Math.Atan2(matrix.M13, matrix.M33) * RD_TO_DEG;
    b = Math.Atan2(matrix.M21, matrix.M22) * RD_TO_DEG; }

一定是错的。如果我取 3 个角度，将它们转换为矩阵，然后将矩阵转换回角度，结果与初始值不同。

我浏览了几个具有不同公式的网站，从 euclideanspace.com 开始，但我现在完全迷失了，找不到正确的计算。我很感激你的一点帮助。船上有数学家吗？

首先，应该：

sinP = -matrix.M32

EDIT:完整解决方案如下

我的推导：

Rx(P)=| 1      0       0 |
      | 0  cos P  -sin P |
      | 0  sin P   cos P |

Ry(H)=|  cos H  0  sin H |
      |      0  1      0 |
      | -sin H  0  cos H |

Rz(B)=| cos B  -sin B  0 |
      | sin B   cos B  0 |
      |     0       0  1 |

乘以您的订单：

R = Ry(H)*Rx(P)*Rz(B)
  = | cos H*cos B+sin H*sin P*sin B  cos B*sin H*sin P-sin B*cos H  cos P*sin H |
    |                   cos P*sin B                    cos B*cos P       -sin P |
    | sin B*cos H*sin P-sin H*cos B  sin H*sin B+cos B*cos H*sin P  cos P*cos H |

给出逆向推导：

tan B = M12/M22

sin P = -M32

tan H = M31/M33