public static void MatrixToHpr(ref VdsVec3d hpr, VdsMatrixd rotation) { VdsMatrixd mat = new VdsMatrixd(); VdsVec3d col1 = new VdsVec3d(rotation.Mat[0, 0], rotation.Mat[0, 1], rotation.Mat[0, 2]); double s = col1.Length(); const double magicEpsilon = 0.00001; if (s <= magicEpsilon) { hpr.X = 0.0; hpr.Y = 0.0; hpr.Z = 0.0; return; } double oneOverS = 1.0f / s; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { mat.Mat[i, j] = rotation.Mat[i, j] * oneOverS; } } double sinPitch = StaticMethod.ClampUnity(mat.Mat[1, 2]); double pitch = Math.Asin(sinPitch); hpr.Y = StaticMethod.RadiansToDegrees(pitch); double cp = Math.Cos(pitch); if (cp > -magicEpsilon && cp < magicEpsilon) { double cr = StaticMethod.ClampUnity(-mat.Mat[2, 1]); double sr = StaticMethod.ClampUnity(mat.Mat[0, 1]); hpr.X = 0.0f; hpr.Z = StaticMethod.RadiansToDegrees(Math.Atan2(sr, cr)); } else { double oneOverCp = 1.0 / cp; double sr = StaticMethod.ClampUnity(-mat.Mat[0, 2] * oneOverCp); double cr = StaticMethod.ClampUnity(mat.Mat[2, 2] * oneOverCp); double sh = StaticMethod.ClampUnity(-mat.Mat[1, 0] * oneOverCp); double ch = StaticMethod.ClampUnity(mat.Mat[1, 1] * oneOverCp); if ((StaticMethod.Equivalent(sh, 0.0, magicEpsilon) && StaticMethod.Equivalent(ch, 0.0, magicEpsilon)) || (StaticMethod.Equivalent(sr, 0.0, magicEpsilon) && StaticMethod.Equivalent(cr, 0.0, magicEpsilon))) { cr = StaticMethod.ClampUnity(-mat.Mat[2, 1]); sr = StaticMethod.ClampUnity(mat.Mat[0, 1]);; hpr.X = 0.0f; } else { hpr.X = StaticMethod.RadiansToDegrees(Math.Atan2(sh, ch)); } hpr.Z = StaticMethod.RadiansToDegrees(Math.Atan2(sr, cr)); } double tmp = hpr.X; hpr.X = hpr.Y; hpr.Y = hpr.Z; hpr.Z = tmp; }