public static void HprToMatrix(ref VdsMatrixd rotation, VdsVec3d hpr)
{
double tmp = hpr.X;
hpr.X = hpr.Z;
hpr.Z = hpr.Y;
hpr.Y = tmp;
double ch, sh, cp, sp, cr, sr, srsp, crsp, srcp;
double magicEpsilon = 0.00001;
if (StaticMethod.Equivalent(hpr.X, 0.0, magicEpsilon))
{
ch = 1.0;
sh = 0.0;
}
else
{
sh = Math.Sin(StaticMethod.DegreesToRadians(hpr.X));
ch = Math.Cos(StaticMethod.DegreesToRadians(hpr.X));
}
if (StaticMethod.Equivalent(hpr.Y, 0.0, magicEpsilon))
{
cp = 1.0;
sp = 0.0;
}
else
{
sp = Math.Sin(StaticMethod.DegreesToRadians(hpr.Y));
cp = Math.Cos(StaticMethod.DegreesToRadians(hpr.Y));
}
if (StaticMethod.Equivalent(hpr.Z, 0.0, magicEpsilon))
{
cr = 1.0;
sr = 0.0;
srsp = 0.0;
srcp = 0.0;
crsp = sp;
}
else
{
sr = Math.Sin(StaticMethod.DegreesToRadians(hpr.Z));
cr = Math.Cos(StaticMethod.DegreesToRadians(hpr.Z));
srsp = sr * sp;
crsp = cr * sp;
srcp = sr * cp;
}
rotation.SetMatrixd(ch * cr - sh * srsp, cr * sh + srsp * ch, -srcp, 0.0,
-sh * cp, ch * cp, sp, 0.0,
sr * ch + sh * crsp, sr * sh - crsp * ch, cr * cp, 0.0,
0.0, 0.0, 0.0, 1.0);
}
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;
}
