From the book.
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.
void ComputeHeadingPitchBank(float *hdg, float *pitch, float *bank)
//Extract pitch from m23, adjust for domain errors
else if (sp>=1.0f)
//Check for gimbal lock
//We are looking/moving straight up
//Bank to zero and just set heading
//Compute bank from m21 and m22