/// <summary>
/// Compute the pitch, heading and roll angles of a quaternion in degrees.
/// </summary>
public static Vector3d PitchHeadingRoll(this QuaternionD q)
{
// Extract angles in order: roll (y), pitch (z), heading (x)
Vector3d eulerAngles = q.EulerAngles(AxisOrder.YZX);
var pitch = eulerAngles.y > 180d ? 360d - eulerAngles.y : -eulerAngles.y;
var heading = eulerAngles.z;
var roll = eulerAngles.x >= 90d ? 270d - eulerAngles.x : -90d - eulerAngles.x;
return(new Vector3d(pitch, heading, roll));
}
