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;
}
