/// <summary>Sets the value of this matrix to that of the Matrix3f provided.</summary>
/// <remarks>Sets the value of this matrix to that of the Matrix3f provided.</remarks>
/// <param name="m1">the matrix</param>
public void Set(Matrix3f m1)
{
int i;
int j;
if (nCol < 3 || nRow < 3)
{
// expand matrix if too small
nCol = 3;
nRow = 3;
//values = new double[nRow][nCol];
values = new double[][] { new double[3], new double[3], new double[3] };
}
values[0][0] = m1.m00;
values[0][1] = m1.m01;
values[0][2] = m1.m02;
values[1][0] = m1.m10;
values[1][1] = m1.m11;
values[1][2] = m1.m12;
values[2][0] = m1.m20;
values[2][1] = m1.m21;
values[2][2] = m1.m22;
for (i = 3; i < nRow; i++)
{
// pad rest or matrix with zeros
for (j = 3; j < nCol; j++)
{
values[i][j] = 0.0;
}
}
}
/// <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, Matrix3f m1)
{
this.m00 = scalar * m1.m00;
this.m01 = scalar * m1.m01;
this.m02 = scalar * m1.m02;
this.m10 = scalar * m1.m10;
this.m11 = scalar * m1.m11;
this.m12 = scalar * m1.m12;
this.m20 = scalar * m1.m20;
this.m21 = scalar * m1.m21;
this.m22 = scalar * m1.m22;
}
/// <summary>
/// Sets the value of this matrix to the result of multiplying
/// the two argument matrices together.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the result of multiplying
/// the two argument matrices together.
/// </remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Mul(Matrix3f m1, Matrix3f m2)
{
if (this != m1 && this != m2)
{
this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
}
else
{
float m00;
float m01;
float m02;
float m10;
float m11;
float m12;
float m20;
float m21;
float m22;
m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
}
}
/// <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>
private void InvertGeneral(Matrix3f m1)
{
double[] temp = new double[9];
double[] result = new double[9];
int[] row_perm = new int[3];
int i;
int r;
int c;
// Use LU decomposition and backsubstitution code specifically
// for floating-point 3x3 matrices.
// Copy source matrix to t1tmp
temp[0] = (double)m1.m00;
temp[1] = (double)m1.m01;
temp[2] = (double)m1.m02;
temp[3] = (double)m1.m10;
temp[4] = (double)m1.m11;
temp[5] = (double)m1.m12;
temp[6] = (double)m1.m20;
temp[7] = (double)m1.m21;
temp[8] = (double)m1.m22;
// 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 < 9; i++)
{
result[i] = 0.0;
}
result[0] = 1.0;
result[4] = 1.0;
result[8] = 1.0;
LuBacksubstitution(temp, row_perm, result);
this.m00 = (float)result[0];
this.m01 = (float)result[1];
this.m02 = (float)result[2];
this.m10 = (float)result[3];
this.m11 = (float)result[4];
this.m12 = (float)result[5];
this.m20 = (float)result[6];
this.m21 = (float)result[7];
this.m22 = (float)result[8];
}
/// <summary>
/// Returns true if all of the data members of Matrix3f m1 are
/// equal to the corresponding data members in this Matrix3f.
/// </summary>
/// <remarks>
/// Returns true if all of the data members of Matrix3f m1 are
/// equal to the corresponding data members in this Matrix3f.
/// </remarks>
/// <param name="m1">the matrix with which the comparison is made</param>
/// <returns>true or false</returns>
public virtual bool Equals(Matrix3f m1)
{
try
{
return (this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02 && this.m10
== m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12 && this.m20 == m1.m20 &&
this.m21 == m1.m21 && this.m22 == m1.m22);
}
catch (ArgumentNullException)
{
return false;
}
}
/// <summary>
/// Sets the value of this matrix to the matrix difference
/// of itself and matrix m1 (this = this - m1).
/// </summary>
/// <remarks>
/// Sets the value of 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(Matrix3f m1)
{
this.m00 -= m1.m00;
this.m01 -= m1.m01;
this.m02 -= m1.m02;
this.m10 -= m1.m10;
this.m11 -= m1.m11;
this.m12 -= m1.m12;
this.m20 -= m1.m20;
this.m21 -= m1.m21;
this.m22 -= m1.m22;
}
/// <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(Matrix3f m1)
{
if (this != m1)
{
this.m00 = m1.m00;
this.m01 = m1.m10;
this.m02 = m1.m20;
this.m10 = m1.m01;
this.m11 = m1.m11;
this.m12 = m1.m21;
this.m20 = m1.m02;
this.m21 = m1.m12;
this.m22 = m1.m22;
}
else
{
this.Transpose();
}
}
/// <summary>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix values in the single precision Matrix3f argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the passed rotation components,
/// and then the scale is reapplied to the rotational components.
/// </summary>
/// <remarks>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix values in the single precision Matrix3f argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the passed rotation components,
/// and then the scale is reapplied to the rotational components.
/// </remarks>
/// <param name="m1">single precision 3x3 matrix</param>
public void SetRotation(Matrix3f m1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
m00 = (float)(m1.m00 * tmp_scale[0]);
m01 = (float)(m1.m01 * tmp_scale[1]);
m02 = (float)(m1.m02 * tmp_scale[2]);
m10 = (float)(m1.m10 * tmp_scale[0]);
m11 = (float)(m1.m11 * tmp_scale[1]);
m12 = (float)(m1.m12 * tmp_scale[2]);
m20 = (float)(m1.m20 * tmp_scale[0]);
m21 = (float)(m1.m21 * tmp_scale[1]);
m22 = (float)(m1.m22 * tmp_scale[2]);
}
/// <summary>
/// Replaces the upper 3x3 matrix values of this matrix with the
/// values in the matrix m1.
/// </summary>
/// <remarks>
/// Replaces the upper 3x3 matrix values of this matrix with the
/// values in the matrix m1.
/// </remarks>
/// <param name="m1">the matrix that will be the new upper 3x3</param>
public void SetRotationScale(Matrix3f m1)
{
m00 = m1.m00;
m01 = m1.m01;
m02 = m1.m02;
m10 = m1.m10;
m11 = m1.m11;
m12 = m1.m12;
m20 = m1.m20;
m21 = m1.m21;
m22 = m1.m22;
}
/// <summary>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix values in the single precision Matrix3f argument; the other
/// elements of this matrix are initialized as if this were an identity
/// matrix (i.e., affine matrix with no translational component).
/// </summary>
/// <remarks>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix values in the single precision Matrix3f argument; the other
/// elements of this matrix are initialized as if this were an identity
/// matrix (i.e., affine matrix with no translational component).
/// </remarks>
/// <param name="m1">the single-precision 3x3 matrix</param>
public void Set(Matrix3f m1)
{
m00 = m1.m00;
m01 = m1.m01;
m02 = m1.m02;
m03 = 0.0f;
m10 = m1.m10;
m11 = m1.m11;
m12 = m1.m12;
m13 = 0.0f;
m20 = m1.m20;
m21 = m1.m21;
m22 = m1.m22;
m23 = 0.0f;
m30 = 0.0f;
m31 = 0.0f;
m32 = 0.0f;
m33 = 1.0f;
}
/// <summary>
/// Constructs and initializes a Matrix4f from the rotation matrix,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components of the matrix.
/// </summary>
/// <remarks>
/// Constructs and initializes a Matrix4f from the rotation matrix,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components of the matrix.
/// </remarks>
/// <param name="m1">the rotation matrix representing the rotational components</param>
/// <param name="t1">the translational components of the matrix</param>
/// <param name="s">the scale value applied to the rotational components</param>
public Matrix4f(Matrix3f m1, Vector3f t1, float s)
{
this.m00 = m1.m00 * s;
this.m01 = m1.m01 * s;
this.m02 = m1.m02 * s;
this.m03 = t1.x;
this.m10 = m1.m10 * s;
this.m11 = m1.m11 * s;
this.m12 = m1.m12 * s;
this.m13 = t1.y;
this.m20 = m1.m20 * s;
this.m21 = m1.m21 * s;
this.m22 = m1.m22 * s;
this.m23 = t1.z;
this.m30 = 0.0f;
this.m31 = 0.0f;
this.m32 = 0.0f;
this.m33 = 1.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 AxisAngle4d is set to an angle of 0 about an axis of (0,1,0).
/// </remarks>
/// <param name="m1">the matrix3f</param>
public void Set(Matrix3f m1)
{
x = (float)(m1.m21 - m1.m12);
y = (float)(m1.m02 - m1.m20);
z = (float)(m1.m10 - m1.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 * (m1.m00 + m1.m11 + m1.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>
/// Places the values in the upper 3x3 of this GMatrix into
/// the matrix m1.
/// </summary>
/// <remarks>
/// Places the values in the upper 3x3 of this GMatrix into
/// the matrix m1.
/// </remarks>
/// <param name="m1">The matrix that will hold the new values</param>
public void Get(Matrix3f m1)
{
if (nRow < 3 || nCol < 3)
{
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 (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 (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];
}
}
}
}
}
}
}
else
{
m1.m00 = (float)values[0][0];
m1.m01 = (float)values[0][1];
m1.m02 = (float)values[0][2];
m1.m10 = (float)values[1][0];
m1.m11 = (float)values[1][1];
m1.m12 = (float)values[1][2];
m1.m20 = (float)values[2][0];
m1.m21 = (float)values[2][1];
m1.m22 = (float)values[2][2];
}
}
/// <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, Matrix3f m1)
{
this.m00 = m1.m00 + scalar;
this.m01 = m1.m01 + scalar;
this.m02 = m1.m02 + scalar;
this.m10 = m1.m10 + scalar;
this.m11 = m1.m11 + scalar;
this.m12 = m1.m12 + scalar;
this.m20 = m1.m20 + scalar;
this.m21 = m1.m21 + scalar;
this.m22 = m1.m22 + scalar;
}
/// <summary>
/// Performs an SVD normalization of this matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Matrix3f parameter.
/// </summary>
/// <remarks>
/// Performs an SVD normalization of this matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Matrix3f parameter.
/// </remarks>
/// <param name="m1">matrix into which the rotational component is placed</param>
public void Get(Matrix3f m1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
m1.m00 = (float)tmp_rot[0];
m1.m01 = (float)tmp_rot[1];
m1.m02 = (float)tmp_rot[2];
m1.m10 = (float)tmp_rot[3];
m1.m11 = (float)tmp_rot[4];
m1.m12 = (float)tmp_rot[5];
m1.m20 = (float)tmp_rot[6];
m1.m21 = (float)tmp_rot[7];
m1.m22 = (float)tmp_rot[8];
}
/// <summary>
/// Sets the value of this matrix to the matrix difference
/// of matrices m1 and m2.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix difference
/// of matrices m1 and m2.
/// </remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Sub(Matrix3f m1, Matrix3f m2)
{
this.m00 = m1.m00 - m2.m00;
this.m01 = m1.m01 - m2.m01;
this.m02 = m1.m02 - m2.m02;
this.m10 = m1.m10 - m2.m10;
this.m11 = m1.m11 - m2.m11;
this.m12 = m1.m12 - m2.m12;
this.m20 = m1.m20 - m2.m20;
this.m21 = m1.m21 - m2.m21;
this.m22 = m1.m22 - m2.m22;
}
/// <summary>
/// Performs an SVD normalization of this matrix to calculate
/// the rotation as a 3x3 matrix, the translation, and the scale.
/// </summary>
/// <remarks>
/// Performs an SVD normalization of this matrix to calculate
/// the rotation as a 3x3 matrix, the translation, and the scale.
/// None of the matrix values are modified.
/// </remarks>
/// <param name="m1">the normalized matrix representing the rotation</param>
/// <param name="t1">the translation component</param>
/// <returns>the scale component of this transform</returns>
public float Get(Matrix3f m1, Vector3f t1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
m1.m00 = (float)tmp_rot[0];
m1.m01 = (float)tmp_rot[1];
m1.m02 = (float)tmp_rot[2];
m1.m10 = (float)tmp_rot[3];
m1.m11 = (float)tmp_rot[4];
m1.m12 = (float)tmp_rot[5];
m1.m20 = (float)tmp_rot[6];
m1.m21 = (float)tmp_rot[7];
m1.m22 = (float)tmp_rot[8];
t1.x = m03;
t1.y = m13;
t1.z = m23;
return ((float)Matrix3d.Max3(tmp_scale));
}
/// <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(Matrix3f m1, Matrix3f m2)
{
this.m00 = m1.m00 + m2.m00;
this.m01 = m1.m01 + m2.m01;
this.m02 = m1.m02 + m2.m02;
this.m10 = m1.m10 + m2.m10;
this.m11 = m1.m11 + m2.m11;
this.m12 = m1.m12 + m2.m12;
this.m20 = m1.m20 + m2.m20;
this.m21 = m1.m21 + m2.m21;
this.m22 = m1.m22 + m2.m22;
}
/// <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 matrix3f</param>
public void Set(Matrix3f m1)
{
double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0);
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 = 0.5 / 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));
}
this.y = 0;
this.z = 1;
}
/// <summary>
/// Sets the value of this matrix to the matrix sum of itself and
/// matrix m1.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix sum of itself and
/// matrix m1.
/// </remarks>
/// <param name="m1">the other matrix</param>
public void Add(Matrix3f m1)
{
this.m00 += m1.m00;
this.m01 += m1.m01;
this.m02 += m1.m02;
this.m10 += m1.m10;
this.m11 += m1.m11;
this.m12 += m1.m12;
this.m20 += m1.m20;
this.m21 += m1.m21;
this.m22 += m1.m22;
}
/// <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(Matrix3f m1, Matrix3f m2)
{
if (this != m1 && this != m2)
{
this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
}
else
{
float m00;
float m01;
float m02;
float m10;
float m11;
float m12;
float m20;
float m21;
float m22;
// vars for temp result matrix
m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
}
}
/// <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 ; j=0,1,2 ; 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(Matrix3f 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.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.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;
}
return (status);
}
/// <summary>
/// Sets the value of this matrix equal to the negation of
/// of the Matrix3f parameter.
/// </summary>
/// <remarks>
/// Sets the value of this matrix equal to the negation of
/// of the Matrix3f parameter.
/// </remarks>
/// <param name="m1">the source matrix</param>
public void Negate(Matrix3f m1)
{
this.m00 = -m1.m00;
this.m01 = -m1.m01;
this.m02 = -m1.m02;
this.m10 = -m1.m10;
this.m11 = -m1.m11;
this.m12 = -m1.m12;
this.m20 = -m1.m20;
this.m21 = -m1.m21;
this.m22 = -m1.m22;
}
/// <summary>
/// Sets the value of this matrix to the matrix inverse
/// of the passed matrix m1.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix inverse
/// of the passed matrix m1.
/// </remarks>
/// <param name="m1">the matrix to be inverted</param>
public void Invert(Matrix3f m1)
{
InvertGeneral(m1);
}
/// <summary>
/// Perform cross product normalization of matrix m1 and place the
/// normalized values into this.
/// </summary>
/// <remarks>
/// Perform cross product normalization of matrix m1 and place the
/// normalized values into this.
/// </remarks>
/// <param name="m1">Provides the matrix values to be normalized</param>
public void NormalizeCP(Matrix3f m1)
{
float mag = 1.0f / (float)Math.Sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10 + m1.m20 *
m1.m20);
m00 = m1.m00 * mag;
m10 = m1.m10 * mag;
m20 = m1.m20 * mag;
mag = 1.0f / (float)Math.Sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11 + m1.m21 * m1.m21
);
m01 = m1.m01 * mag;
m11 = m1.m11 * mag;
m21 = m1.m21 * mag;
m02 = m10 * m21 - m11 * m20;
m12 = m01 * m20 - m00 * m21;
m22 = m00 * m11 - m01 * m10;
}
/// <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(Matrix3f m1)
{
float m00;
float m01;
float m02;
float m10;
float m11;
float m12;
float m20;
float m21;
float m22;
m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
}
/// <summary>
/// Sets the value of this matrix to the value of the Matrix3f
/// argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the value of the Matrix3f
/// argument.
/// </remarks>
/// <param name="m1">the source matrix3f</param>
public void Set(Matrix3f m1)
{
this.m00 = m1.m00;
this.m01 = m1.m01;
this.m02 = m1.m02;
this.m10 = m1.m10;
this.m11 = m1.m11;
this.m12 = m1.m12;
this.m20 = m1.m20;
this.m21 = m1.m21;
this.m22 = m1.m22;
}
/// <summary>
/// Multiplies matrix m1 by matrix m2, does an SVD normalization
/// of the result, and places the result into this matrix.
/// </summary>
/// <remarks>
/// Multiplies matrix m1 by matrix m2, does an SVD normalization
/// of the result, and places the result into this matrix.
/// this = SVDnorm(m1*m2).
/// </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 MulNormalize(Matrix3f m1, Matrix3f m2)
{
double[] tmp = new double[9];
// scratch matrix
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
tmp[0] = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
tmp[1] = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
tmp[2] = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
tmp[3] = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
tmp[4] = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
tmp[5] = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
tmp[6] = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
tmp[7] = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
tmp[8] = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
Matrix3d.Compute_svd(tmp, tmp_scale, tmp_rot);
this.m00 = (float)(tmp_rot[0]);
this.m01 = (float)(tmp_rot[1]);
this.m02 = (float)(tmp_rot[2]);
this.m10 = (float)(tmp_rot[3]);
this.m11 = (float)(tmp_rot[4]);
this.m12 = (float)(tmp_rot[5]);
this.m20 = (float)(tmp_rot[6]);
this.m21 = (float)(tmp_rot[7]);
this.m22 = (float)(tmp_rot[8]);
}
/// <summary>
/// Constructs and initializes a Matrix4d from the rotation matrix,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components of the matrix.
/// </summary>
/// <remarks>
/// Constructs and initializes a Matrix4d from the rotation matrix,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components of the matrix.
/// </remarks>
/// <param name="m1">the rotation matrix representing the rotational components</param>
/// <param name="t1">the translational components of the matrix</param>
/// <param name="s">the scale value applied to the rotational components</param>
public Matrix4d(Matrix3f m1, Vector3d t1, double s)
{
this.m00 = m1.m00 * s;
this.m01 = m1.m01 * s;
this.m02 = m1.m02 * s;
this.m03 = t1.x;
this.m10 = m1.m10 * s;
this.m11 = m1.m11 * s;
this.m12 = m1.m12 * s;
this.m13 = t1.y;
this.m20 = m1.m20 * s;
this.m21 = m1.m21 * s;
this.m22 = m1.m22 * s;
this.m23 = t1.z;
this.m30 = 0.0;
this.m31 = 0.0;
this.m32 = 0.0;
this.m33 = 1.0;
}
