Próbuję przekonwertować rotację 3D opisaną w terminach kątów eulera na macierz, a następnie z powrotem, używając .NET / C #. Moje konwencje to:

leworęczny system (x prawo, y góra, z do przodu)kolejność obrotów: kierunek wokół y, nachylenie wokół x, brzeg wokół zobroty są dodatnie przy użyciu reguły lewej ręki (kciuk wskazujący na + nieskończoność)

Moja próba to:

Euler do macierzy (Usunąłem część tłumaczenia x, y, z dla uproszczenia)

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,
};

Matrix do Eulera

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; }

To musi być złe. Jeśli przyjmę 3 kąty, przekształć je w macierz i przekonwertuj macierz z powrotem na kąty, wynik, jeśli jest inny niż wartości początkowe.

Przeglądałem kilka witryn z różnymi formułami, zaczynając od euclideanspace.com, ale teraz jestem całkowicie zagubiony i nie mogę znaleźć właściwych obliczeń. Doceniam małą pomoc. Czy na pokładzie jest matematyk?