private AdaptiveDeterminant ( int &subMatrixCode ) : float | ||
subMatrixCode | int | Represents the submatrix that was used to compute the determinant. /// 0 is the full 3x3. 1 is the upper left 2x2. 2 is the lower right 2x2. 3 is the four corners. /// 4 is M11. 5 is M22. 6 is M33. |
return | float |
/// <summary> /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements. /// </summary> /// <param name="matrix">Matrix to be inverted.</param> /// <param name="result">Inverted matrix.</param> public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result) { // Perform full Gauss-invert and return if successful if (Invert(ref matrix, out result)) { return; } int submatrix; Fix64 determinantInverse = F64.C1 / matrix.AdaptiveDeterminant(out submatrix); Fix64 m11, m12, m13, m21, m22, m23, m31, m32, m33; switch (submatrix) { case 1: //Upper left matrix, m11, m12, m21, m22. m11 = matrix.M22 * determinantInverse; m12 = -matrix.M12 * determinantInverse; m13 = F64.C0; m21 = -matrix.M21 * determinantInverse; m22 = matrix.M11 * determinantInverse; m23 = F64.C0; m31 = F64.C0; m32 = F64.C0; m33 = F64.C0; break; case 2: //Lower right matrix, m22, m23, m32, m33. m11 = F64.C0; m12 = F64.C0; m13 = F64.C0; m21 = F64.C0; m22 = matrix.M33 * determinantInverse; m23 = -matrix.M23 * determinantInverse; m31 = F64.C0; m32 = -matrix.M32 * determinantInverse; m33 = matrix.M22 * determinantInverse; break; case 3: //Corners, m11, m31, m13, m33. m11 = matrix.M33 * determinantInverse; m12 = F64.C0; m13 = -matrix.M13 * determinantInverse; m21 = F64.C0; m22 = F64.C0; m23 = F64.C0; m31 = -matrix.M31 * determinantInverse; m32 = F64.C0; m33 = matrix.M11 * determinantInverse; break; case 4: //M11 m11 = F64.C1 / matrix.M11; m12 = F64.C0; m13 = F64.C0; m21 = F64.C0; m22 = F64.C0; m23 = F64.C0; m31 = F64.C0; m32 = F64.C0; m33 = F64.C0; break; case 5: //M22 m11 = F64.C0; m12 = F64.C0; m13 = F64.C0; m21 = F64.C0; m22 = F64.C1 / matrix.M22; m23 = F64.C0; m31 = F64.C0; m32 = F64.C0; m33 = F64.C0; break; case 6: //M33 m11 = F64.C0; m12 = F64.C0; m13 = F64.C0; m21 = F64.C0; m22 = F64.C0; m23 = F64.C0; m31 = F64.C0; m32 = F64.C0; m33 = F64.C1 / matrix.M33; break; default: //Completely singular. m11 = F64.C0; m12 = F64.C0; m13 = F64.C0; m21 = F64.C0; m22 = F64.C0; m23 = F64.C0; m31 = F64.C0; m32 = F64.C0; m33 = F64.C0; break; } result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; }
/// <summary> /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements. /// </summary> /// <param name="matrix">Matrix to be inverted.</param> /// <param name="result">Inverted matrix.</param> public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result) { int submatrix; float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix); float m11, m12, m13, m21, m22, m23, m31, m32, m33; switch (submatrix) { case 0: //Full matrix. m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse; m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse; m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse; m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse; m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse; m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse; m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse; m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse; m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse; break; case 1: //Upper left matrix, m11, m12, m21, m22. m11 = matrix.M22 * determinantInverse; m12 = -matrix.M12 * determinantInverse; m13 = 0; m21 = -matrix.M21 * determinantInverse; m22 = matrix.M11 * determinantInverse; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 2: //Lower right matrix, m22, m23, m32, m33. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = matrix.M33 * determinantInverse; m23 = -matrix.M23 * determinantInverse; m31 = 0; m32 = -matrix.M32 * determinantInverse; m33 = matrix.M22 * determinantInverse; break; case 3: //Corners, m11, m31, m13, m33. m11 = matrix.M33 * determinantInverse; m12 = 0; m13 = -matrix.M13 * determinantInverse; m21 = 0; m22 = 0; m23 = 0; m31 = -matrix.M31 * determinantInverse; m32 = 0; m33 = matrix.M11 * determinantInverse; break; case 4: //M11 m11 = 1 / matrix.M11; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 5: //M22 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 1 / matrix.M22; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 6: //M33 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 1 / matrix.M33; break; default: //Completely singular. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; } result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; }
/// <summary> /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements. /// </summary> /// <param name="matrix">Matrix to be inverted.</param> /// <param name="result">Inverted matrix.</param> public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result) { int submatrix; float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix); float m11, m12, m13, m21, m22, m23, m31, m32, m33; switch (submatrix) { case 0: //Full matrix. m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse; m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse; m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse; m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse; m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse; m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse; m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse; m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse; m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse; break; case 1: //Upper left matrix, m11, m12, m21, m22. m11 = matrix.M22 * determinantInverse; m12 = -matrix.M12 * determinantInverse; m13 = 0; m21 = -matrix.M21 * determinantInverse; m22 = matrix.M11 * determinantInverse; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 2: //Lower right matrix, m22, m23, m32, m33. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = matrix.M33 * determinantInverse; m23 = -matrix.M23 * determinantInverse; m31 = 0; m32 = -matrix.M32 * determinantInverse; m33 = matrix.M22 * determinantInverse; break; case 3: //Corners, m11, m31, m13, m33. m11 = matrix.M33 * determinantInverse; m12 = 0; m13 = -matrix.M13 * determinantInverse; m21 = 0; m22 = 0; m23 = 0; m31 = -matrix.M31 * determinantInverse; m32 = 0; m33 = matrix.M11 * determinantInverse; break; case 4: //M11 m11 = 1 / matrix.M11; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 5: //M22 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 1 / matrix.M22; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 6: //M33 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 1 / matrix.M33; break; default: //Completely singular. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; } result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; }
/// <summary> /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M1.Z or M3.X, and excluding 1x1's that include nondiagonal elements. /// </summary> /// <param name="matrix">Matrix to be inverted.</param> /// <param name="result">Inverted matrix.</param> public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result) { int submatrix; float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix); float m11, m12, m13, m21, m22, m23, m31, m32, m33; switch (submatrix) { case 0: //Full matrix. m11 = (matrix.M2.Y * matrix.M3.Z - matrix.M2.Z * matrix.M3.Y) * determinantInverse; m12 = (matrix.M1.Z * matrix.M3.Y - matrix.M3.Z * matrix.M1.Y) * determinantInverse; m13 = (matrix.M1.Y * matrix.M2.Z - matrix.M2.Y * matrix.M1.Z) * determinantInverse; m21 = (matrix.M2.Z * matrix.M3.X - matrix.M2.X * matrix.M3.Z) * determinantInverse; m22 = (matrix.M1.X * matrix.M3.Z - matrix.M1.Z * matrix.M3.X) * determinantInverse; m23 = (matrix.M1.Z * matrix.M2.X - matrix.M1.X * matrix.M2.Z) * determinantInverse; m31 = (matrix.M2.X * matrix.M3.Y - matrix.M2.Y * matrix.M3.X) * determinantInverse; m32 = (matrix.M1.Y * matrix.M3.X - matrix.M1.X * matrix.M3.Y) * determinantInverse; m33 = (matrix.M1.X * matrix.M2.Y - matrix.M1.Y * matrix.M2.X) * determinantInverse; break; case 1: //Upper left matrix, m11, m12, m21, m22. m11 = matrix.M2.Y * determinantInverse; m12 = -matrix.M1.Y * determinantInverse; m13 = 0; m21 = -matrix.M2.X * determinantInverse; m22 = matrix.M1.X * determinantInverse; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 2: //Lower right matrix, m22, m23, m32, m33. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = matrix.M3.Z * determinantInverse; m23 = -matrix.M2.Z * determinantInverse; m31 = 0; m32 = -matrix.M3.Y * determinantInverse; m33 = matrix.M2.Y * determinantInverse; break; case 3: //Corners, m11, m31, m13, m33. m11 = matrix.M3.Z * determinantInverse; m12 = 0; m13 = -matrix.M1.Z * determinantInverse; m21 = 0; m22 = 0; m23 = 0; m31 = -matrix.M3.X * determinantInverse; m32 = 0; m33 = matrix.M1.X * determinantInverse; break; case 4: //M1.X m11 = 1 / matrix.M1.X; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 5: //M2.Y m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 1 / matrix.M2.Y; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 6: //M3.Z m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 1 / matrix.M3.Z; break; default: //Completely singular. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; } result.M1.X = m11; result.M1.Y = m12; result.M1.Z = m13; result.M2.X = m21; result.M2.Y = m22; result.M2.Z = m23; result.M3.X = m31; result.M3.Y = m32; result.M3.Z = m33; }