void GetEulerAngles(DMatrix q, out double yaw, out double pitch, out double roll) { yaw = Math.Atan2(q.M32, q.M33); pitch = Math.Atan2(-q.M31, Math.Sqrt(q.M32*q.M32 + q.M33*q.M33)); roll = Math.Atan2(q.M21, q.M11); }