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

Cabinetwork

Are you sure you want "attitude, heading, bank"? I assume attitude means pitch and heading means yaw, in airplane terms. If you do anything but heading first, it ceases to be a geographically meaningful heading. – Bodice

You are right, i've change the question, the rotation order i'm trying to achieve is yaw, pitch, roll. The thing is that my axes are different from the ones at the example I've posted. I use, yaw=Y, pitch=X, roll=Z, while the example uses yaw=Y, pitch=Z, roll=X. – Cabinetwork

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

Cabinetwork