Matrix to EulerAngles

Question

I'm trying to extract euler angles from a rotation matrix. My convetions: Matrix column-major, Coordinate System right-handed, Positive Angle right-handed, Rotation Order YXZ (first heading, then attitude, then bank)

I've found this, but couldn't use it because they use other axes orders: (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

Ok, I solve this doing some math. I took a papper and a pen, and wrote the 3 rotation matrix (in my case: X,Y,Z). And then I multiplied them in the order I was willing to rotate (in my case: Y*X*Z).

The resulting matrix has one of its values equal to -sinB, being B the second rotation. You can calculate the B rotation from that value. If you continue looking at the matrix, you will also notice there are 2 values equal to sinA*cosB and cosA*cosB, the division of this two values simplifies the cosB resulting in sinA/cosA that is the same as tanA, being A the first rotation. You can calculate the A rotation from that division. Similarly, you will notice sinC*cosB and cosC*cosB values.

Finally, you need to consider the case where cosB=0 this is when B=90 or B=-90, in this case you CAN'T make the division I told before, because you will be dividing by zero! So in this case you consider B=+-90 C=0 and you calculate A from the much more simple resulting matrix.

So this is the code I wrote for my conventinons!!

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