/// <summary>
/// The Inverse of the matrix copied into mInv.
/// Returns false if the matrix has no inverse.
/// A matrix multipled by its inverse is the idenity.
/// Invert a 3x3 using cofactors. This is about 8 times faster than
/// the Numerical Recipes code which uses Gaussian elimination.
/// </summary>
public bool TryInverse(out Matrix3x3d mInv)
{
mInv.m00 = m11 * m22 - m12 * m21;
mInv.m01 = m02 * m21 - m01 * m22;
mInv.m02 = m01 * m12 - m02 * m11;
mInv.m10 = m12 * m20 - m10 * m22;
mInv.m11 = m00 * m22 - m02 * m20;
mInv.m12 = m02 * m10 - m00 * m12;
mInv.m20 = m10 * m21 - m11 * m20;
mInv.m21 = m01 * m20 - m00 * m21;
mInv.m22 = m00 * m11 - m01 * m10;
double det = m00 * mInv.m00 + m01 * mInv.m10 + m02 * mInv.m20;
if (DMath.IsZero(det))
{
mInv = Identity;
return(false);
}
double invDet = 1.0 / det;
mInv.m00 *= invDet; mInv.m01 *= invDet; mInv.m02 *= invDet;
mInv.m10 *= invDet; mInv.m11 *= invDet; mInv.m12 *= invDet;
mInv.m20 *= invDet; mInv.m21 *= invDet; mInv.m22 *= invDet;
return(true);
}
/// <summary>
/// Convert to a 3 dimension matrix.
/// </summary>
public Matrix3x3d ToMatrix3x3d()
{
Matrix3x3d mat = new Matrix3x3d();
mat.m00 = m00; mat.m01 = m01; mat.m02 = m02;
mat.m10 = m10; mat.m11 = m11; mat.m12 = m12;
mat.m20 = m20; mat.m21 = m21; mat.m22 = m22;
return(mat);
}
/// <summary>
/// Are these matrices equal.
/// </summary>
public override bool Equals(object obj)
{
if (!(obj is Matrix3x3d))
{
return(false);
}
Matrix3x3d mat = (Matrix3x3d)obj;
return(this == mat);
}
/// <summary>
/// Subtract two matrices.
/// </summary>
public static Matrix3x3d operator -(Matrix3x3d m1, Matrix3x3d m2)
{
Matrix3x3d kSum = new Matrix3x3d();
kSum.m00 = m1.m00 - m2.m00;
kSum.m01 = m1.m01 - m2.m01;
kSum.m02 = m1.m02 - m2.m02;
kSum.m10 = m1.m10 - m2.m10;
kSum.m11 = m1.m11 - m2.m11;
kSum.m12 = m1.m12 - m2.m12;
kSum.m20 = m1.m20 - m2.m20;
kSum.m21 = m1.m21 - m2.m21;
kSum.m22 = m1.m22 - m2.m22;
return(kSum);
}
/// <summary>
/// Multiply a matrix by a scalar.
/// </summary>
public static Matrix3x3d operator *(double s, Matrix3x3d m1)
{
Matrix3x3d kProd = new Matrix3x3d();
kProd.m00 = m1.m00 * s;
kProd.m01 = m1.m01 * s;
kProd.m02 = m1.m02 * s;
kProd.m10 = m1.m10 * s;
kProd.m11 = m1.m11 * s;
kProd.m12 = m1.m12 * s;
kProd.m20 = m1.m20 * s;
kProd.m21 = m1.m21 * s;
kProd.m22 = m1.m22 * s;
return(kProd);
}
/// <summary>
/// Multiply two matrices.
/// </summary>
public static Matrix3x3d operator *(Matrix3x3d m1, Matrix3x3d m2)
{
Matrix3x3d kProd = new Matrix3x3d();
kProd.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
kProd.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
kProd.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
kProd.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
kProd.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
kProd.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
kProd.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
kProd.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
kProd.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
return(kProd);
}
/// <summary>
/// Add two matrices.
/// </summary>
public static Matrix3x3d operator +(Matrix3x3d m1, Matrix3x3d m2)
{
Matrix3x3d kSum = new Matrix3x3d();
kSum.m00 = m1.m00 + m2.m00;
kSum.m01 = m1.m01 + m2.m01;
kSum.m02 = m1.m02 + m2.m02;
kSum.m10 = m1.m10 + m2.m10;
kSum.m11 = m1.m11 + m2.m11;
kSum.m12 = m1.m12 + m2.m12;
kSum.m20 = m1.m20 + m2.m20;
kSum.m21 = m1.m21 + m2.m21;
kSum.m22 = m1.m22 + m2.m22;
return(kSum);
}
/// <summary>
/// Are these matrices equal.
/// </summary>
public bool EqualsWithError(Matrix3x3d m, double eps)
{
if (Math.Abs(m00 - m.m00) > eps)
{
return(false);
}
if (Math.Abs(m10 - m.m10) > eps)
{
return(false);
}
if (Math.Abs(m20 - m.m20) > eps)
{
return(false);
}
if (Math.Abs(m01 - m.m01) > eps)
{
return(false);
}
if (Math.Abs(m11 - m.m11) > eps)
{
return(false);
}
if (Math.Abs(m21 - m.m21) > eps)
{
return(false);
}
if (Math.Abs(m02 - m.m02) > eps)
{
return(false);
}
if (Math.Abs(m12 - m.m12) > eps)
{
return(false);
}
if (Math.Abs(m22 - m.m22) > eps)
{
return(false);
}
return(true);
}
/// <summary>
/// Are these matrices equal.
/// </summary>
public bool Equals(Matrix3x3d mat)
{
return(this == mat);
}
