Conversion euler to matrix and matrix to euler

I'm trying to convert a 3D rotation described in term of euler angles into a matrix and then back, using .NET/C#. My conventions are:

• left handed system (x right, y top, z forward)
• order of rotations: heading around y, pitch around x, bank around z
• rotations are positive using the left hand rule (thumb pointing to +infinity)

My trial is:

Euler to matrix (I've removed the x,y,z translation part for simplification)

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 to Euler

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

It must be wrong. If I take 3 angles, convert them into a matrix and convert the matrix back into angles, the result if different than the intial values.

I have browsed several sites with different formulas, starting with euclideanspace.com, but I'm now completely lost, and can't find the right computations. I' appreciate a little help. Is there a mathematician onboard?

Firstly, should:

sinP = -matrix.M32

EDIT: Full solution follows

My derivation:

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 |

Multiplied with your ordering:

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 |

Which gives reverse derivations:

tan B = M12/M22

sin P = -M32

tan H = M31/M33

There are a huge number of combinations of these functions as the answer changes depending on your conventions. I'm typically using DirectX and the same conventions as Unity. Plus my background is flightsims, space and maps, so yaw then pitch then roll matches lat/lon style too.

Being unclear on the conventions or having mismatched compose/decompose functions can lead to very odd bugs. Also worth bearing in mind that multiple sets of euler angles can produce the same orientation.

Conventions (as above):

• Euler angles: X = Pitch, Y = Yaw, Z = Roll
• Euler order: Rotation applied, yaw then pitch then roll
• Axes: +X Right, +Y Up, +Z Forward
• Matrices: DirectX conventions (using SimpleMath.h from MS DirectXTK)

To convert to OpenGL version, take a look at this.

I've taken Mike Tunnicliffe's answer and converted it to C++ code and added it to my library. I hope other people will save some time by using it.

Worth noting that the compose function clears the 4th column and the translation component to identity, and the decompose function assumes the 3x3 rotation element contains pure rotation (ie no scale etc).

Firstly the code to generate a matrix from Eulers:

//====================================================================================================
// MatrixFromYawPitchRoll
//
// Create matrix based on provided yaw (heading), pitch and roll (bank).
//
// Assumptions:
//  Euler:   X = Pitch, Y = Yaw, Z = Roll
//  Applied: Yaw then pitch then roll
//  Axes:    X = Right, Y = Up, Z = Forward
//  DirectX: Matrices are row major (http://www.mindcontrol.org/~hplus/graphics/matrix-layout.html)
//
// Code is based on Mike Tunnicliffe's answer to this question:
//   https://stackoverflow.com/questions/1996957/conversion-euler-to-matrix-and-matrix-to-euler
inline void MatrixFromYawPitchRoll(
    const DirectX::SimpleMath::Vector3& euler,
    DirectX::SimpleMath::Matrix&        mat)
{
    float cosY = cosf(euler.y);     // Yaw
    float sinY = sinf(euler.y);

    float cosP = cosf(euler.x);     // Pitch
    float sinP = sinf(euler.x);

    float cosR = cosf(euler.z);     // Roll
    float sinR = sinf(euler.z);

    mat = DirectX::SimpleMath::Matrix::Identity;
    mat._11 = cosY * cosR + sinY * sinP * sinR;
    mat._21 = cosR * sinY * sinP - sinR * cosY;
    mat._31 = cosP * sinY;

    mat._12 = cosP * sinR;
    mat._22 = cosR * cosP;
    mat._32 = -sinP;

    mat._13 = sinR * cosY * sinP - sinY * cosR;
    mat._23 = sinY * sinR + cosR * cosY * sinP;
    mat._33 = cosP * cosY;
}

Then code to get back Euler angles from matrix:

//====================================================================================================
// MatrixDecomposeYawPitchRoll
//
// Extract the rotation contained in the provided matrix as yaw (heading), pitch and roll (bank) in
// radiuans.
//
// Assumptions:
//  Euler:   X = Pitch, Y = Yaw, Z = Roll
//  Applied: Yaw then pitch then roll
//  Axes:    X = Right, Y = Up, Z = Forward
//  DirectX: Matrices are row major (http://www.mindcontrol.org/~hplus/graphics/matrix-layout.html)
//
// Code is based on Mike Tunnicliffe's answer to this question:
//   https://stackoverflow.com/questions/1996957/conversion-euler-to-matrix-and-matrix-to-euler
inline void MatrixDecomposeYawPitchRoll(
    const DirectX::SimpleMath::Matrix&  mat,
    DirectX::SimpleMath::Vector3&       euler)
{
    euler.x = asinf(-mat._32);                  // Pitch
    if (cosf(euler.x) > 0.0001)                 // Not at poles
    {
        euler.y = atan2f(mat._31, mat._33);     // Yaw
        euler.z = atan2f(mat._12, mat._22);     // Roll
    }
    else
    {
        euler.y = 0.0f;                         // Yaw
        euler.z = atan2f(-mat._21, mat._11);    // Roll
    }
}

Your idea is wrong: "It must be wrong. If I take 3 angles, convert them into a matrix and convert the matrix back into angles, the result if different than the intial values." It would have been beautiful, but is not necessarily true. In general, more than one triplet of Euler Angles (fixed the convention) leads to the same orientation in the space. This doesn't mean that in your calculation there isn't an error, though. From Wikipedia:

"For example, suppose we use the zyz convention above; then we have the following equivalent pairs:

(90°, 45°, −105°) ≡ (−270°, −315°, 255°)   multiples of 360°

(72°, 0°, 0°) ≡ (40°, 0°, 32°)   singular alignment

(45°, 60°, −30°) ≡ (−135°, −60°, 150°)   bistable flip "