public Matrix4X4(Matrix4X4 CopyFrom)
{
for(int i=0; i<16; i++)
{
matrix[i] = CopyFrom.matrix[i];
}
}
public void PrepareMatrix(double Tx, double Ty, double Tz,
double Rx, double Ry, double Rz,
double Sx, double Sy, double Sz)
{
bool Initialized = false;
if (Sx != 1.0f || Sy != 1.0f || Sz != 1.0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Scale(Sx, Sy, Sz);
Multiply(Temp);
}
else
{
Scale(Sx, Sy, Sz);
Initialized = true;
}
}
if (Rx != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(0, Rx);
Multiply(Temp);
}
else
{
Rotate(0, Rx);
Initialized = true;
}
}
if (Ry != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(1, Ry);
Multiply(Temp);
}
else
{
Rotate(1, Ry);
Initialized = true;
}
}
if (Rz != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(2, Rz);
Multiply(Temp);
}
else
{
Rotate(2, Rz);
Initialized = true;
}
}
if (Tx != 0.0f || Ty != 0.0f || Tz != 0.0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Translate(Tx, Ty, Tz);
Multiply(Temp);
}
else
{
Translate(Tx, Ty, Tz);
Initialized = true;
}
if (!Initialized)
{
Identity();
}
}
}
public bool Invert()
{
Matrix4X4 Temp = new Matrix4X4(this);
return(SetToInverse(Temp));
}
public bool SetToInverse(Matrix4X4 OriginalMatrix)
{
IntelInvertC(OriginalMatrix.matrix, matrix);
return(true);
}
public void SetElements(Matrix4X4 CopyFrom)
{
SetElements(CopyFrom.GetElements());
}
public void Multiply(Matrix4X4 One, Matrix4X4 Two)
{
if (this == One || this == Two)
{
throw new System.FormatException("Neither of the input parameters can be the same Matrix as this.");
}
for(int i = 0; i < 4; i++)
{
for(int j = 0; j < 4; j++)
{
SetElement(i, j, 0);
for(int k = 0; k < 4; k++)
{
AddElement(i, j, One.GetElement(i, k) * Two.GetElement(k, j));
}
}
}
}
public void Multiply(Matrix4X4 Two)
{
Matrix4X4 Hold = new Matrix4X4(this);
Multiply(Hold, Two);
}
public bool SetToInverse(Matrix4X4 OriginalMatrix)
{
IntelInvertC(OriginalMatrix.matrix, matrix);
return true;
}
public void PrepareInvMatrix(double Tx, double Ty, double Tz,
double Rx, double Ry, double Rz,
double Sx, double Sy, double Sz)
{
Matrix4X4 M0 = new Matrix4X4();
Matrix4X4 M1 = new Matrix4X4();
Matrix4X4 M2 = new Matrix4X4();
Matrix4X4 M3 = new Matrix4X4();
Matrix4X4 M4 = new Matrix4X4();
Matrix4X4 M5 = new Matrix4X4();
Matrix4X4 M6 = new Matrix4X4();
Matrix4X4 M7 = new Matrix4X4();
M0.Scale(Sx, Sy, Sz);
M1.Rotate(0, Rx);
M2.Rotate(1, Ry);
M3.Rotate(2, Rz);
M4.Translate(Tx, Ty, Tz);
// 4 * 3 * 2 * 1 * 0
M5.Multiply(M4, M3);
M6.Multiply(M5, M2);
M7.Multiply(M6, M1);
Multiply(M7, M0);
}
public void PrepareMatrix(double Tx, double Ty, double Tz,
double Rx, double Ry, double Rz,
double Sx, double Sy, double Sz)
{
bool Initialized = false;
if (Sx != 1.0f || Sy != 1.0f || Sz != 1.0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Scale(Sx, Sy, Sz);
Multiply(Temp);
}
else
{
Scale(Sx, Sy, Sz);
Initialized = true;
}
}
if (Rx != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(0, Rx);
Multiply(Temp);
}
else
{
Rotate(0, Rx);
Initialized = true;
}
}
if (Ry != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(1, Ry);
Multiply(Temp);
}
else
{
Rotate(1, Ry);
Initialized = true;
}
}
if (Rz != .0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(2, Rz);
Multiply(Temp);
}
else
{
Rotate(2, Rz);
Initialized = true;
}
}
if (Tx != 0.0f || Ty != 0.0f || Tz != 0.0f)
{
if (Initialized)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Translate(Tx, Ty, Tz);
Multiply(Temp);
}
else
{
Translate(Tx, Ty, Tz);
Initialized = true;
}
if (!Initialized)
{
Identity();
}
}
}
public bool Equals(Matrix4X4 OtherMatrix, double ErrorRange)
{
for(int i=0; i<4; i++)
{
for(int j=0; j<4; j++)
{
if( GetElement(i, j) < OtherMatrix.GetElement(i, j) - ErrorRange
|| GetElement(i, j) > OtherMatrix.GetElement(i, j) + ErrorRange)
{
return false;
}
}
}
return true;
}
public void AddRotate(uint Axis, double Theta)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Rotate(Axis, Theta);
Multiply(Temp);
}
public void AddTranslate(Vector3 Vect)
{
Matrix4X4 Temp = new Matrix4X4();
Temp.Translate(Vect.x, Vect.y, Vect.z);
Multiply(Temp);
}
public bool Invert()
{
Matrix4X4 Temp = new Matrix4X4(this);
return SetToInverse(Temp);
}
public void Multiply(Matrix4X4 Two)
{
Matrix4X4 Hold = new Matrix4X4(this);
Multiply(Hold, Two);
}
public void SetElements(Matrix4X4 CopyFrom)
{
SetElements(CopyFrom.GetElements());
}
public static Matrix4X4 operator *(Matrix4X4 A, Matrix4X4 B)
{
Matrix4X4 Temp = new Matrix4X4(A);
Temp.Multiply(B);
return Temp;
}