vector3df getRotationRadians()
{
matrix4 mat = this;
vector3df scale = getScale();
vector3df invScale = new vector3df(1.0f / scale.X, 1.0f / scale.Y, 1.0f / scale.Z);
float Y = -NewMath.FASin(mat[2] * invScale.X);
float C = NewMath.FCos(Y);
float rotx, roty, X, Z;
if (Math.Abs(C) > 0.0005f)
{
float invC = 1.0f / C;
rotx = mat[10] * invC * invScale.Z;
roty = mat[6] * invC * invScale.Y;
X = NewMath.FATan2(roty, rotx);
rotx = mat[0] * invC * invScale.X;
roty = mat[1] * invC * invScale.X;
Z = NewMath.FATan2(roty, rotx);
}
else
{
X = 0;
rotx = mat[5] * invScale.Y;
roty = -mat[4] * invScale.Y;
Z = NewMath.FATan2(roty, rotx);
}
// fix values that get below zero
// before it would set (!) values to 360
// that were above 360:
if (X < 0.0)
{
X += 2 * NewMath.PI;
}
if (Y < 0.0)
{
Y += 2 * NewMath.PI;
}
if (Z < 0.0)
{
Z += 2 * NewMath.PI;
}
return(new vector3df(X, Y, Z));
}
void setRotationRadians(vector3df rotation)
{
float cr = NewMath.FCos(rotation.X);
float sr = NewMath.FSin(rotation.X);
float cp = NewMath.FCos(rotation.Y);
float sp = NewMath.FSin(rotation.Y);
float cy = NewMath.FCos(rotation.Z);
float sy = NewMath.FSin(rotation.Z);
M[0] = (float)(cp * cy);
M[1] = (float)(cp * sy);
M[2] = (float)(-sp);
double srsp = sr * sp;
double crsp = cr * sp;
M[4] = (float)(srsp * cy - cr * sy);
M[5] = (float)(srsp * sy + cr * cy);
M[6] = (float)(sr * cp);
M[8] = (float)(crsp * cy + sr * sy);
M[9] = (float)(crsp * sy - sr * cy);
M[10] = (float)(cr * cp);
}
