/// <summary>General invert routine.</summary>
/// <remarks>
/// General invert routine. Inverts m1 and places the result in "this".
/// Note that this routine handles both the "this" version and the
/// non-"this" version.
/// Also note that since this routine is slow anyway, we won't worry
/// about allocating a little bit of garbage.
/// </remarks>
internal void InvertGeneral(Matrix4f m1)
{
double[] temp = new double[16];
double[] result = new double[16];
int[] row_perm = new int[4];
int i;
int r;
int c;
// Use LU decomposition and backsubstitution code specifically
// for floating-point 4x4 matrices.
// Copy source matrix to t1tmp
temp[0] = m1.m00;
temp[1] = m1.m01;
temp[2] = m1.m02;
temp[3] = m1.m03;
temp[4] = m1.m10;
temp[5] = m1.m11;
temp[6] = m1.m12;
temp[7] = m1.m13;
temp[8] = m1.m20;
temp[9] = m1.m21;
temp[10] = m1.m22;
temp[11] = m1.m23;
temp[12] = m1.m30;
temp[13] = m1.m31;
temp[14] = m1.m32;
temp[15] = m1.m33;
// Calculate LU decomposition: Is the matrix singular?
if (!LuDecomposition(temp, row_perm))
{
// Matrix has no inverse
throw new SingularMatrixException("cannot invert matrix");
}
// Perform back substitution on the identity matrix
for (i = 0; i < 16; i++)
{
result[i] = 0.0;
}
result[0] = 1.0;
result[5] = 1.0;
result[10] = 1.0;
result[15] = 1.0;
LuBacksubstitution(temp, row_perm, result);
this.m00 = (float)result[0];
this.m01 = (float)result[1];
this.m02 = (float)result[2];
this.m03 = (float)result[3];
this.m10 = (float)result[4];
this.m11 = (float)result[5];
this.m12 = (float)result[6];
this.m13 = (float)result[7];
this.m20 = (float)result[8];
this.m21 = (float)result[9];
this.m22 = (float)result[10];
this.m23 = (float)result[11];
this.m30 = (float)result[12];
this.m31 = (float)result[13];
this.m32 = (float)result[14];
this.m33 = (float)result[15];
}
/// <summary>Sets the value of this matrix to the transpose of the argument matrix.</summary>
/// <remarks>Sets the value of this matrix to the transpose of the argument matrix.</remarks>
/// <param name="m1">the matrix to be transposed</param>
public void Transpose(Matrix4f m1)
{
if (this != m1)
{
this.m00 = m1.m00;
this.m01 = m1.m10;
this.m02 = m1.m20;
this.m03 = m1.m30;
this.m10 = m1.m01;
this.m11 = m1.m11;
this.m12 = m1.m21;
this.m13 = m1.m31;
this.m20 = m1.m02;
this.m21 = m1.m12;
this.m22 = m1.m22;
this.m23 = m1.m32;
this.m30 = m1.m03;
this.m31 = m1.m13;
this.m32 = m1.m23;
this.m33 = m1.m33;
}
else
{
this.Transpose();
}
}
/// <summary>Sets the value of this matrix to the matrix sum of matrices m1 and m2.</summary>
/// <remarks>Sets the value of this matrix to the matrix sum of matrices m1 and m2.</remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Add(Matrix4f m1, Matrix4f m2)
{
this.m00 = m1.m00 + m2.m00;
this.m01 = m1.m01 + m2.m01;
this.m02 = m1.m02 + m2.m02;
this.m03 = m1.m03 + m2.m03;
this.m10 = m1.m10 + m2.m10;
this.m11 = m1.m11 + m2.m11;
this.m12 = m1.m12 + m2.m12;
this.m13 = m1.m13 + m2.m13;
this.m20 = m1.m20 + m2.m20;
this.m21 = m1.m21 + m2.m21;
this.m22 = m1.m22 + m2.m22;
this.m23 = m1.m23 + m2.m23;
this.m30 = m1.m30 + m2.m30;
this.m31 = m1.m31 + m2.m31;
this.m32 = m1.m32 + m2.m32;
this.m33 = m1.m33 + m2.m33;
}
/// <summary>
/// Adds a scalar to each component of the matrix m1 and places
/// the result into this.
/// </summary>
/// <remarks>
/// Adds a scalar to each component of the matrix m1 and places
/// the result into this. Matrix m1 is not modified.
/// </remarks>
/// <param name="scalar">the scalar adder</param>
/// <param name="m1">the original matrix values</param>
public void Add(float scalar, Matrix4f m1)
{
this.m00 = m1.m00 + scalar;
this.m01 = m1.m01 + scalar;
this.m02 = m1.m02 + scalar;
this.m03 = m1.m03 + scalar;
this.m10 = m1.m10 + scalar;
this.m11 = m1.m11 + scalar;
this.m12 = m1.m12 + scalar;
this.m13 = m1.m13 + scalar;
this.m20 = m1.m20 + scalar;
this.m21 = m1.m21 + scalar;
this.m22 = m1.m22 + scalar;
this.m23 = m1.m23 + scalar;
this.m30 = m1.m30 + scalar;
this.m31 = m1.m31 + scalar;
this.m32 = m1.m32 + scalar;
this.m33 = m1.m33 + scalar;
}
/// <summary>
/// Sets this matrix to the matrix difference of itself and
/// matrix m1 (this = this - m1).
/// </summary>
/// <remarks>
/// Sets this matrix to the matrix difference of itself and
/// matrix m1 (this = this - m1).
/// </remarks>
/// <param name="m1">the other matrix</param>
public void Sub(Matrix4f m1)
{
this.m00 -= m1.m00;
this.m01 -= m1.m01;
this.m02 -= m1.m02;
this.m03 -= m1.m03;
this.m10 -= m1.m10;
this.m11 -= m1.m11;
this.m12 -= m1.m12;
this.m13 -= m1.m13;
this.m20 -= m1.m20;
this.m21 -= m1.m21;
this.m22 -= m1.m22;
this.m23 -= m1.m23;
this.m30 -= m1.m30;
this.m31 -= m1.m31;
this.m32 -= m1.m32;
this.m33 -= m1.m33;
}
/// <summary>
/// Returns true if all of the data members of Matrix4f m1 are
/// equal to the corresponding data members in this Matrix4f.
/// </summary>
/// <remarks>
/// Returns true if all of the data members of Matrix4f m1 are
/// equal to the corresponding data members in this Matrix4f.
/// </remarks>
/// <param name="m1">the matrix with which the comparison is made.</param>
/// <returns>true or false</returns>
public virtual bool Equals(Matrix4f m1)
{
try
{
return (this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02 && this.m03
== m1.m03 && this.m10 == m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12 &&
this.m13 == m1.m13 && this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1
.m22 && this.m23 == m1.m23 && this.m30 == m1.m30 && this.m31 == m1.m31 && this.m32
== m1.m32 && this.m33 == m1.m33);
}
catch (ArgumentNullException)
{
return false;
}
}
/// <summary>
/// Places the values in the upper 4x4 of this GMatrix into
/// the matrix m1.
/// </summary>
/// <remarks>
/// Places the values in the upper 4x4 of this GMatrix into
/// the matrix m1.
/// </remarks>
/// <param name="m1">The matrix that will hold the new values</param>
public void Get(Matrix4f m1)
{
if (nRow < 4 || nCol < 4)
{
m1.SetZero();
if (nCol > 0)
{
if (nRow > 0)
{
m1.m00 = (float)values[0][0];
if (nRow > 1)
{
m1.m10 = (float)values[1][0];
if (nRow > 2)
{
m1.m20 = (float)values[2][0];
if (nRow > 3)
{
m1.m30 = (float)values[3][0];
}
}
}
}
if (nCol > 1)
{
if (nRow > 0)
{
m1.m01 = (float)values[0][1];
if (nRow > 1)
{
m1.m11 = (float)values[1][1];
if (nRow > 2)
{
m1.m21 = (float)values[2][1];
if (nRow > 3)
{
m1.m31 = (float)values[3][1];
}
}
}
}
if (nCol > 2)
{
if (nRow > 0)
{
m1.m02 = (float)values[0][2];
if (nRow > 1)
{
m1.m12 = (float)values[1][2];
if (nRow > 2)
{
m1.m22 = (float)values[2][2];
if (nRow > 3)
{
m1.m32 = (float)values[3][2];
}
}
}
}
if (nCol > 3)
{
if (nRow > 0)
{
m1.m03 = (float)values[0][3];
if (nRow > 1)
{
m1.m13 = (float)values[1][3];
if (nRow > 2)
{
m1.m23 = (float)values[2][3];
if (nRow > 3)
{
m1.m33 = (float)values[3][3];
}
}
}
}
}
}
}
}
}
else
{
m1.m00 = (float)values[0][0];
m1.m01 = (float)values[0][1];
m1.m02 = (float)values[0][2];
m1.m03 = (float)values[0][3];
m1.m10 = (float)values[1][0];
m1.m11 = (float)values[1][1];
m1.m12 = (float)values[1][2];
m1.m13 = (float)values[1][3];
m1.m20 = (float)values[2][0];
m1.m21 = (float)values[2][1];
m1.m22 = (float)values[2][2];
m1.m23 = (float)values[2][3];
m1.m30 = (float)values[3][0];
m1.m31 = (float)values[3][1];
m1.m32 = (float)values[3][2];
m1.m33 = (float)values[3][3];
}
}
/// <summary>
/// Multiplies matrix m1 times the transpose of matrix m2, and
/// places the result into this.
/// </summary>
/// <remarks>
/// Multiplies matrix m1 times the transpose of matrix m2, and
/// places the result into this.
/// </remarks>
/// <param name="m1">the matrix on the left hand side of the multiplication</param>
/// <param name="m2">the matrix on the right hand side of the multiplication</param>
public void MulTransposeRight(Matrix4f m1, Matrix4f m2)
{
if (this != m1 && this != m2)
{
this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02 + m1.m03 * m2.m03;
this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12 + m1.m03 * m2.m13;
this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22 + m1.m03 * m2.m23;
this.m03 = m1.m00 * m2.m30 + m1.m01 * m2.m31 + m1.m02 * m2.m32 + m1.m03 * m2.m33;
this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02 + m1.m13 * m2.m03;
this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12 + m1.m13 * m2.m13;
this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22 + m1.m13 * m2.m23;
this.m13 = m1.m10 * m2.m30 + m1.m11 * m2.m31 + m1.m12 * m2.m32 + m1.m13 * m2.m33;
this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02 + m1.m23 * m2.m03;
this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12 + m1.m23 * m2.m13;
this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22 + m1.m23 * m2.m23;
this.m23 = m1.m20 * m2.m30 + m1.m21 * m2.m31 + m1.m22 * m2.m32 + m1.m23 * m2.m33;
this.m30 = m1.m30 * m2.m00 + m1.m31 * m2.m01 + m1.m32 * m2.m02 + m1.m33 * m2.m03;
this.m31 = m1.m30 * m2.m10 + m1.m31 * m2.m11 + m1.m32 * m2.m12 + m1.m33 * m2.m13;
this.m32 = m1.m30 * m2.m20 + m1.m31 * m2.m21 + m1.m32 * m2.m22 + m1.m33 * m2.m23;
this.m33 = m1.m30 * m2.m30 + m1.m31 * m2.m31 + m1.m32 * m2.m32 + m1.m33 * m2.m33;
}
else
{
float m00;
float m01;
float m02;
float m03;
float m10;
float m11;
float m12;
float m13;
float m20;
float m21;
float m22;
float m23;
float m30;
float m31;
float m32;
float m33;
// vars for temp result matrix
m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02 + m1.m03 * m2.m03;
m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12 + m1.m03 * m2.m13;
m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22 + m1.m03 * m2.m23;
m03 = m1.m00 * m2.m30 + m1.m01 * m2.m31 + m1.m02 * m2.m32 + m1.m03 * m2.m33;
m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02 + m1.m13 * m2.m03;
m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12 + m1.m13 * m2.m13;
m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22 + m1.m13 * m2.m23;
m13 = m1.m10 * m2.m30 + m1.m11 * m2.m31 + m1.m12 * m2.m32 + m1.m13 * m2.m33;
m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02 + m1.m23 * m2.m03;
m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12 + m1.m23 * m2.m13;
m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22 + m1.m23 * m2.m23;
m23 = m1.m20 * m2.m30 + m1.m21 * m2.m31 + m1.m22 * m2.m32 + m1.m23 * m2.m33;
m30 = m1.m30 * m2.m00 + m1.m31 * m2.m01 + m1.m32 * m2.m02 + m1.m33 * m2.m03;
m31 = m1.m30 * m2.m10 + m1.m31 * m2.m11 + m1.m32 * m2.m12 + m1.m33 * m2.m13;
m32 = m1.m30 * m2.m20 + m1.m31 * m2.m21 + m1.m32 * m2.m22 + m1.m33 * m2.m23;
m33 = m1.m30 * m2.m30 + m1.m31 * m2.m31 + m1.m32 * m2.m32 + m1.m33 * m2.m33;
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m03 = m03;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m13 = m13;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
this.m23 = m23;
this.m30 = m30;
this.m31 = m31;
this.m32 = m32;
this.m33 = m33;
}
}
/// <summary>
/// Sets the value of this matrix equal to the negation of
/// of the Matrix4f parameter.
/// </summary>
/// <remarks>
/// Sets the value of this matrix equal to the negation of
/// of the Matrix4f parameter.
/// </remarks>
/// <param name="m1">the source matrix</param>
public void Negate(Matrix4f m1)
{
this.m00 = -m1.m00;
this.m01 = -m1.m01;
this.m02 = -m1.m02;
this.m03 = -m1.m03;
this.m10 = -m1.m10;
this.m11 = -m1.m11;
this.m12 = -m1.m12;
this.m13 = -m1.m13;
this.m20 = -m1.m20;
this.m21 = -m1.m21;
this.m22 = -m1.m22;
this.m23 = -m1.m23;
this.m30 = -m1.m30;
this.m31 = -m1.m31;
this.m32 = -m1.m32;
this.m33 = -m1.m33;
}
/// <summary>
/// Multiplies each element of matrix m1 by a scalar and places
/// the result into this.
/// </summary>
/// <remarks>
/// Multiplies each element of matrix m1 by a scalar and places
/// the result into this. Matrix m1 is not modified.
/// </remarks>
/// <param name="scalar">the scalar multiplier.</param>
/// <param name="m1">the original matrix.</param>
public void Mul(float scalar, Matrix4f m1)
{
this.m00 = m1.m00 * scalar;
this.m01 = m1.m01 * scalar;
this.m02 = m1.m02 * scalar;
this.m03 = m1.m03 * scalar;
this.m10 = m1.m10 * scalar;
this.m11 = m1.m11 * scalar;
this.m12 = m1.m12 * scalar;
this.m13 = m1.m13 * scalar;
this.m20 = m1.m20 * scalar;
this.m21 = m1.m21 * scalar;
this.m22 = m1.m22 * scalar;
this.m23 = m1.m23 * scalar;
this.m30 = m1.m30 * scalar;
this.m31 = m1.m31 * scalar;
this.m32 = m1.m32 * scalar;
this.m33 = m1.m33 * scalar;
}
/// <summary>
/// Sets the value of this matrix to the result of multiplying itself
/// with matrix m1.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the result of multiplying itself
/// with matrix m1.
/// </remarks>
/// <param name="m1">the other matrix</param>
public void Mul(Matrix4f m1)
{
float m00;
float m01;
float m02;
float m03;
float m10;
float m11;
float m12;
float m13;
float m20;
float m21;
float m22;
float m23;
float m30;
float m31;
float m32;
float m33;
// vars for temp result matrix
m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20 + this.m03 * m1.m30;
m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21 + this.m03 * m1.m31;
m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22 + this.m03 * m1.m32;
m03 = this.m00 * m1.m03 + this.m01 * m1.m13 + this.m02 * m1.m23 + this.m03 * m1.m33;
m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20 + this.m13 * m1.m30;
m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21 + this.m13 * m1.m31;
m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22 + this.m13 * m1.m32;
m13 = this.m10 * m1.m03 + this.m11 * m1.m13 + this.m12 * m1.m23 + this.m13 * m1.m33;
m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20 + this.m23 * m1.m30;
m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21 + this.m23 * m1.m31;
m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22 + this.m23 * m1.m32;
m23 = this.m20 * m1.m03 + this.m21 * m1.m13 + this.m22 * m1.m23 + this.m23 * m1.m33;
m30 = this.m30 * m1.m00 + this.m31 * m1.m10 + this.m32 * m1.m20 + this.m33 * m1.m30;
m31 = this.m30 * m1.m01 + this.m31 * m1.m11 + this.m32 * m1.m21 + this.m33 * m1.m31;
m32 = this.m30 * m1.m02 + this.m31 * m1.m12 + this.m32 * m1.m22 + this.m33 * m1.m32;
m33 = this.m30 * m1.m03 + this.m31 * m1.m13 + this.m32 * m1.m23 + this.m33 * m1.m33;
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m03 = m03;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m13 = m13;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
this.m23 = m23;
this.m30 = m30;
this.m31 = m31;
this.m32 = m32;
this.m33 = m33;
}
/// <summary>
/// Sets the value of this matrix to the matrix inverse
/// of the passed (user declared) matrix m1.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix inverse
/// of the passed (user declared) matrix m1.
/// </remarks>
/// <param name="m1">the matrix to be inverted</param>
public void Invert(Matrix4f m1)
{
InvertGeneral(m1);
}
/// <summary>
/// Sets the value of this axis-angle to the rotational component of
/// the passed matrix.
/// </summary>
/// <remarks>
/// Sets the value of this axis-angle to the rotational component of
/// the passed matrix.
/// If the specified matrix has no rotational component, the value
/// of this AxisAngle4d is set to an angle of 0 about an axis of (0,1,0).
/// </remarks>
/// <param name="m1">the matrix4f</param>
public void Set(Matrix4f m1)
{
Matrix3d m3d = new Matrix3d();
m1.Get(m3d);
x = (float)(m3d.m21 - m3d.m12);
y = (float)(m3d.m02 - m3d.m20);
z = (float)(m3d.m10 - m3d.m01);
double mag = x * x + y * y + z * z;
if (mag > Eps)
{
mag = Math.Sqrt(mag);
double sin = 0.5 * mag;
double cos = 0.5 * (m3d.m00 + m3d.m11 + m3d.m22 - 1.0);
angle = (float)Math.Atan2(sin, cos);
double invMag = 1.0 / mag;
x = x * invMag;
y = y * invMag;
z = z * invMag;
}
else
{
x = 0.0f;
y = 1.0f;
z = 0.0f;
angle = 0.0f;
}
}
/// <summary>
/// Sets the value of this axis-angle to the rotational component of
/// the passed matrix.
/// </summary>
/// <remarks>
/// Sets the value of this axis-angle to the rotational component of
/// the passed matrix.
/// If the specified matrix has no rotational component, the value
/// of this AxisAngle4f is set to an angle of 0 about an axis of (0,1,0).
/// </remarks>
/// <param name="m1">the matrix4f</param>
public void Set(Matrix4f m1)
{
Matrix3f m3f = new Matrix3f();
m1.Get(m3f);
x = m3f.m21 - m3f.m12;
y = m3f.m02 - m3f.m20;
z = m3f.m10 - m3f.m01;
double mag = x * x + y * y + z * z;
if (mag > Eps)
{
mag = Math.Sqrt(mag);
double sin = 0.5 * mag;
double cos = 0.5 * (m3f.m00 + m3f.m11 + m3f.m22 - 1.0);
angle = (float)Math.Atan2(sin, cos);
double invMag = 1.0 / mag;
x = (float)(x * invMag);
y = (float)(y * invMag);
z = (float)(z * invMag);
}
else
{
x = 0.0f;
y = 1.0f;
z = 0.0f;
angle = 0.0f;
}
}
/// <summary>Sets the value of this matrix to the sum of itself and matrix m1.</summary>
/// <remarks>Sets the value of this matrix to the sum of itself and matrix m1.</remarks>
/// <param name="m1">the other matrix</param>
public void Add(Matrix4f m1)
{
this.m00 += m1.m00;
this.m01 += m1.m01;
this.m02 += m1.m02;
this.m03 += m1.m03;
this.m10 += m1.m10;
this.m11 += m1.m11;
this.m12 += m1.m12;
this.m13 += m1.m13;
this.m20 += m1.m20;
this.m21 += m1.m21;
this.m22 += m1.m22;
this.m23 += m1.m23;
this.m30 += m1.m30;
this.m31 += m1.m31;
this.m32 += m1.m32;
this.m33 += m1.m33;
}
/// <summary>
/// Constructs a new matrix with the same values as the
/// Matrix4f parameter.
/// </summary>
/// <remarks>
/// Constructs a new matrix with the same values as the
/// Matrix4f parameter.
/// </remarks>
/// <param name="m1">the source matrix</param>
public Matrix4f(Matrix4f m1)
{
this.m00 = m1.m00;
this.m01 = m1.m01;
this.m02 = m1.m02;
this.m03 = m1.m03;
this.m10 = m1.m10;
this.m11 = m1.m11;
this.m12 = m1.m12;
this.m13 = m1.m13;
this.m20 = m1.m20;
this.m21 = m1.m21;
this.m22 = m1.m22;
this.m23 = m1.m23;
this.m30 = m1.m30;
this.m31 = m1.m31;
this.m32 = m1.m32;
this.m33 = m1.m33;
}
/// <summary>
/// Returns true if the L-infinite distance between this matrix
/// and matrix m1 is less than or equal to the epsilon parameter,
/// otherwise returns false.
/// </summary>
/// <remarks>
/// Returns true if the L-infinite distance between this matrix
/// and matrix m1 is less than or equal to the epsilon parameter,
/// otherwise returns false. The L-infinite
/// distance is equal to
/// MAX[i=0,1,2,3 ; j=0,1,2,3 ; abs(this.m(i,j) - m1.m(i,j)]
/// </remarks>
/// <param name="m1">the matrix to be compared to this matrix</param>
/// <param name="epsilon">the threshold value</param>
public virtual bool EpsilonEquals(Matrix4f m1, float epsilon)
{
bool status = true;
if (Math.Abs(this.m00 - m1.m00) > epsilon)
{
status = false;
}
if (Math.Abs(this.m01 - m1.m01) > epsilon)
{
status = false;
}
if (Math.Abs(this.m02 - m1.m02) > epsilon)
{
status = false;
}
if (Math.Abs(this.m03 - m1.m03) > epsilon)
{
status = false;
}
if (Math.Abs(this.m10 - m1.m10) > epsilon)
{
status = false;
}
if (Math.Abs(this.m11 - m1.m11) > epsilon)
{
status = false;
}
if (Math.Abs(this.m12 - m1.m12) > epsilon)
{
status = false;
}
if (Math.Abs(this.m13 - m1.m13) > epsilon)
{
status = false;
}
if (Math.Abs(this.m20 - m1.m20) > epsilon)
{
status = false;
}
if (Math.Abs(this.m21 - m1.m21) > epsilon)
{
status = false;
}
if (Math.Abs(this.m22 - m1.m22) > epsilon)
{
status = false;
}
if (Math.Abs(this.m23 - m1.m23) > epsilon)
{
status = false;
}
if (Math.Abs(this.m30 - m1.m30) > epsilon)
{
status = false;
}
if (Math.Abs(this.m31 - m1.m31) > epsilon)
{
status = false;
}
if (Math.Abs(this.m32 - m1.m32) > epsilon)
{
status = false;
}
if (Math.Abs(this.m33 - m1.m33) > epsilon)
{
status = false;
}
return (status);
}
/// <summary>
/// Performs an element-by-element subtraction of matrix m2 from
/// matrix m1 and places the result into matrix this (this =
/// m2 - m1).
/// </summary>
/// <remarks>
/// Performs an element-by-element subtraction of matrix m2 from
/// matrix m1 and places the result into matrix this (this =
/// m2 - m1).
/// </remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Sub(Matrix4f m1, Matrix4f m2)
{
this.m00 = m1.m00 - m2.m00;
this.m01 = m1.m01 - m2.m01;
this.m02 = m1.m02 - m2.m02;
this.m03 = m1.m03 - m2.m03;
this.m10 = m1.m10 - m2.m10;
this.m11 = m1.m11 - m2.m11;
this.m12 = m1.m12 - m2.m12;
this.m13 = m1.m13 - m2.m13;
this.m20 = m1.m20 - m2.m20;
this.m21 = m1.m21 - m2.m21;
this.m22 = m1.m22 - m2.m22;
this.m23 = m1.m23 - m2.m23;
this.m30 = m1.m30 - m2.m30;
this.m31 = m1.m31 - m2.m31;
this.m32 = m1.m32 - m2.m32;
this.m33 = m1.m33 - m2.m33;
}
/// <summary>
/// Sets the value of this quaternion to the rotational component of
/// the passed matrix.
/// </summary>
/// <remarks>
/// Sets the value of this quaternion to the rotational component of
/// the passed matrix.
/// </remarks>
/// <param name="m1">the matrix4f</param>
public void Set(Matrix4f m1)
{
double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + m1.m33);
if (ww >= 0)
{
if (ww >= Eps2)
{
this.w = Math.Sqrt(ww);
ww = 0.25 / this.w;
this.x = ((m1.m21 - m1.m12) * ww);
this.y = ((m1.m02 - m1.m20) * ww);
this.z = ((m1.m10 - m1.m01) * ww);
return;
}
}
else
{
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5 * (m1.m11 + m1.m22);
if (ww >= 0)
{
if (ww >= Eps2)
{
this.x = Math.Sqrt(ww);
ww = 1.0 / (2.0 * this.x);
this.y = (m1.m10 * ww);
this.z = (m1.m20 * ww);
return;
}
}
else
{
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5 * (1.0 - m1.m22);
if (ww >= Eps2)
{
this.y = Math.Sqrt(ww);
this.z = (m1.m21) / (2.0 * this.y);
return;
}
this.y = 0;
this.z = 1;
}
/// <summary>Sets the value of this matrix to that of the Matrix4f provided.</summary>
/// <remarks>Sets the value of this matrix to that of the Matrix4f provided.</remarks>
/// <param name="m1">the matrix</param>
public void Set(Matrix4f m1)
{
if (nRow < 4 || nCol < 4)
{
values = new double[][] { new double[4], new double[4], new double[4], new double
[4] };
nRow = 4;
nCol = 4;
}
values[0][0] = m1.m00;
values[0][1] = m1.m01;
values[0][2] = m1.m02;
values[0][3] = m1.m03;
values[1][0] = m1.m10;
values[1][1] = m1.m11;
values[1][2] = m1.m12;
values[1][3] = m1.m13;
values[2][0] = m1.m20;
values[2][1] = m1.m21;
values[2][2] = m1.m22;
values[2][3] = m1.m23;
values[3][0] = m1.m30;
values[3][1] = m1.m31;
values[3][2] = m1.m32;
values[3][3] = m1.m33;
for (int i = 4; i < nRow; i++)
{
// pad rest or matrix with zeros
for (int j = 4; j < nCol; j++)
{
values[i][j] = 0.0;
}
}
}
