|
|
@@ -0,0 +1,641 @@
|
|
|
+using System;
|
|
|
+using System.Collections.Generic;
|
|
|
+using System.Diagnostics;
|
|
|
+using System.Numerics;
|
|
|
+using System.Runtime.InteropServices;
|
|
|
+using System.Text;
|
|
|
+
|
|
|
+namespace SharpGLTF.Transforms
|
|
|
+{
|
|
|
+ // stripped from https://github.com/dotnet/runtime/blob/master/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.cs
|
|
|
+
|
|
|
+ [StructLayout(LayoutKind.Sequential)]
|
|
|
+ [DebuggerTypeProxy(typeof(Debug._Matrix4x4DoubleProxy))]
|
|
|
+ public struct Matrix4x4Double : IEquatable<Matrix4x4Double>
|
|
|
+ {
|
|
|
+ #region constants
|
|
|
+
|
|
|
+ private static readonly Matrix4x4Double _identity = new Matrix4x4Double
|
|
|
+ (
|
|
|
+ 1, 0, 0, 0,
|
|
|
+ 0, 1, 0, 0,
|
|
|
+ 0, 0, 1, 0,
|
|
|
+ 0, 0, 0, 1
|
|
|
+ );
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region constructors
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Constructs a Matrix4x4 from the given components.
|
|
|
+ /// </summary>
|
|
|
+ public Matrix4x4Double(double m11, double m12, double m13, double m14,
|
|
|
+ double m21, double m22, double m23, double m24,
|
|
|
+ double m31, double m32, double m33, double m34,
|
|
|
+ double m41, double m42, double m43, double m44)
|
|
|
+ {
|
|
|
+ this.M11 = m11;
|
|
|
+ this.M12 = m12;
|
|
|
+ this.M13 = m13;
|
|
|
+ this.M14 = m14;
|
|
|
+
|
|
|
+ this.M21 = m21;
|
|
|
+ this.M22 = m22;
|
|
|
+ this.M23 = m23;
|
|
|
+ this.M24 = m24;
|
|
|
+
|
|
|
+ this.M31 = m31;
|
|
|
+ this.M32 = m32;
|
|
|
+ this.M33 = m33;
|
|
|
+ this.M34 = m34;
|
|
|
+
|
|
|
+ this.M41 = m41;
|
|
|
+ this.M42 = m42;
|
|
|
+ this.M43 = m43;
|
|
|
+ this.M44 = m44;
|
|
|
+ }
|
|
|
+
|
|
|
+ public Matrix4x4Double(Matrix4x4 other)
|
|
|
+ {
|
|
|
+ this.M11 = other.M11;
|
|
|
+ this.M12 = other.M12;
|
|
|
+ this.M13 = other.M13;
|
|
|
+ this.M14 = other.M14;
|
|
|
+
|
|
|
+ this.M21 = other.M21;
|
|
|
+ this.M22 = other.M22;
|
|
|
+ this.M23 = other.M23;
|
|
|
+ this.M24 = other.M24;
|
|
|
+
|
|
|
+ this.M31 = other.M31;
|
|
|
+ this.M32 = other.M32;
|
|
|
+ this.M33 = other.M33;
|
|
|
+ this.M34 = other.M34;
|
|
|
+
|
|
|
+ this.M41 = other.M41;
|
|
|
+ this.M42 = other.M42;
|
|
|
+ this.M43 = other.M43;
|
|
|
+ this.M44 = other.M44;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Creates a translation matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="xPosition">The amount to translate on the X-axis.</param>
|
|
|
+ /// <param name="yPosition">The amount to translate on the Y-axis.</param>
|
|
|
+ /// <param name="zPosition">The amount to translate on the Z-axis.</param>
|
|
|
+ /// <returns>The translation matrix.</returns>
|
|
|
+ public static Matrix4x4Double CreateTranslation(double xPosition, double yPosition, double zPosition)
|
|
|
+ {
|
|
|
+ Matrix4x4Double result;
|
|
|
+
|
|
|
+ result.M11 = 1.0;
|
|
|
+ result.M12 = 0.0;
|
|
|
+ result.M13 = 0.0;
|
|
|
+ result.M14 = 0.0;
|
|
|
+ result.M21 = 0.0;
|
|
|
+ result.M22 = 1.0;
|
|
|
+ result.M23 = 0.0;
|
|
|
+ result.M24 = 0.0;
|
|
|
+ result.M31 = 0.0;
|
|
|
+ result.M32 = 0.0;
|
|
|
+ result.M33 = 1.0;
|
|
|
+ result.M34 = 0.0;
|
|
|
+
|
|
|
+ result.M41 = xPosition;
|
|
|
+ result.M42 = yPosition;
|
|
|
+ result.M43 = zPosition;
|
|
|
+ result.M44 = 1.0f;
|
|
|
+
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Creates a scaling matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="xScale">Value to scale by on the X-axis.</param>
|
|
|
+ /// <param name="yScale">Value to scale by on the Y-axis.</param>
|
|
|
+ /// <param name="zScale">Value to scale by on the Z-axis.</param>
|
|
|
+ /// <returns>The scaling matrix.</returns>
|
|
|
+ public static Matrix4x4Double CreateScale(double xScale, double yScale, double zScale)
|
|
|
+ {
|
|
|
+ Matrix4x4Double result;
|
|
|
+
|
|
|
+ result.M11 = xScale;
|
|
|
+ result.M12 = 0.0;
|
|
|
+ result.M13 = 0.0;
|
|
|
+ result.M14 = 0.0;
|
|
|
+ result.M21 = 0.0;
|
|
|
+ result.M22 = yScale;
|
|
|
+ result.M23 = 0.0;
|
|
|
+ result.M24 = 0.0;
|
|
|
+ result.M31 = 0.0;
|
|
|
+ result.M32 = 0.0;
|
|
|
+ result.M33 = zScale;
|
|
|
+ result.M34 = 0.0;
|
|
|
+ result.M41 = 0.0;
|
|
|
+ result.M42 = 0.0;
|
|
|
+ result.M43 = 0.0;
|
|
|
+ result.M44 = 1.0;
|
|
|
+
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Creates a rotation matrix from the given Quaternion rotation value.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="quaternion">The source Quaternion.</param>
|
|
|
+ /// <returns>The rotation matrix.</returns>
|
|
|
+ public static Matrix4x4Double CreateFromQuaternion(Quaternion quaternion)
|
|
|
+ {
|
|
|
+ Matrix4x4Double result;
|
|
|
+
|
|
|
+ double qX = quaternion.X;
|
|
|
+ double qY = quaternion.Y;
|
|
|
+ double qZ = quaternion.Z;
|
|
|
+ double qW = quaternion.W;
|
|
|
+
|
|
|
+ double xx = qX * qX;
|
|
|
+ double yy = qY * qY;
|
|
|
+ double zz = qZ * qZ;
|
|
|
+
|
|
|
+ double xy = qX * qY;
|
|
|
+ double wz = qZ * qW;
|
|
|
+ double xz = qZ * qX;
|
|
|
+ double wy = qY * qW;
|
|
|
+ double yz = qY * qZ;
|
|
|
+ double wx = qX * qW;
|
|
|
+
|
|
|
+ result.M11 = 1.0 - (2.0 * (yy + zz));
|
|
|
+ result.M12 = 2.0 * (xy + wz);
|
|
|
+ result.M13 = 2.0 * (xz - wy);
|
|
|
+ result.M14 = 0.0;
|
|
|
+ result.M21 = 2.0 * (xy - wz);
|
|
|
+ result.M22 = 1.0 - (2.0 * (zz + xx));
|
|
|
+ result.M23 = 2.0 * (yz + wx);
|
|
|
+ result.M24 = 0.0;
|
|
|
+ result.M31 = 2.0 * (xz + wy);
|
|
|
+ result.M32 = 2.0 * (yz - wx);
|
|
|
+ result.M33 = 1.0 - (2.0 * (yy + xx));
|
|
|
+ result.M34 = 0.0;
|
|
|
+ result.M41 = 0.0;
|
|
|
+ result.M42 = 0.0;
|
|
|
+ result.M43 = 0.0;
|
|
|
+ result.M44 = 1.0;
|
|
|
+
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Converts a <see cref="Matrix4x4Double"/> to a <see cref="Matrix4x4"/>
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="mat">The matrix to convert.</param>
|
|
|
+ /// <remarks>
|
|
|
+ /// Since converting From double precission to single precission
|
|
|
+ /// implies a loss of precission, this conversion is explicit, so
|
|
|
+ /// developers will be aware of when the precission loss is happening.
|
|
|
+ /// </remarks>
|
|
|
+ public static explicit operator Matrix4x4(Matrix4x4Double mat)
|
|
|
+ {
|
|
|
+ return new Matrix4x4
|
|
|
+ (
|
|
|
+ (float)mat.M11,
|
|
|
+ (float)mat.M12,
|
|
|
+ (float)mat.M13,
|
|
|
+ (float)mat.M14,
|
|
|
+ (float)mat.M21,
|
|
|
+ (float)mat.M22,
|
|
|
+ (float)mat.M23,
|
|
|
+ (float)mat.M24,
|
|
|
+ (float)mat.M31,
|
|
|
+ (float)mat.M32,
|
|
|
+ (float)mat.M33,
|
|
|
+ (float)mat.M34,
|
|
|
+ (float)mat.M41,
|
|
|
+ (float)mat.M42,
|
|
|
+ (float)mat.M43,
|
|
|
+ (float)mat.M44
|
|
|
+ );
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// converts from <see cref="Matrix4x4"/> to <see cref="Matrix4x4Double"/>
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="mat">The matrix to convert.</param>
|
|
|
+ public static implicit operator Matrix4x4Double(Matrix4x4 mat)
|
|
|
+ {
|
|
|
+ return new Matrix4x4Double
|
|
|
+ (
|
|
|
+ mat.M11,
|
|
|
+ mat.M12,
|
|
|
+ mat.M13,
|
|
|
+ mat.M14,
|
|
|
+ mat.M21,
|
|
|
+ mat.M22,
|
|
|
+ mat.M23,
|
|
|
+ mat.M24,
|
|
|
+ mat.M31,
|
|
|
+ mat.M32,
|
|
|
+ mat.M33,
|
|
|
+ mat.M34,
|
|
|
+ mat.M41,
|
|
|
+ mat.M42,
|
|
|
+ mat.M43,
|
|
|
+ mat.M44
|
|
|
+ );
|
|
|
+ }
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region Public Fields
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 1, column 1 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M11;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 1, column 2 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M12;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 1, column 3 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M13;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 1, column 4 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M14;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 2, column 1 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M21;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 2, column 2 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M22;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 2, column 3 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M23;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 2, column 4 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M24;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 3, column 1 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M31;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 3, column 2 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M32;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 3, column 3 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M33;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 3, column 4 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M34;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 4, column 1 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M41;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 4, column 2 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M42;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 4, column 3 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M43;
|
|
|
+ /// <summary>
|
|
|
+ /// Value at row 4, column 4 of the matrix.
|
|
|
+ /// </summary>
|
|
|
+ [DebuggerBrowsable(DebuggerBrowsableState.Never)]
|
|
|
+ public Double M44;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns the hash code for this instance.
|
|
|
+ /// </summary>
|
|
|
+ /// <returns>The hash code.</returns>
|
|
|
+ public override int GetHashCode()
|
|
|
+ {
|
|
|
+ unchecked
|
|
|
+ {
|
|
|
+ return M11.GetHashCode() + M12.GetHashCode() + M13.GetHashCode() + M14.GetHashCode() +
|
|
|
+ M21.GetHashCode() + M22.GetHashCode() + M23.GetHashCode() + M24.GetHashCode() +
|
|
|
+ M31.GetHashCode() + M32.GetHashCode() + M33.GetHashCode() + M34.GetHashCode() +
|
|
|
+ M41.GetHashCode() + M42.GetHashCode() + M43.GetHashCode() + M44.GetHashCode();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns a boolean indicating whether the given two matrices are equal.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value1">The first matrix to compare.</param>
|
|
|
+ /// <param name="value2">The second matrix to compare.</param>
|
|
|
+ /// <returns>True if the given matrices are equal; False otherwise.</returns>
|
|
|
+ public static bool operator ==(Matrix4x4Double value1, Matrix4x4Double value2)
|
|
|
+ {
|
|
|
+ return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out.
|
|
|
+ value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && value1.M21 == value2.M21 &&
|
|
|
+ value1.M23 == value2.M23 && value1.M24 == value2.M24 && value1.M31 == value2.M31 && value1.M32 == value2.M32 &&
|
|
|
+ value1.M34 == value2.M34 && value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns a boolean indicating whether the given two matrices are not equal.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value1">The first matrix to compare.</param>
|
|
|
+ /// <param name="value2">The second matrix to compare.</param>
|
|
|
+ /// <returns>True if the given matrices are not equal; False if they are equal.</returns>
|
|
|
+ public static bool operator !=(Matrix4x4Double value1, Matrix4x4Double value2)
|
|
|
+ {
|
|
|
+ return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 ||
|
|
|
+ value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 ||
|
|
|
+ value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 ||
|
|
|
+ value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns a boolean indicating whether this matrix instance is equal to the other given matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="other">The matrix to compare this instance to.</param>
|
|
|
+ /// <returns>True if the matrices are equal; False otherwise.</returns>
|
|
|
+ public bool Equals(Matrix4x4Double other) => this == other;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns a boolean indicating whether the given Object is equal to this matrix instance.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="obj">The Object to compare against.</param>
|
|
|
+ /// <returns>True if the Object is equal to this matrix; False otherwise.</returns>
|
|
|
+ public override bool Equals(object obj) => (obj is Matrix4x4Double other) && (this == other);
|
|
|
+
|
|
|
+ #endregion Public Fields
|
|
|
+
|
|
|
+ #region properties
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Returns the multiplicative identity matrix.
|
|
|
+ /// </summary>
|
|
|
+ public static Matrix4x4Double Identity
|
|
|
+ {
|
|
|
+ get { return _identity; }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Gets or sets the translation component of this matrix.
|
|
|
+ /// </summary>
|
|
|
+ public (Double x, Double y, Double z) Translation
|
|
|
+ {
|
|
|
+ get
|
|
|
+ {
|
|
|
+ return (M41, M42, M43);
|
|
|
+ }
|
|
|
+ set
|
|
|
+ {
|
|
|
+ M41 = value.x;
|
|
|
+ M42 = value.y;
|
|
|
+ M43 = value.z;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region methods
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Attempts to calculate the inverse of the given matrix. If successful, result will contain the inverted matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="matrix">The source matrix to invert.</param>
|
|
|
+ /// <param name="result">If successful, contains the inverted matrix.</param>
|
|
|
+ /// <returns>True if the source matrix could be inverted; False otherwise.</returns>
|
|
|
+ public static bool Invert(Matrix4x4Double matrix, out Matrix4x4Double result)
|
|
|
+ {
|
|
|
+ // -1
|
|
|
+ // If you have matrix M, inverse Matrix M can compute
|
|
|
+ //
|
|
|
+ // -1 1
|
|
|
+ // M = --------- A
|
|
|
+ // det(M)
|
|
|
+ //
|
|
|
+ // A is adjugate (adjoint) of M, where,
|
|
|
+ //
|
|
|
+ // T
|
|
|
+ // A = C
|
|
|
+ //
|
|
|
+ // C is Cofactor matrix of M, where,
|
|
|
+ // i + j
|
|
|
+ // C = (-1) * det(M )
|
|
|
+ // ij ij
|
|
|
+ //
|
|
|
+ // [ a b c d ]
|
|
|
+ // M = [ e f g h ]
|
|
|
+ // [ i j k l ]
|
|
|
+ // [ m n o p ]
|
|
|
+ //
|
|
|
+ // First Row
|
|
|
+ // 2 | f g h |
|
|
|
+ // C = (-1) | j k l | = + ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
|
|
|
+ // 11 | n o p |
|
|
|
+ //
|
|
|
+ // 3 | e g h |
|
|
|
+ // C = (-1) | i k l | = - ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
|
|
|
+ // 12 | m o p |
|
|
|
+ //
|
|
|
+ // 4 | e f h |
|
|
|
+ // C = (-1) | i j l | = + ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
|
|
|
+ // 13 | m n p |
|
|
|
+ //
|
|
|
+ // 5 | e f g |
|
|
|
+ // C = (-1) | i j k | = - ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
|
|
|
+ // 14 | m n o |
|
|
|
+ //
|
|
|
+ // Second Row
|
|
|
+ // 3 | b c d |
|
|
|
+ // C = (-1) | j k l | = - ( b ( kp - lo ) - c ( jp - ln ) + d ( jo - kn ) )
|
|
|
+ // 21 | n o p |
|
|
|
+ //
|
|
|
+ // 4 | a c d |
|
|
|
+ // C = (-1) | i k l | = + ( a ( kp - lo ) - c ( ip - lm ) + d ( io - km ) )
|
|
|
+ // 22 | m o p |
|
|
|
+ //
|
|
|
+ // 5 | a b d |
|
|
|
+ // C = (-1) | i j l | = - ( a ( jp - ln ) - b ( ip - lm ) + d ( in - jm ) )
|
|
|
+ // 23 | m n p |
|
|
|
+ //
|
|
|
+ // 6 | a b c |
|
|
|
+ // C = (-1) | i j k | = + ( a ( jo - kn ) - b ( io - km ) + c ( in - jm ) )
|
|
|
+ // 24 | m n o |
|
|
|
+ //
|
|
|
+ // Third Row
|
|
|
+ // 4 | b c d |
|
|
|
+ // C = (-1) | f g h | = + ( b ( gp - ho ) - c ( fp - hn ) + d ( fo - gn ) )
|
|
|
+ // 31 | n o p |
|
|
|
+ //
|
|
|
+ // 5 | a c d |
|
|
|
+ // C = (-1) | e g h | = - ( a ( gp - ho ) - c ( ep - hm ) + d ( eo - gm ) )
|
|
|
+ // 32 | m o p |
|
|
|
+ //
|
|
|
+ // 6 | a b d |
|
|
|
+ // C = (-1) | e f h | = + ( a ( fp - hn ) - b ( ep - hm ) + d ( en - fm ) )
|
|
|
+ // 33 | m n p |
|
|
|
+ //
|
|
|
+ // 7 | a b c |
|
|
|
+ // C = (-1) | e f g | = - ( a ( fo - gn ) - b ( eo - gm ) + c ( en - fm ) )
|
|
|
+ // 34 | m n o |
|
|
|
+ //
|
|
|
+ // Fourth Row
|
|
|
+ // 5 | b c d |
|
|
|
+ // C = (-1) | f g h | = - ( b ( gl - hk ) - c ( fl - hj ) + d ( fk - gj ) )
|
|
|
+ // 41 | j k l |
|
|
|
+ //
|
|
|
+ // 6 | a c d |
|
|
|
+ // C = (-1) | e g h | = + ( a ( gl - hk ) - c ( el - hi ) + d ( ek - gi ) )
|
|
|
+ // 42 | i k l |
|
|
|
+ //
|
|
|
+ // 7 | a b d |
|
|
|
+ // C = (-1) | e f h | = - ( a ( fl - hj ) - b ( el - hi ) + d ( ej - fi ) )
|
|
|
+ // 43 | i j l |
|
|
|
+ //
|
|
|
+ // 8 | a b c |
|
|
|
+ // C = (-1) | e f g | = + ( a ( fk - gj ) - b ( ek - gi ) + c ( ej - fi ) )
|
|
|
+ // 44 | i j k |
|
|
|
+ //
|
|
|
+ // Cost of operation
|
|
|
+ // 53 adds, 104 muls, and 1 div.
|
|
|
+ double a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
|
|
|
+ double e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
|
|
|
+ double i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
|
|
|
+ double m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
|
|
|
+
|
|
|
+ double kp_lo = (k * p) - (l * o);
|
|
|
+ double jp_ln = (j * p) - (l * n);
|
|
|
+ double jo_kn = (j * o) - (k * n);
|
|
|
+ double ip_lm = (i * p) - (l * m);
|
|
|
+ double io_km = (i * o) - (k * m);
|
|
|
+ double in_jm = (i * n) - (j * m);
|
|
|
+
|
|
|
+ double a11 = +(f * kp_lo - g * jp_ln + h * jo_kn);
|
|
|
+ double a12 = -(e * kp_lo - g * ip_lm + h * io_km);
|
|
|
+ double a13 = +(e * jp_ln - f * ip_lm + h * in_jm);
|
|
|
+ double a14 = -(e * jo_kn - f * io_km + g * in_jm);
|
|
|
+
|
|
|
+ double det = (a * a11) + (b * a12) + (c * a13) + (d * a14);
|
|
|
+
|
|
|
+ if (Math.Abs(det) < double.Epsilon)
|
|
|
+ {
|
|
|
+ result = new Matrix4x4Double(double.NaN, double.NaN, double.NaN, double.NaN,
|
|
|
+ double.NaN, double.NaN, double.NaN, double.NaN,
|
|
|
+ double.NaN, double.NaN, double.NaN, double.NaN,
|
|
|
+ double.NaN, double.NaN, double.NaN, double.NaN);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ double invDet = 1.0 / det;
|
|
|
+
|
|
|
+ result.M11 = a11 * invDet;
|
|
|
+ result.M21 = a12 * invDet;
|
|
|
+ result.M31 = a13 * invDet;
|
|
|
+ result.M41 = a14 * invDet;
|
|
|
+
|
|
|
+ result.M12 = -(b * kp_lo - c * jp_ln + d * jo_kn) * invDet;
|
|
|
+ result.M22 = +(a * kp_lo - c * ip_lm + d * io_km) * invDet;
|
|
|
+ result.M32 = -(a * jp_ln - b * ip_lm + d * in_jm) * invDet;
|
|
|
+ result.M42 = +(a * jo_kn - b * io_km + c * in_jm) * invDet;
|
|
|
+
|
|
|
+ double gp_ho = (g * p) - (h * o);
|
|
|
+ double fp_hn = (f * p) - (h * n);
|
|
|
+ double fo_gn = (f * o) - (g * n);
|
|
|
+ double ep_hm = (e * p) - (h * m);
|
|
|
+ double eo_gm = (e * o) - (g * m);
|
|
|
+ double en_fm = (e * n) - (f * m);
|
|
|
+
|
|
|
+ result.M13 = +(b * gp_ho - c * fp_hn + d * fo_gn) * invDet;
|
|
|
+ result.M23 = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
|
|
|
+ result.M33 = +(a * fp_hn - b * ep_hm + d * en_fm) * invDet;
|
|
|
+ result.M43 = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
|
|
|
+
|
|
|
+ double gl_hk = (g * l) - (h * k);
|
|
|
+ double fl_hj = (f * l) - (h * j);
|
|
|
+ double fk_gj = (f * k) - (g * j);
|
|
|
+ double el_hi = (e * l) - (h * i);
|
|
|
+ double ek_gi = (e * k) - (g * i);
|
|
|
+ double ej_fi = (e * j) - (f * i);
|
|
|
+
|
|
|
+ result.M14 = -(b * gl_hk - c * fl_hj + d * fk_gj) * invDet;
|
|
|
+ result.M24 = +(a * gl_hk - c * el_hi + d * ek_gi) * invDet;
|
|
|
+ result.M34 = -(a * fl_hj - b * el_hi + d * ej_fi) * invDet;
|
|
|
+ result.M44 = +(a * fk_gj - b * ek_gi + c * ej_fi) * invDet;
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Multiplies a matrix by another matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value1">The first source matrix.</param>
|
|
|
+ /// <param name="value2">The second source matrix.</param>
|
|
|
+ /// <returns>The result of the multiplication.</returns>
|
|
|
+ public static Matrix4x4Double Multiply(Matrix4x4Double value1, Matrix4x4Double value2) => value1 * value2;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Multiplies a matrix by another matrix.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value1">The first source matrix.</param>
|
|
|
+ /// <param name="value2">The second source matrix.</param>
|
|
|
+ /// <returns>The result of the multiplication.</returns>
|
|
|
+ public static Matrix4x4Double operator *(Matrix4x4Double value1, Matrix4x4Double value2)
|
|
|
+ {
|
|
|
+ Matrix4x4Double m;
|
|
|
+
|
|
|
+ // First row
|
|
|
+ m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41;
|
|
|
+ m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42;
|
|
|
+ m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43;
|
|
|
+ m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44;
|
|
|
+
|
|
|
+ // Second row
|
|
|
+ m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41;
|
|
|
+ m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42;
|
|
|
+ m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43;
|
|
|
+ m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44;
|
|
|
+
|
|
|
+ // Third row
|
|
|
+ m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41;
|
|
|
+ m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42;
|
|
|
+ m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43;
|
|
|
+ m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44;
|
|
|
+
|
|
|
+ // Fourth row
|
|
|
+ m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41;
|
|
|
+ m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42;
|
|
|
+ m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43;
|
|
|
+ m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44;
|
|
|
+
|
|
|
+ return m;
|
|
|
+ }
|
|
|
+
|
|
|
+ #endregion
|
|
|
+ }
|
|
|
+}
|
Vicente Penades