From the book.
Code:
void ComputeHeadingPitchBank(float *hdg, float *pitch, float *bank)
{
float m11,m12,m13;
float m21,m22,m23;
float m31,m32,m33;
*pitch=0.0f;
*hdg=0.0f;
*bank=0.0f;
//Extract pitch from m23, adjust for domain errors
float sp=-m23;
if (sp<=-1.0f)
{
pitch=-1.570796f; //-pi/2
}
else if (sp>=1.0f)
{
pitch=1.570796f; //pi/2
}
else
{
pitch=asin(sp);
}
//Check for gimbal lock
if (sp>0.9999f)
{
//We are looking/moving straight up
//Bank to zero and just set heading
bank=0.0f;
hdg=atan2(-m31,m11);
}
else
{
hdg=atan2(m13,m33);
//Compute bank from m21 and m22
bank=atan2(m21,m22);
}
}
Is there a way in ODE to retrieve the x,y and z matrices prior to all of the calculations? If there is then you can get the original angles from these. The way I'm doing it here is very slow.