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