public bool isZero()
{
for (int i = 0; i < 16; ++i)
{
if (!NewMath.Equals(M[i], 0))
{
return(false);
}
}
return(true);
}
public static bool operator ==(matrix4 first, matrix4 other)
{
for (int i = 0; i < 16; ++i)
{
if (!NewMath.Equals(first[i], other[i]))
{
return(false);
}
}
return(true);
}
vector3df getScale()
{
if (NewMath.IsZero(M[1]) && NewMath.IsZero(M[2]) &&
NewMath.IsZero(M[4]) && NewMath.IsZero(M[6]) &&
NewMath.IsZero(M[8]) && NewMath.IsZero(M[9]))
{
return(new vector3df(M[0], M[5], M[10]));
}
// We have to do the full calculation.
return(new vector3df(NewMath.Sqrt(M[0] * M[0] + M[1] * M[1] + M[2] * M[2]),
NewMath.Sqrt(M[4] * M[4] + M[5] * M[5] + M[6] * M[6]),
NewMath.Sqrt(M[8] * M[8] + M[9] * M[9] + M[10] * M[10])));
}
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));
}
public bool isIdentity()
{
if (!NewMath.Equals(M[0], 1) ||
!NewMath.Equals(M[5], 1) ||
!NewMath.Equals(M[10], 1) ||
!NewMath.Equals(M[15], 1))
{
return(false);
}
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
if ((j != i) && (!NewMath.IsZero(M[i * 4 + j])))
{
return(false);
}
}
}
return(true);
}
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);
}
public static bool operator ==(vector3df first, vector3df other)
{
return(NewMath.Equals(first.X, other.X) &&
NewMath.Equals(first.Y, other.Y) &&
NewMath.Equals(first.Z, other.Z));
}