--- /dev/null
+// Licensed to the .NET Foundation under one or more agreements.
+// The .NET Foundation licenses this file to you under the MIT license.
+// See the LICENSE file in the project root for more information.
+
+using System.Globalization;
+
+namespace System.Numerics
+{
+ /// <summary>
+ /// A structure encapsulating a 3x2 matrix.
+ /// </summary>
+ public struct Matrix3x2 : IEquatable<Matrix3x2>
+ {
+ private const float RotationEpsilon = 0.001f * MathF.PI / 180f; // 0.1% of a degree
+
+ #region Public Fields
+ /// <summary>
+ /// The first element of the first row
+ /// </summary>
+ public float M11;
+ /// <summary>
+ /// The second element of the first row
+ /// </summary>
+ public float M12;
+ /// <summary>
+ /// The first element of the second row
+ /// </summary>
+ public float M21;
+ /// <summary>
+ /// The second element of the second row
+ /// </summary>
+ public float M22;
+ /// <summary>
+ /// The first element of the third row
+ /// </summary>
+ public float M31;
+ /// <summary>
+ /// The second element of the third row
+ /// </summary>
+ public float M32;
+ #endregion Public Fields
+
+ private static readonly Matrix3x2 _identity = new Matrix3x2
+ (
+ 1f, 0f,
+ 0f, 1f,
+ 0f, 0f
+ );
+
+ /// <summary>
+ /// Returns the multiplicative identity matrix.
+ /// </summary>
+ public static Matrix3x2 Identity
+ {
+ get { return _identity; }
+ }
+
+ /// <summary>
+ /// Returns whether the matrix is the identity matrix.
+ /// </summary>
+ public readonly bool IsIdentity
+ {
+ get
+ {
+ return M11 == 1f && M22 == 1f && // Check diagonal element first for early out.
+ M12 == 0f &&
+ M21 == 0f &&
+ M31 == 0f && M32 == 0f;
+ }
+ }
+
+ /// <summary>
+ /// Gets or sets the translation component of this matrix.
+ /// </summary>
+ public Vector2 Translation
+ {
+ readonly get
+ {
+ return new Vector2(M31, M32);
+ }
+
+ set
+ {
+ M31 = value.X;
+ M32 = value.Y;
+ }
+ }
+
+ /// <summary>
+ /// Constructs a Matrix3x2 from the given components.
+ /// </summary>
+ public Matrix3x2(float m11, float m12,
+ float m21, float m22,
+ float m31, float m32)
+ {
+ this.M11 = m11;
+ this.M12 = m12;
+ this.M21 = m21;
+ this.M22 = m22;
+ this.M31 = m31;
+ this.M32 = m32;
+ }
+
+ /// <summary>
+ /// Creates a translation matrix from the given vector.
+ /// </summary>
+ /// <param name="position">The translation position.</param>
+ /// <returns>A translation matrix.</returns>
+ public static Matrix3x2 CreateTranslation(Vector2 position)
+ {
+ Matrix3x2 result;
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+
+ result.M31 = position.X;
+ result.M32 = position.Y;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a translation matrix from the given X and Y components.
+ /// </summary>
+ /// <param name="xPosition">The X position.</param>
+ /// <param name="yPosition">The Y position.</param>
+ /// <returns>A translation matrix.</returns>
+ public static Matrix3x2 CreateTranslation(float xPosition, float yPosition)
+ {
+ Matrix3x2 result;
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+
+ result.M31 = xPosition;
+ result.M32 = yPosition;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix from the given X and Y components.
+ /// </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>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(float xScale, float yScale)
+ {
+ Matrix3x2 result;
+
+ result.M11 = xScale;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = yScale;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix that is offset by a given center point.
+ /// </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="centerPoint">The center point.</param>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(float xScale, float yScale, Vector2 centerPoint)
+ {
+ Matrix3x2 result;
+
+ float tx = centerPoint.X * (1 - xScale);
+ float ty = centerPoint.Y * (1 - yScale);
+
+ result.M11 = xScale;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = yScale;
+ result.M31 = tx;
+ result.M32 = ty;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix from the given vector scale.
+ /// </summary>
+ /// <param name="scales">The scale to use.</param>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(Vector2 scales)
+ {
+ Matrix3x2 result;
+
+ result.M11 = scales.X;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scales.Y;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix from the given vector scale with an offset from the given center point.
+ /// </summary>
+ /// <param name="scales">The scale to use.</param>
+ /// <param name="centerPoint">The center offset.</param>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(Vector2 scales, Vector2 centerPoint)
+ {
+ Matrix3x2 result;
+
+ float tx = centerPoint.X * (1 - scales.X);
+ float ty = centerPoint.Y * (1 - scales.Y);
+
+ result.M11 = scales.X;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scales.Y;
+ result.M31 = tx;
+ result.M32 = ty;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix that scales uniformly with the given scale.
+ /// </summary>
+ /// <param name="scale">The uniform scale to use.</param>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(float scale)
+ {
+ Matrix3x2 result;
+
+ result.M11 = scale;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scale;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scale matrix that scales uniformly with the given scale with an offset from the given center.
+ /// </summary>
+ /// <param name="scale">The uniform scale to use.</param>
+ /// <param name="centerPoint">The center offset.</param>
+ /// <returns>A scaling matrix.</returns>
+ public static Matrix3x2 CreateScale(float scale, Vector2 centerPoint)
+ {
+ Matrix3x2 result;
+
+ float tx = centerPoint.X * (1 - scale);
+ float ty = centerPoint.Y * (1 - scale);
+
+ result.M11 = scale;
+ result.M12 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scale;
+ result.M31 = tx;
+ result.M32 = ty;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a skew matrix from the given angles in radians.
+ /// </summary>
+ /// <param name="radiansX">The X angle, in radians.</param>
+ /// <param name="radiansY">The Y angle, in radians.</param>
+ /// <returns>A skew matrix.</returns>
+ public static Matrix3x2 CreateSkew(float radiansX, float radiansY)
+ {
+ Matrix3x2 result;
+
+ float xTan = MathF.Tan(radiansX);
+ float yTan = MathF.Tan(radiansY);
+
+ result.M11 = 1.0f;
+ result.M12 = yTan;
+ result.M21 = xTan;
+ result.M22 = 1.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a skew matrix from the given angles in radians and a center point.
+ /// </summary>
+ /// <param name="radiansX">The X angle, in radians.</param>
+ /// <param name="radiansY">The Y angle, in radians.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>A skew matrix.</returns>
+ public static Matrix3x2 CreateSkew(float radiansX, float radiansY, Vector2 centerPoint)
+ {
+ Matrix3x2 result;
+
+ float xTan = MathF.Tan(radiansX);
+ float yTan = MathF.Tan(radiansY);
+
+ float tx = -centerPoint.Y * xTan;
+ float ty = -centerPoint.X * yTan;
+
+ result.M11 = 1.0f;
+ result.M12 = yTan;
+ result.M21 = xTan;
+ result.M22 = 1.0f;
+ result.M31 = tx;
+ result.M32 = ty;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a rotation matrix using the given rotation in radians.
+ /// </summary>
+ /// <param name="radians">The amount of rotation, in radians.</param>
+ /// <returns>A rotation matrix.</returns>
+ public static Matrix3x2 CreateRotation(float radians)
+ {
+ Matrix3x2 result;
+
+ radians = MathF.IEEERemainder(radians, MathF.PI * 2);
+
+ float c, s;
+
+ if (radians > -RotationEpsilon && radians < RotationEpsilon)
+ {
+ // Exact case for zero rotation.
+ c = 1;
+ s = 0;
+ }
+ else if (radians > MathF.PI / 2 - RotationEpsilon && radians < MathF.PI / 2 + RotationEpsilon)
+ {
+ // Exact case for 90 degree rotation.
+ c = 0;
+ s = 1;
+ }
+ else if (radians < -MathF.PI + RotationEpsilon || radians > MathF.PI - RotationEpsilon)
+ {
+ // Exact case for 180 degree rotation.
+ c = -1;
+ s = 0;
+ }
+ else if (radians > -MathF.PI / 2 - RotationEpsilon && radians < -MathF.PI / 2 + RotationEpsilon)
+ {
+ // Exact case for 270 degree rotation.
+ c = 0;
+ s = -1;
+ }
+ else
+ {
+ // Arbitrary rotation.
+ c = MathF.Cos(radians);
+ s = MathF.Sin(radians);
+ }
+
+ // [ c s ]
+ // [ -s c ]
+ // [ 0 0 ]
+ result.M11 = c;
+ result.M12 = s;
+ result.M21 = -s;
+ result.M22 = c;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a rotation matrix using the given rotation in radians and a center point.
+ /// </summary>
+ /// <param name="radians">The amount of rotation, in radians.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>A rotation matrix.</returns>
+ public static Matrix3x2 CreateRotation(float radians, Vector2 centerPoint)
+ {
+ Matrix3x2 result;
+
+ radians = MathF.IEEERemainder(radians, MathF.PI * 2);
+
+ float c, s;
+
+ if (radians > -RotationEpsilon && radians < RotationEpsilon)
+ {
+ // Exact case for zero rotation.
+ c = 1;
+ s = 0;
+ }
+ else if (radians > MathF.PI / 2 - RotationEpsilon && radians < MathF.PI / 2 + RotationEpsilon)
+ {
+ // Exact case for 90 degree rotation.
+ c = 0;
+ s = 1;
+ }
+ else if (radians < -MathF.PI + RotationEpsilon || radians > MathF.PI - RotationEpsilon)
+ {
+ // Exact case for 180 degree rotation.
+ c = -1;
+ s = 0;
+ }
+ else if (radians > -MathF.PI / 2 - RotationEpsilon && radians < -MathF.PI / 2 + RotationEpsilon)
+ {
+ // Exact case for 270 degree rotation.
+ c = 0;
+ s = -1;
+ }
+ else
+ {
+ // Arbitrary rotation.
+ c = MathF.Cos(radians);
+ s = MathF.Sin(radians);
+ }
+
+ float x = centerPoint.X * (1 - c) + centerPoint.Y * s;
+ float y = centerPoint.Y * (1 - c) - centerPoint.X * s;
+
+ // [ c s ]
+ // [ -s c ]
+ // [ x y ]
+ result.M11 = c;
+ result.M12 = s;
+ result.M21 = -s;
+ result.M22 = c;
+ result.M31 = x;
+ result.M32 = y;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Calculates the determinant for this matrix.
+ /// The determinant is calculated by expanding the matrix with a third column whose values are (0,0,1).
+ /// </summary>
+ /// <returns>The determinant.</returns>
+ public readonly float GetDeterminant()
+ {
+ // There isn't actually any such thing as a determinant for a non-square matrix,
+ // but this 3x2 type is really just an optimization of a 3x3 where we happen to
+ // know the rightmost column is always (0, 0, 1). So we expand to 3x3 format:
+ //
+ // [ M11, M12, 0 ]
+ // [ M21, M22, 0 ]
+ // [ M31, M32, 1 ]
+ //
+ // Sum the diagonal products:
+ // (M11 * M22 * 1) + (M12 * 0 * M31) + (0 * M21 * M32)
+ //
+ // Subtract the opposite diagonal products:
+ // (M31 * M22 * 0) + (M32 * 0 * M11) + (1 * M21 * M12)
+ //
+ // Collapse out the constants and oh look, this is just a 2x2 determinant!
+
+ return (M11 * M22) - (M21 * M12);
+ }
+
+ /// <summary>
+ /// Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter.
+ /// </summary>
+ /// <param name="matrix">The source matrix.</param>
+ /// <param name="result">The output matrix.</param>
+ /// <returns>True if the operation succeeded, False otherwise.</returns>
+ public static bool Invert(Matrix3x2 matrix, out Matrix3x2 result)
+ {
+ float det = (matrix.M11 * matrix.M22) - (matrix.M21 * matrix.M12);
+
+ if (MathF.Abs(det) < float.Epsilon)
+ {
+ result = new Matrix3x2(float.NaN, float.NaN, float.NaN, float.NaN, float.NaN, float.NaN);
+ return false;
+ }
+
+ float invDet = 1.0f / det;
+
+ result.M11 = matrix.M22 * invDet;
+ result.M12 = -matrix.M12 * invDet;
+ result.M21 = -matrix.M21 * invDet;
+ result.M22 = matrix.M11 * invDet;
+ result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * invDet;
+ result.M32 = (matrix.M31 * matrix.M12 - matrix.M11 * matrix.M32) * invDet;
+
+ return true;
+ }
+
+ /// <summary>
+ /// Linearly interpolates from matrix1 to matrix2, based on the third parameter.
+ /// </summary>
+ /// <param name="matrix1">The first source matrix.</param>
+ /// <param name="matrix2">The second source matrix.</param>
+ /// <param name="amount">The relative weighting of matrix2.</param>
+ /// <returns>The interpolated matrix.</returns>
+ public static Matrix3x2 Lerp(Matrix3x2 matrix1, Matrix3x2 matrix2, float amount)
+ {
+ Matrix3x2 result;
+
+ // First row
+ result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount;
+ result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount;
+
+ // Second row
+ result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount;
+ result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount;
+
+ // Third row
+ result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount;
+ result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Negates the given matrix by multiplying all values by -1.
+ /// </summary>
+ /// <param name="value">The source matrix.</param>
+ /// <returns>The negated matrix.</returns>
+ public static Matrix3x2 Negate(Matrix3x2 value)
+ {
+ Matrix3x2 result;
+
+ result.M11 = -value.M11;
+ result.M12 = -value.M12;
+ result.M21 = -value.M21;
+ result.M22 = -value.M22;
+ result.M31 = -value.M31;
+ result.M32 = -value.M32;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Adds each matrix element in value1 with its corresponding element in value2.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The matrix containing the summed values.</returns>
+ public static Matrix3x2 Add(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 result;
+
+ result.M11 = value1.M11 + value2.M11;
+ result.M12 = value1.M12 + value2.M12;
+ result.M21 = value1.M21 + value2.M21;
+ result.M22 = value1.M22 + value2.M22;
+ result.M31 = value1.M31 + value2.M31;
+ result.M32 = value1.M32 + value2.M32;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Subtracts each matrix element in value2 from its corresponding element in value1.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The matrix containing the resulting values.</returns>
+ public static Matrix3x2 Subtract(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 result;
+
+ result.M11 = value1.M11 - value2.M11;
+ result.M12 = value1.M12 - value2.M12;
+ result.M21 = value1.M21 - value2.M21;
+ result.M22 = value1.M22 - value2.M22;
+ result.M31 = value1.M31 - value2.M31;
+ result.M32 = value1.M32 - value2.M32;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Multiplies two matrices together and returns the resulting matrix.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The product matrix.</returns>
+ public static Matrix3x2 Multiply(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 result;
+
+ // First row
+ result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21;
+ result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22;
+
+ // Second row
+ result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21;
+ result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22;
+
+ // Third row
+ result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value2.M31;
+ result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value2.M32;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Scales all elements in a matrix by the given scalar factor.
+ /// </summary>
+ /// <param name="value1">The source matrix.</param>
+ /// <param name="value2">The scaling value to use.</param>
+ /// <returns>The resulting matrix.</returns>
+ public static Matrix3x2 Multiply(Matrix3x2 value1, float value2)
+ {
+ Matrix3x2 result;
+
+ result.M11 = value1.M11 * value2;
+ result.M12 = value1.M12 * value2;
+ result.M21 = value1.M21 * value2;
+ result.M22 = value1.M22 * value2;
+ result.M31 = value1.M31 * value2;
+ result.M32 = value1.M32 * value2;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Negates the given matrix by multiplying all values by -1.
+ /// </summary>
+ /// <param name="value">The source matrix.</param>
+ /// <returns>The negated matrix.</returns>
+ public static Matrix3x2 operator -(Matrix3x2 value)
+ {
+ Matrix3x2 m;
+
+ m.M11 = -value.M11;
+ m.M12 = -value.M12;
+ m.M21 = -value.M21;
+ m.M22 = -value.M22;
+ m.M31 = -value.M31;
+ m.M32 = -value.M32;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Adds each matrix element in value1 with its corresponding element in value2.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The matrix containing the summed values.</returns>
+ public static Matrix3x2 operator +(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 m;
+
+ m.M11 = value1.M11 + value2.M11;
+ m.M12 = value1.M12 + value2.M12;
+ m.M21 = value1.M21 + value2.M21;
+ m.M22 = value1.M22 + value2.M22;
+ m.M31 = value1.M31 + value2.M31;
+ m.M32 = value1.M32 + value2.M32;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Subtracts each matrix element in value2 from its corresponding element in value1.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The matrix containing the resulting values.</returns>
+ public static Matrix3x2 operator -(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 m;
+
+ m.M11 = value1.M11 - value2.M11;
+ m.M12 = value1.M12 - value2.M12;
+ m.M21 = value1.M21 - value2.M21;
+ m.M22 = value1.M22 - value2.M22;
+ m.M31 = value1.M31 - value2.M31;
+ m.M32 = value1.M32 - value2.M32;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Multiplies two matrices together and returns the resulting matrix.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The product matrix.</returns>
+ public static Matrix3x2 operator *(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ Matrix3x2 m;
+
+ // First row
+ m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21;
+ m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22;
+
+ // Second row
+ m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21;
+ m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22;
+
+ // Third row
+ m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value2.M31;
+ m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value2.M32;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Scales all elements in a matrix by the given scalar factor.
+ /// </summary>
+ /// <param name="value1">The source matrix.</param>
+ /// <param name="value2">The scaling value to use.</param>
+ /// <returns>The resulting matrix.</returns>
+ public static Matrix3x2 operator *(Matrix3x2 value1, float value2)
+ {
+ Matrix3x2 m;
+
+ m.M11 = value1.M11 * value2;
+ m.M12 = value1.M12 * value2;
+ m.M21 = value1.M21 * value2;
+ m.M22 = value1.M22 * value2;
+ m.M31 = value1.M31 * value2;
+ m.M32 = value1.M32 * value2;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the given matrices are equal.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>True if the matrices are equal; False otherwise.</returns>
+ public static bool operator ==(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && // Check diagonal element first for early out.
+ value1.M12 == value2.M12 &&
+ value1.M21 == value2.M21 &&
+ value1.M31 == value2.M31 && value1.M32 == value2.M32);
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the given matrices are not equal.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>True if the matrices are not equal; False if they are equal.</returns>
+ public static bool operator !=(Matrix3x2 value1, Matrix3x2 value2)
+ {
+ return (value1.M11 != value2.M11 || value1.M12 != value2.M12 ||
+ value1.M21 != value2.M21 || value1.M22 != value2.M22 ||
+ value1.M31 != value2.M31 || value1.M32 != value2.M32);
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the matrix is equal to the other given matrix.
+ /// </summary>
+ /// <param name="other">The other matrix to test equality against.</param>
+ /// <returns>True if this matrix is equal to other; False otherwise.</returns>
+ public readonly bool Equals(Matrix3x2 other)
+ {
+ return (M11 == other.M11 && M22 == other.M22 && // Check diagonal element first for early out.
+ M12 == other.M12 &&
+ M21 == other.M21 &&
+ M31 == other.M31 && M32 == other.M32);
+ }
+
+ /// <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 readonly bool Equals(object? obj)
+ {
+ if (obj is Matrix3x2)
+ {
+ return Equals((Matrix3x2)obj);
+ }
+
+ return false;
+ }
+
+ /// <summary>
+ /// Returns a String representing this matrix instance.
+ /// </summary>
+ /// <returns>The string representation.</returns>
+ public override readonly string ToString()
+ {
+ return string.Format(CultureInfo.CurrentCulture, "{{ {{M11:{0} M12:{1}}} {{M21:{2} M22:{3}}} {{M31:{4} M32:{5}}} }}",
+ M11, M12,
+ M21, M22,
+ M31, M32);
+ }
+
+ /// <summary>
+ /// Returns the hash code for this instance.
+ /// </summary>
+ /// <returns>The hash code.</returns>
+ public override readonly int GetHashCode()
+ {
+ return unchecked(M11.GetHashCode() + M12.GetHashCode() +
+ M21.GetHashCode() + M22.GetHashCode() +
+ M31.GetHashCode() + M32.GetHashCode());
+ }
+ }
+}
--- /dev/null
+// Licensed to the .NET Foundation under one or more agreements.
+// The .NET Foundation licenses this file to you under the MIT license.
+// See the LICENSE file in the project root for more information.
+
+using System.Globalization;
+using System.Runtime.InteropServices;
+#if HAS_INTRINSICS
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
+#endif
+
+namespace System.Numerics
+{
+ /// <summary>
+ /// A structure encapsulating a 4x4 matrix.
+ /// </summary>
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Matrix4x4 : IEquatable<Matrix4x4>
+ {
+ private const float BillboardEpsilon = 1e-4f;
+ private const float BillboardMinAngle = 1.0f - (0.1f * (MathF.PI / 180.0f)); // 0.1 degrees
+ private const float DecomposeEpsilon = 0.0001f;
+
+ #region Public Fields
+ /// <summary>
+ /// Value at row 1, column 1 of the matrix.
+ /// </summary>
+ public float M11;
+ /// <summary>
+ /// Value at row 1, column 2 of the matrix.
+ /// </summary>
+ public float M12;
+ /// <summary>
+ /// Value at row 1, column 3 of the matrix.
+ /// </summary>
+ public float M13;
+ /// <summary>
+ /// Value at row 1, column 4 of the matrix.
+ /// </summary>
+ public float M14;
+
+ /// <summary>
+ /// Value at row 2, column 1 of the matrix.
+ /// </summary>
+ public float M21;
+ /// <summary>
+ /// Value at row 2, column 2 of the matrix.
+ /// </summary>
+ public float M22;
+ /// <summary>
+ /// Value at row 2, column 3 of the matrix.
+ /// </summary>
+ public float M23;
+ /// <summary>
+ /// Value at row 2, column 4 of the matrix.
+ /// </summary>
+ public float M24;
+
+ /// <summary>
+ /// Value at row 3, column 1 of the matrix.
+ /// </summary>
+ public float M31;
+ /// <summary>
+ /// Value at row 3, column 2 of the matrix.
+ /// </summary>
+ public float M32;
+ /// <summary>
+ /// Value at row 3, column 3 of the matrix.
+ /// </summary>
+ public float M33;
+ /// <summary>
+ /// Value at row 3, column 4 of the matrix.
+ /// </summary>
+ public float M34;
+
+ /// <summary>
+ /// Value at row 4, column 1 of the matrix.
+ /// </summary>
+ public float M41;
+ /// <summary>
+ /// Value at row 4, column 2 of the matrix.
+ /// </summary>
+ public float M42;
+ /// <summary>
+ /// Value at row 4, column 3 of the matrix.
+ /// </summary>
+ public float M43;
+ /// <summary>
+ /// Value at row 4, column 4 of the matrix.
+ /// </summary>
+ public float M44;
+ #endregion Public Fields
+
+ private static readonly Matrix4x4 _identity = new Matrix4x4
+ (
+ 1f, 0f, 0f, 0f,
+ 0f, 1f, 0f, 0f,
+ 0f, 0f, 1f, 0f,
+ 0f, 0f, 0f, 1f
+ );
+
+ /// <summary>
+ /// Returns the multiplicative identity matrix.
+ /// </summary>
+ public static Matrix4x4 Identity
+ {
+ get { return _identity; }
+ }
+
+ /// <summary>
+ /// Returns whether the matrix is the identity matrix.
+ /// </summary>
+ public readonly bool IsIdentity
+ {
+ get
+ {
+ return M11 == 1f && M22 == 1f && M33 == 1f && M44 == 1f && // Check diagonal element first for early out.
+ M12 == 0f && M13 == 0f && M14 == 0f &&
+ M21 == 0f && M23 == 0f && M24 == 0f &&
+ M31 == 0f && M32 == 0f && M34 == 0f &&
+ M41 == 0f && M42 == 0f && M43 == 0f;
+ }
+ }
+
+ /// <summary>
+ /// Gets or sets the translation component of this matrix.
+ /// </summary>
+ public Vector3 Translation
+ {
+ readonly get
+ {
+ return new Vector3(M41, M42, M43);
+ }
+ set
+ {
+ M41 = value.X;
+ M42 = value.Y;
+ M43 = value.Z;
+ }
+ }
+
+ /// <summary>
+ /// Constructs a Matrix4x4 from the given components.
+ /// </summary>
+ public Matrix4x4(float m11, float m12, float m13, float m14,
+ float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34,
+ float m41, float m42, float m43, float 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;
+ }
+
+ /// <summary>
+ /// Constructs a Matrix4x4 from the given Matrix3x2.
+ /// </summary>
+ /// <param name="value">The source Matrix3x2.</param>
+ public Matrix4x4(Matrix3x2 value)
+ {
+ M11 = value.M11;
+ M12 = value.M12;
+ M13 = 0f;
+ M14 = 0f;
+ M21 = value.M21;
+ M22 = value.M22;
+ M23 = 0f;
+ M24 = 0f;
+ M31 = 0f;
+ M32 = 0f;
+ M33 = 1f;
+ M34 = 0f;
+ M41 = value.M31;
+ M42 = value.M32;
+ M43 = 0f;
+ M44 = 1f;
+ }
+
+ /// <summary>
+ /// Creates a spherical billboard that rotates around a specified object position.
+ /// </summary>
+ /// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
+ /// <param name="cameraPosition">Position of the camera.</param>
+ /// <param name="cameraUpVector">The up vector of the camera.</param>
+ /// <param name="cameraForwardVector">The forward vector of the camera.</param>
+ /// <returns>The created billboard matrix</returns>
+ public static Matrix4x4 CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraUpVector, Vector3 cameraForwardVector)
+ {
+ Vector3 zaxis = new Vector3(
+ objectPosition.X - cameraPosition.X,
+ objectPosition.Y - cameraPosition.Y,
+ objectPosition.Z - cameraPosition.Z);
+
+ float norm = zaxis.LengthSquared();
+
+ if (norm < BillboardEpsilon)
+ {
+ zaxis = -cameraForwardVector;
+ }
+ else
+ {
+ zaxis = Vector3.Multiply(zaxis, 1.0f / MathF.Sqrt(norm));
+ }
+
+ Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis));
+
+ Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
+
+ Matrix4x4 result;
+
+ result.M11 = xaxis.X;
+ result.M12 = xaxis.Y;
+ result.M13 = xaxis.Z;
+ result.M14 = 0.0f;
+ result.M21 = yaxis.X;
+ result.M22 = yaxis.Y;
+ result.M23 = yaxis.Z;
+ result.M24 = 0.0f;
+ result.M31 = zaxis.X;
+ result.M32 = zaxis.Y;
+ result.M33 = zaxis.Z;
+ result.M34 = 0.0f;
+
+ result.M41 = objectPosition.X;
+ result.M42 = objectPosition.Y;
+ result.M43 = objectPosition.Z;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a cylindrical billboard that rotates around a specified axis.
+ /// </summary>
+ /// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
+ /// <param name="cameraPosition">Position of the camera.</param>
+ /// <param name="rotateAxis">Axis to rotate the billboard around.</param>
+ /// <param name="cameraForwardVector">Forward vector of the camera.</param>
+ /// <param name="objectForwardVector">Forward vector of the object.</param>
+ /// <returns>The created billboard matrix.</returns>
+ public static Matrix4x4 CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 rotateAxis, Vector3 cameraForwardVector, Vector3 objectForwardVector)
+ {
+ // Treat the case when object and camera positions are too close.
+ Vector3 faceDir = new Vector3(
+ objectPosition.X - cameraPosition.X,
+ objectPosition.Y - cameraPosition.Y,
+ objectPosition.Z - cameraPosition.Z);
+
+ float norm = faceDir.LengthSquared();
+
+ if (norm < BillboardEpsilon)
+ {
+ faceDir = -cameraForwardVector;
+ }
+ else
+ {
+ faceDir = Vector3.Multiply(faceDir, (1.0f / MathF.Sqrt(norm)));
+ }
+
+ Vector3 yaxis = rotateAxis;
+ Vector3 xaxis;
+ Vector3 zaxis;
+
+ // Treat the case when angle between faceDir and rotateAxis is too close to 0.
+ float dot = Vector3.Dot(rotateAxis, faceDir);
+
+ if (MathF.Abs(dot) > BillboardMinAngle)
+ {
+ zaxis = objectForwardVector;
+
+ // Make sure passed values are useful for compute.
+ dot = Vector3.Dot(rotateAxis, zaxis);
+
+ if (MathF.Abs(dot) > BillboardMinAngle)
+ {
+ zaxis = (MathF.Abs(rotateAxis.Z) > BillboardMinAngle) ? new Vector3(1, 0, 0) : new Vector3(0, 0, -1);
+ }
+
+ xaxis = Vector3.Normalize(Vector3.Cross(rotateAxis, zaxis));
+ zaxis = Vector3.Normalize(Vector3.Cross(xaxis, rotateAxis));
+ }
+ else
+ {
+ xaxis = Vector3.Normalize(Vector3.Cross(rotateAxis, faceDir));
+ zaxis = Vector3.Normalize(Vector3.Cross(xaxis, yaxis));
+ }
+
+ Matrix4x4 result;
+
+ result.M11 = xaxis.X;
+ result.M12 = xaxis.Y;
+ result.M13 = xaxis.Z;
+ result.M14 = 0.0f;
+ result.M21 = yaxis.X;
+ result.M22 = yaxis.Y;
+ result.M23 = yaxis.Z;
+ result.M24 = 0.0f;
+ result.M31 = zaxis.X;
+ result.M32 = zaxis.Y;
+ result.M33 = zaxis.Z;
+ result.M34 = 0.0f;
+
+ result.M41 = objectPosition.X;
+ result.M42 = objectPosition.Y;
+ result.M43 = objectPosition.Z;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a translation matrix.
+ /// </summary>
+ /// <param name="position">The amount to translate in each axis.</param>
+ /// <returns>The translation matrix.</returns>
+ public static Matrix4x4 CreateTranslation(Vector3 position)
+ {
+ Matrix4x4 result;
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = 0.0f;
+
+ result.M41 = position.X;
+ result.M42 = position.Y;
+ result.M43 = position.Z;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <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 Matrix4x4 CreateTranslation(float xPosition, float yPosition, float zPosition)
+ {
+ Matrix4x4 result;
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = 0.0f;
+
+ 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 Matrix4x4 CreateScale(float xScale, float yScale, float zScale)
+ {
+ Matrix4x4 result;
+
+ result.M11 = xScale;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = yScale;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = zScale;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scaling matrix with a center point.
+ /// </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>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The scaling matrix.</returns>
+ public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float tx = centerPoint.X * (1 - xScale);
+ float ty = centerPoint.Y * (1 - yScale);
+ float tz = centerPoint.Z * (1 - zScale);
+
+ result.M11 = xScale;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = yScale;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = zScale;
+ result.M34 = 0.0f;
+ result.M41 = tx;
+ result.M42 = ty;
+ result.M43 = tz;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scaling matrix.
+ /// </summary>
+ /// <param name="scales">The vector containing the amount to scale by on each axis.</param>
+ /// <returns>The scaling matrix.</returns>
+ public static Matrix4x4 CreateScale(Vector3 scales)
+ {
+ Matrix4x4 result;
+
+ result.M11 = scales.X;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scales.Y;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = scales.Z;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a scaling matrix with a center point.
+ /// </summary>
+ /// <param name="scales">The vector containing the amount to scale by on each axis.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The scaling matrix.</returns>
+ public static Matrix4x4 CreateScale(Vector3 scales, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float tx = centerPoint.X * (1 - scales.X);
+ float ty = centerPoint.Y * (1 - scales.Y);
+ float tz = centerPoint.Z * (1 - scales.Z);
+
+ result.M11 = scales.X;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scales.Y;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = scales.Z;
+ result.M34 = 0.0f;
+ result.M41 = tx;
+ result.M42 = ty;
+ result.M43 = tz;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a uniform scaling matrix that scales equally on each axis.
+ /// </summary>
+ /// <param name="scale">The uniform scaling factor.</param>
+ /// <returns>The scaling matrix.</returns>
+ public static Matrix4x4 CreateScale(float scale)
+ {
+ Matrix4x4 result;
+
+ result.M11 = scale;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scale;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = scale;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a uniform scaling matrix that scales equally on each axis with a center point.
+ /// </summary>
+ /// <param name="scale">The uniform scaling factor.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The scaling matrix.</returns>
+ public static Matrix4x4 CreateScale(float scale, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float tx = centerPoint.X * (1 - scale);
+ float ty = centerPoint.Y * (1 - scale);
+ float tz = centerPoint.Z * (1 - scale);
+
+ result.M11 = scale;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = scale;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = scale;
+ result.M34 = 0.0f;
+ result.M41 = tx;
+ result.M42 = ty;
+ result.M43 = tz;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the X-axis.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationX(float radians)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ // [ 1 0 0 0 ]
+ // [ 0 c s 0 ]
+ // [ 0 -s c 0 ]
+ // [ 0 0 0 1 ]
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = c;
+ result.M23 = s;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = -s;
+ result.M33 = c;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the X-axis, from a center point.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationX(float radians, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ float y = centerPoint.Y * (1 - c) + centerPoint.Z * s;
+ float z = centerPoint.Z * (1 - c) - centerPoint.Y * s;
+
+ // [ 1 0 0 0 ]
+ // [ 0 c s 0 ]
+ // [ 0 -s c 0 ]
+ // [ 0 y z 1 ]
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = c;
+ result.M23 = s;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = -s;
+ result.M33 = c;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = y;
+ result.M43 = z;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the Y-axis.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationY(float radians)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ // [ c 0 -s 0 ]
+ // [ 0 1 0 0 ]
+ // [ s 0 c 0 ]
+ // [ 0 0 0 1 ]
+ result.M11 = c;
+ result.M12 = 0.0f;
+ result.M13 = -s;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = s;
+ result.M32 = 0.0f;
+ result.M33 = c;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the Y-axis, from a center point.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationY(float radians, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ float x = centerPoint.X * (1 - c) - centerPoint.Z * s;
+ float z = centerPoint.Z * (1 - c) + centerPoint.X * s;
+
+ // [ c 0 -s 0 ]
+ // [ 0 1 0 0 ]
+ // [ s 0 c 0 ]
+ // [ x 0 z 1 ]
+ result.M11 = c;
+ result.M12 = 0.0f;
+ result.M13 = -s;
+ result.M14 = 0.0f;
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = s;
+ result.M32 = 0.0f;
+ result.M33 = c;
+ result.M34 = 0.0f;
+ result.M41 = x;
+ result.M42 = 0.0f;
+ result.M43 = z;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the Z-axis.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationZ(float radians)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ // [ c s 0 0 ]
+ // [ -s c 0 0 ]
+ // [ 0 0 1 0 ]
+ // [ 0 0 0 1 ]
+ result.M11 = c;
+ result.M12 = s;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = -s;
+ result.M22 = c;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix for rotating points around the Z-axis, from a center point.
+ /// </summary>
+ /// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
+ /// <param name="centerPoint">The center point.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateRotationZ(float radians, Vector3 centerPoint)
+ {
+ Matrix4x4 result;
+
+ float c = MathF.Cos(radians);
+ float s = MathF.Sin(radians);
+
+ float x = centerPoint.X * (1 - c) + centerPoint.Y * s;
+ float y = centerPoint.Y * (1 - c) - centerPoint.X * s;
+
+ // [ c s 0 0 ]
+ // [ -s c 0 0 ]
+ // [ 0 0 1 0 ]
+ // [ x y 0 1 ]
+ result.M11 = c;
+ result.M12 = s;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+ result.M21 = -s;
+ result.M22 = c;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = 0.0f;
+ result.M41 = x;
+ result.M42 = y;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a matrix that rotates around an arbitrary vector.
+ /// </summary>
+ /// <param name="axis">The axis to rotate around.</param>
+ /// <param name="angle">The angle to rotate around the given axis, in radians.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateFromAxisAngle(Vector3 axis, float angle)
+ {
+ // a: angle
+ // x, y, z: unit vector for axis.
+ //
+ // Rotation matrix M can compute by using below equation.
+ //
+ // T T
+ // M = uu + (cos a)( I-uu ) + (sin a)S
+ //
+ // Where:
+ //
+ // u = ( x, y, z )
+ //
+ // [ 0 -z y ]
+ // S = [ z 0 -x ]
+ // [ -y x 0 ]
+ //
+ // [ 1 0 0 ]
+ // I = [ 0 1 0 ]
+ // [ 0 0 1 ]
+ //
+ //
+ // [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
+ // M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
+ // [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
+ //
+ float x = axis.X, y = axis.Y, z = axis.Z;
+ float sa = MathF.Sin(angle), ca = MathF.Cos(angle);
+ float xx = x * x, yy = y * y, zz = z * z;
+ float xy = x * y, xz = x * z, yz = y * z;
+
+ Matrix4x4 result;
+
+ result.M11 = xx + ca * (1.0f - xx);
+ result.M12 = xy - ca * xy + sa * z;
+ result.M13 = xz - ca * xz - sa * y;
+ result.M14 = 0.0f;
+ result.M21 = xy - ca * xy - sa * z;
+ result.M22 = yy + ca * (1.0f - yy);
+ result.M23 = yz - ca * yz + sa * x;
+ result.M24 = 0.0f;
+ result.M31 = xz - ca * xz + sa * y;
+ result.M32 = yz - ca * yz - sa * x;
+ result.M33 = zz + ca * (1.0f - zz);
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a perspective projection matrix based on a field of view, aspect ratio, and near and far view plane distances.
+ /// </summary>
+ /// <param name="fieldOfView">Field of view in the y direction, in radians.</param>
+ /// <param name="aspectRatio">Aspect ratio, defined as view space width divided by height.</param>
+ /// <param name="nearPlaneDistance">Distance to the near view plane.</param>
+ /// <param name="farPlaneDistance">Distance to the far view plane.</param>
+ /// <returns>The perspective projection matrix.</returns>
+ public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance)
+ {
+ if (fieldOfView <= 0.0f || fieldOfView >= MathF.PI)
+ throw new ArgumentOutOfRangeException(nameof(fieldOfView));
+
+ if (nearPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ if (farPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(farPlaneDistance));
+
+ if (nearPlaneDistance >= farPlaneDistance)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ float yScale = 1.0f / MathF.Tan(fieldOfView * 0.5f);
+ float xScale = yScale / aspectRatio;
+
+ Matrix4x4 result;
+
+ result.M11 = xScale;
+ result.M12 = result.M13 = result.M14 = 0.0f;
+
+ result.M22 = yScale;
+ result.M21 = result.M23 = result.M24 = 0.0f;
+
+ result.M31 = result.M32 = 0.0f;
+ var negFarRange = float.IsPositiveInfinity(farPlaneDistance) ? -1.0f : farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
+ result.M33 = negFarRange;
+ result.M34 = -1.0f;
+
+ result.M41 = result.M42 = result.M44 = 0.0f;
+ result.M43 = nearPlaneDistance * negFarRange;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a perspective projection matrix from the given view volume dimensions.
+ /// </summary>
+ /// <param name="width">Width of the view volume at the near view plane.</param>
+ /// <param name="height">Height of the view volume at the near view plane.</param>
+ /// <param name="nearPlaneDistance">Distance to the near view plane.</param>
+ /// <param name="farPlaneDistance">Distance to the far view plane.</param>
+ /// <returns>The perspective projection matrix.</returns>
+ public static Matrix4x4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance)
+ {
+ if (nearPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ if (farPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(farPlaneDistance));
+
+ if (nearPlaneDistance >= farPlaneDistance)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ Matrix4x4 result;
+
+ result.M11 = 2.0f * nearPlaneDistance / width;
+ result.M12 = result.M13 = result.M14 = 0.0f;
+
+ result.M22 = 2.0f * nearPlaneDistance / height;
+ result.M21 = result.M23 = result.M24 = 0.0f;
+
+ var negFarRange = float.IsPositiveInfinity(farPlaneDistance) ? -1.0f : farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
+ result.M33 = negFarRange;
+ result.M31 = result.M32 = 0.0f;
+ result.M34 = -1.0f;
+
+ result.M41 = result.M42 = result.M44 = 0.0f;
+ result.M43 = nearPlaneDistance * negFarRange;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a customized, perspective projection matrix.
+ /// </summary>
+ /// <param name="left">Minimum x-value of the view volume at the near view plane.</param>
+ /// <param name="right">Maximum x-value of the view volume at the near view plane.</param>
+ /// <param name="bottom">Minimum y-value of the view volume at the near view plane.</param>
+ /// <param name="top">Maximum y-value of the view volume at the near view plane.</param>
+ /// <param name="nearPlaneDistance">Distance to the near view plane.</param>
+ /// <param name="farPlaneDistance">Distance to of the far view plane.</param>
+ /// <returns>The perspective projection matrix.</returns>
+ public static Matrix4x4 CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance)
+ {
+ if (nearPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ if (farPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException(nameof(farPlaneDistance));
+
+ if (nearPlaneDistance >= farPlaneDistance)
+ throw new ArgumentOutOfRangeException(nameof(nearPlaneDistance));
+
+ Matrix4x4 result;
+
+ result.M11 = 2.0f * nearPlaneDistance / (right - left);
+ result.M12 = result.M13 = result.M14 = 0.0f;
+
+ result.M22 = 2.0f * nearPlaneDistance / (top - bottom);
+ result.M21 = result.M23 = result.M24 = 0.0f;
+
+ result.M31 = (left + right) / (right - left);
+ result.M32 = (top + bottom) / (top - bottom);
+ var negFarRange = float.IsPositiveInfinity(farPlaneDistance) ? -1.0f : farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
+ result.M33 = negFarRange;
+ result.M34 = -1.0f;
+
+ result.M43 = nearPlaneDistance * negFarRange;
+ result.M41 = result.M42 = result.M44 = 0.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates an orthographic perspective matrix from the given view volume dimensions.
+ /// </summary>
+ /// <param name="width">Width of the view volume.</param>
+ /// <param name="height">Height of the view volume.</param>
+ /// <param name="zNearPlane">Minimum Z-value of the view volume.</param>
+ /// <param name="zFarPlane">Maximum Z-value of the view volume.</param>
+ /// <returns>The orthographic projection matrix.</returns>
+ public static Matrix4x4 CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
+ {
+ Matrix4x4 result;
+
+ result.M11 = 2.0f / width;
+ result.M12 = result.M13 = result.M14 = 0.0f;
+
+ result.M22 = 2.0f / height;
+ result.M21 = result.M23 = result.M24 = 0.0f;
+
+ result.M33 = 1.0f / (zNearPlane - zFarPlane);
+ result.M31 = result.M32 = result.M34 = 0.0f;
+
+ result.M41 = result.M42 = 0.0f;
+ result.M43 = zNearPlane / (zNearPlane - zFarPlane);
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Builds a customized, orthographic projection matrix.
+ /// </summary>
+ /// <param name="left">Minimum X-value of the view volume.</param>
+ /// <param name="right">Maximum X-value of the view volume.</param>
+ /// <param name="bottom">Minimum Y-value of the view volume.</param>
+ /// <param name="top">Maximum Y-value of the view volume.</param>
+ /// <param name="zNearPlane">Minimum Z-value of the view volume.</param>
+ /// <param name="zFarPlane">Maximum Z-value of the view volume.</param>
+ /// <returns>The orthographic projection matrix.</returns>
+ public static Matrix4x4 CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
+ {
+ Matrix4x4 result;
+
+ result.M11 = 2.0f / (right - left);
+ result.M12 = result.M13 = result.M14 = 0.0f;
+
+ result.M22 = 2.0f / (top - bottom);
+ result.M21 = result.M23 = result.M24 = 0.0f;
+
+ result.M33 = 1.0f / (zNearPlane - zFarPlane);
+ result.M31 = result.M32 = result.M34 = 0.0f;
+
+ result.M41 = (left + right) / (left - right);
+ result.M42 = (top + bottom) / (bottom - top);
+ result.M43 = zNearPlane / (zNearPlane - zFarPlane);
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a view matrix.
+ /// </summary>
+ /// <param name="cameraPosition">The position of the camera.</param>
+ /// <param name="cameraTarget">The target towards which the camera is pointing.</param>
+ /// <param name="cameraUpVector">The direction that is "up" from the camera's point of view.</param>
+ /// <returns>The view matrix.</returns>
+ public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
+ {
+ Vector3 zaxis = Vector3.Normalize(cameraPosition - cameraTarget);
+ Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis));
+ Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
+
+ Matrix4x4 result;
+
+ result.M11 = xaxis.X;
+ result.M12 = yaxis.X;
+ result.M13 = zaxis.X;
+ result.M14 = 0.0f;
+ result.M21 = xaxis.Y;
+ result.M22 = yaxis.Y;
+ result.M23 = zaxis.Y;
+ result.M24 = 0.0f;
+ result.M31 = xaxis.Z;
+ result.M32 = yaxis.Z;
+ result.M33 = zaxis.Z;
+ result.M34 = 0.0f;
+ result.M41 = -Vector3.Dot(xaxis, cameraPosition);
+ result.M42 = -Vector3.Dot(yaxis, cameraPosition);
+ result.M43 = -Vector3.Dot(zaxis, cameraPosition);
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a world matrix with the specified parameters.
+ /// </summary>
+ /// <param name="position">The position of the object; used in translation operations.</param>
+ /// <param name="forward">Forward direction of the object.</param>
+ /// <param name="up">Upward direction of the object; usually [0, 1, 0].</param>
+ /// <returns>The world matrix.</returns>
+ public static Matrix4x4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up)
+ {
+ Vector3 zaxis = Vector3.Normalize(-forward);
+ Vector3 xaxis = Vector3.Normalize(Vector3.Cross(up, zaxis));
+ Vector3 yaxis = Vector3.Cross(zaxis, xaxis);
+
+ Matrix4x4 result;
+
+ result.M11 = xaxis.X;
+ result.M12 = xaxis.Y;
+ result.M13 = xaxis.Z;
+ result.M14 = 0.0f;
+ result.M21 = yaxis.X;
+ result.M22 = yaxis.Y;
+ result.M23 = yaxis.Z;
+ result.M24 = 0.0f;
+ result.M31 = zaxis.X;
+ result.M32 = zaxis.Y;
+ result.M33 = zaxis.Z;
+ result.M34 = 0.0f;
+ result.M41 = position.X;
+ result.M42 = position.Y;
+ result.M43 = position.Z;
+ result.M44 = 1.0f;
+
+ 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 Matrix4x4 CreateFromQuaternion(Quaternion quaternion)
+ {
+ Matrix4x4 result;
+
+ float xx = quaternion.X * quaternion.X;
+ float yy = quaternion.Y * quaternion.Y;
+ float zz = quaternion.Z * quaternion.Z;
+
+ float xy = quaternion.X * quaternion.Y;
+ float wz = quaternion.Z * quaternion.W;
+ float xz = quaternion.Z * quaternion.X;
+ float wy = quaternion.Y * quaternion.W;
+ float yz = quaternion.Y * quaternion.Z;
+ float wx = quaternion.X * quaternion.W;
+
+ result.M11 = 1.0f - 2.0f * (yy + zz);
+ result.M12 = 2.0f * (xy + wz);
+ result.M13 = 2.0f * (xz - wy);
+ result.M14 = 0.0f;
+ result.M21 = 2.0f * (xy - wz);
+ result.M22 = 1.0f - 2.0f * (zz + xx);
+ result.M23 = 2.0f * (yz + wx);
+ result.M24 = 0.0f;
+ result.M31 = 2.0f * (xz + wy);
+ result.M32 = 2.0f * (yz - wx);
+ result.M33 = 1.0f - 2.0f * (yy + xx);
+ result.M34 = 0.0f;
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a rotation matrix from the specified yaw, pitch, and roll.
+ /// </summary>
+ /// <param name="yaw">Angle of rotation, in radians, around the Y-axis.</param>
+ /// <param name="pitch">Angle of rotation, in radians, around the X-axis.</param>
+ /// <param name="roll">Angle of rotation, in radians, around the Z-axis.</param>
+ /// <returns>The rotation matrix.</returns>
+ public static Matrix4x4 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
+ {
+ Quaternion q = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll);
+
+ return Matrix4x4.CreateFromQuaternion(q);
+ }
+
+ /// <summary>
+ /// Creates a Matrix that flattens geometry into a specified Plane as if casting a shadow from a specified light source.
+ /// </summary>
+ /// <param name="lightDirection">The direction from which the light that will cast the shadow is coming.</param>
+ /// <param name="plane">The Plane onto which the new matrix should flatten geometry so as to cast a shadow.</param>
+ /// <returns>A new Matrix that can be used to flatten geometry onto the specified plane from the specified direction.</returns>
+ public static Matrix4x4 CreateShadow(Vector3 lightDirection, Plane plane)
+ {
+ Plane p = Plane.Normalize(plane);
+
+ float dot = p.Normal.X * lightDirection.X + p.Normal.Y * lightDirection.Y + p.Normal.Z * lightDirection.Z;
+ float a = -p.Normal.X;
+ float b = -p.Normal.Y;
+ float c = -p.Normal.Z;
+ float d = -p.D;
+
+ Matrix4x4 result;
+
+ result.M11 = a * lightDirection.X + dot;
+ result.M21 = b * lightDirection.X;
+ result.M31 = c * lightDirection.X;
+ result.M41 = d * lightDirection.X;
+
+ result.M12 = a * lightDirection.Y;
+ result.M22 = b * lightDirection.Y + dot;
+ result.M32 = c * lightDirection.Y;
+ result.M42 = d * lightDirection.Y;
+
+ result.M13 = a * lightDirection.Z;
+ result.M23 = b * lightDirection.Z;
+ result.M33 = c * lightDirection.Z + dot;
+ result.M43 = d * lightDirection.Z;
+
+ result.M14 = 0.0f;
+ result.M24 = 0.0f;
+ result.M34 = 0.0f;
+ result.M44 = dot;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a Matrix that reflects the coordinate system about a specified Plane.
+ /// </summary>
+ /// <param name="value">The Plane about which to create a reflection.</param>
+ /// <returns>A new matrix expressing the reflection.</returns>
+ public static Matrix4x4 CreateReflection(Plane value)
+ {
+ value = Plane.Normalize(value);
+
+ float a = value.Normal.X;
+ float b = value.Normal.Y;
+ float c = value.Normal.Z;
+
+ float fa = -2.0f * a;
+ float fb = -2.0f * b;
+ float fc = -2.0f * c;
+
+ Matrix4x4 result;
+
+ result.M11 = fa * a + 1.0f;
+ result.M12 = fb * a;
+ result.M13 = fc * a;
+ result.M14 = 0.0f;
+
+ result.M21 = fa * b;
+ result.M22 = fb * b + 1.0f;
+ result.M23 = fc * b;
+ result.M24 = 0.0f;
+
+ result.M31 = fa * c;
+ result.M32 = fb * c;
+ result.M33 = fc * c + 1.0f;
+ result.M34 = 0.0f;
+
+ result.M41 = fa * value.D;
+ result.M42 = fb * value.D;
+ result.M43 = fc * value.D;
+ result.M44 = 1.0f;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Calculates the determinant of the matrix.
+ /// </summary>
+ /// <returns>The determinant of the matrix.</returns>
+ public readonly float GetDeterminant()
+ {
+ // | a b c d | | f g h | | e g h | | e f h | | e f g |
+ // | e f g h | = a | j k l | - b | i k l | + c | i j l | - d | i j k |
+ // | i j k l | | n o p | | m o p | | m n p | | m n o |
+ // | m n o p |
+ //
+ // | f g h |
+ // a | j k l | = a ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
+ // | n o p |
+ //
+ // | e g h |
+ // b | i k l | = b ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
+ // | m o p |
+ //
+ // | e f h |
+ // c | i j l | = c ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
+ // | m n p |
+ //
+ // | e f g |
+ // d | i j k | = d ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
+ // | m n o |
+ //
+ // Cost of operation
+ // 17 adds and 28 muls.
+ //
+ // add: 6 + 8 + 3 = 17
+ // mul: 12 + 16 = 28
+
+ float a = M11, b = M12, c = M13, d = M14;
+ float e = M21, f = M22, g = M23, h = M24;
+ float i = M31, j = M32, k = M33, l = M34;
+ float m = M41, n = M42, o = M43, p = M44;
+
+ float kp_lo = k * p - l * o;
+ float jp_ln = j * p - l * n;
+ float jo_kn = j * o - k * n;
+ float ip_lm = i * p - l * m;
+ float io_km = i * o - k * m;
+ float in_jm = i * n - j * m;
+
+ return a * (f * kp_lo - g * jp_ln + h * jo_kn) -
+ b * (e * kp_lo - g * ip_lm + h * io_km) +
+ c * (e * jp_ln - f * ip_lm + h * in_jm) -
+ d * (e * jo_kn - f * io_km + g * in_jm);
+ }
+
+ /// <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(Matrix4x4 matrix, out Matrix4x4 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.
+ float a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
+ float e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
+ float i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
+ float m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
+
+ float kp_lo = k * p - l * o;
+ float jp_ln = j * p - l * n;
+ float jo_kn = j * o - k * n;
+ float ip_lm = i * p - l * m;
+ float io_km = i * o - k * m;
+ float in_jm = i * n - j * m;
+
+ float a11 = +(f * kp_lo - g * jp_ln + h * jo_kn);
+ float a12 = -(e * kp_lo - g * ip_lm + h * io_km);
+ float a13 = +(e * jp_ln - f * ip_lm + h * in_jm);
+ float a14 = -(e * jo_kn - f * io_km + g * in_jm);
+
+ float det = a * a11 + b * a12 + c * a13 + d * a14;
+
+ if (MathF.Abs(det) < float.Epsilon)
+ {
+ result = new Matrix4x4(float.NaN, float.NaN, float.NaN, float.NaN,
+ float.NaN, float.NaN, float.NaN, float.NaN,
+ float.NaN, float.NaN, float.NaN, float.NaN,
+ float.NaN, float.NaN, float.NaN, float.NaN);
+ return false;
+ }
+
+ float invDet = 1.0f / 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;
+
+ float gp_ho = g * p - h * o;
+ float fp_hn = f * p - h * n;
+ float fo_gn = f * o - g * n;
+ float ep_hm = e * p - h * m;
+ float eo_gm = e * o - g * m;
+ float 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;
+
+ float gl_hk = g * l - h * k;
+ float fl_hj = f * l - h * j;
+ float fk_gj = f * k - g * j;
+ float el_hi = e * l - h * i;
+ float ek_gi = e * k - g * i;
+ float 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;
+ }
+
+
+ private struct CanonicalBasis
+ {
+ public Vector3 Row0;
+ public Vector3 Row1;
+ public Vector3 Row2;
+ };
+
+
+ private struct VectorBasis
+ {
+ public unsafe Vector3* Element0;
+ public unsafe Vector3* Element1;
+ public unsafe Vector3* Element2;
+ }
+
+ /// <summary>
+ /// Attempts to extract the scale, translation, and rotation components from the given scale/rotation/translation matrix.
+ /// If successful, the out parameters will contained the extracted values.
+ /// </summary>
+ /// <param name="matrix">The source matrix.</param>
+ /// <param name="scale">The scaling component of the transformation matrix.</param>
+ /// <param name="rotation">The rotation component of the transformation matrix.</param>
+ /// <param name="translation">The translation component of the transformation matrix</param>
+ /// <returns>True if the source matrix was successfully decomposed; False otherwise.</returns>
+ public static bool Decompose(Matrix4x4 matrix, out Vector3 scale, out Quaternion rotation, out Vector3 translation)
+ {
+ bool result = true;
+
+ unsafe
+ {
+ fixed (Vector3* scaleBase = &scale)
+ {
+ float* pfScales = (float*)scaleBase;
+ float det;
+
+ VectorBasis vectorBasis;
+ Vector3** pVectorBasis = (Vector3**)&vectorBasis;
+
+ Matrix4x4 matTemp = Matrix4x4.Identity;
+ CanonicalBasis canonicalBasis = new CanonicalBasis();
+ Vector3* pCanonicalBasis = &canonicalBasis.Row0;
+
+ canonicalBasis.Row0 = new Vector3(1.0f, 0.0f, 0.0f);
+ canonicalBasis.Row1 = new Vector3(0.0f, 1.0f, 0.0f);
+ canonicalBasis.Row2 = new Vector3(0.0f, 0.0f, 1.0f);
+
+ translation = new Vector3(
+ matrix.M41,
+ matrix.M42,
+ matrix.M43);
+
+ pVectorBasis[0] = (Vector3*)&matTemp.M11;
+ pVectorBasis[1] = (Vector3*)&matTemp.M21;
+ pVectorBasis[2] = (Vector3*)&matTemp.M31;
+
+ *(pVectorBasis[0]) = new Vector3(matrix.M11, matrix.M12, matrix.M13);
+ *(pVectorBasis[1]) = new Vector3(matrix.M21, matrix.M22, matrix.M23);
+ *(pVectorBasis[2]) = new Vector3(matrix.M31, matrix.M32, matrix.M33);
+
+ scale.X = pVectorBasis[0]->Length();
+ scale.Y = pVectorBasis[1]->Length();
+ scale.Z = pVectorBasis[2]->Length();
+
+ uint a, b, c;
+ #region Ranking
+ float x = pfScales[0], y = pfScales[1], z = pfScales[2];
+ if (x < y)
+ {
+ if (y < z)
+ {
+ a = 2;
+ b = 1;
+ c = 0;
+ }
+ else
+ {
+ a = 1;
+
+ if (x < z)
+ {
+ b = 2;
+ c = 0;
+ }
+ else
+ {
+ b = 0;
+ c = 2;
+ }
+ }
+ }
+ else
+ {
+ if (x < z)
+ {
+ a = 2;
+ b = 0;
+ c = 1;
+ }
+ else
+ {
+ a = 0;
+
+ if (y < z)
+ {
+ b = 2;
+ c = 1;
+ }
+ else
+ {
+ b = 1;
+ c = 2;
+ }
+ }
+ }
+ #endregion
+
+ if (pfScales[a] < DecomposeEpsilon)
+ {
+ *(pVectorBasis[a]) = pCanonicalBasis[a];
+ }
+
+ *pVectorBasis[a] = Vector3.Normalize(*pVectorBasis[a]);
+
+ if (pfScales[b] < DecomposeEpsilon)
+ {
+ uint cc;
+ float fAbsX, fAbsY, fAbsZ;
+
+ fAbsX = MathF.Abs(pVectorBasis[a]->X);
+ fAbsY = MathF.Abs(pVectorBasis[a]->Y);
+ fAbsZ = MathF.Abs(pVectorBasis[a]->Z);
+
+ #region Ranking
+ if (fAbsX < fAbsY)
+ {
+ if (fAbsY < fAbsZ)
+ {
+ cc = 0;
+ }
+ else
+ {
+ if (fAbsX < fAbsZ)
+ {
+ cc = 0;
+ }
+ else
+ {
+ cc = 2;
+ }
+ }
+ }
+ else
+ {
+ if (fAbsX < fAbsZ)
+ {
+ cc = 1;
+ }
+ else
+ {
+ if (fAbsY < fAbsZ)
+ {
+ cc = 1;
+ }
+ else
+ {
+ cc = 2;
+ }
+ }
+ }
+ #endregion
+
+ *pVectorBasis[b] = Vector3.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc));
+ }
+
+ *pVectorBasis[b] = Vector3.Normalize(*pVectorBasis[b]);
+
+ if (pfScales[c] < DecomposeEpsilon)
+ {
+ *pVectorBasis[c] = Vector3.Cross(*pVectorBasis[a], *pVectorBasis[b]);
+ }
+
+ *pVectorBasis[c] = Vector3.Normalize(*pVectorBasis[c]);
+
+ det = matTemp.GetDeterminant();
+
+ // use Kramer's rule to check for handedness of coordinate system
+ if (det < 0.0f)
+ {
+ // switch coordinate system by negating the scale and inverting the basis vector on the x-axis
+ pfScales[a] = -pfScales[a];
+ *pVectorBasis[a] = -(*pVectorBasis[a]);
+
+ det = -det;
+ }
+
+ det -= 1.0f;
+ det *= det;
+
+ if ((DecomposeEpsilon < det))
+ {
+ // Non-SRT matrix encountered
+ rotation = Quaternion.Identity;
+ result = false;
+ }
+ else
+ {
+ // generate the quaternion from the matrix
+ rotation = Quaternion.CreateFromRotationMatrix(matTemp);
+ }
+ }
+ }
+
+ return result;
+ }
+
+ /// <summary>
+ /// Transforms the given matrix by applying the given Quaternion rotation.
+ /// </summary>
+ /// <param name="value">The source matrix to transform.</param>
+ /// <param name="rotation">The rotation to apply.</param>
+ /// <returns>The transformed matrix.</returns>
+ public static Matrix4x4 Transform(Matrix4x4 value, Quaternion rotation)
+ {
+ // Compute rotation matrix.
+ float x2 = rotation.X + rotation.X;
+ float y2 = rotation.Y + rotation.Y;
+ float z2 = rotation.Z + rotation.Z;
+
+ float wx2 = rotation.W * x2;
+ float wy2 = rotation.W * y2;
+ float wz2 = rotation.W * z2;
+ float xx2 = rotation.X * x2;
+ float xy2 = rotation.X * y2;
+ float xz2 = rotation.X * z2;
+ float yy2 = rotation.Y * y2;
+ float yz2 = rotation.Y * z2;
+ float zz2 = rotation.Z * z2;
+
+ float q11 = 1.0f - yy2 - zz2;
+ float q21 = xy2 - wz2;
+ float q31 = xz2 + wy2;
+
+ float q12 = xy2 + wz2;
+ float q22 = 1.0f - xx2 - zz2;
+ float q32 = yz2 - wx2;
+
+ float q13 = xz2 - wy2;
+ float q23 = yz2 + wx2;
+ float q33 = 1.0f - xx2 - yy2;
+
+ Matrix4x4 result;
+
+ // First row
+ result.M11 = value.M11 * q11 + value.M12 * q21 + value.M13 * q31;
+ result.M12 = value.M11 * q12 + value.M12 * q22 + value.M13 * q32;
+ result.M13 = value.M11 * q13 + value.M12 * q23 + value.M13 * q33;
+ result.M14 = value.M14;
+
+ // Second row
+ result.M21 = value.M21 * q11 + value.M22 * q21 + value.M23 * q31;
+ result.M22 = value.M21 * q12 + value.M22 * q22 + value.M23 * q32;
+ result.M23 = value.M21 * q13 + value.M22 * q23 + value.M23 * q33;
+ result.M24 = value.M24;
+
+ // Third row
+ result.M31 = value.M31 * q11 + value.M32 * q21 + value.M33 * q31;
+ result.M32 = value.M31 * q12 + value.M32 * q22 + value.M33 * q32;
+ result.M33 = value.M31 * q13 + value.M32 * q23 + value.M33 * q33;
+ result.M34 = value.M34;
+
+ // Fourth row
+ result.M41 = value.M41 * q11 + value.M42 * q21 + value.M43 * q31;
+ result.M42 = value.M41 * q12 + value.M42 * q22 + value.M43 * q32;
+ result.M43 = value.M41 * q13 + value.M42 * q23 + value.M43 * q33;
+ result.M44 = value.M44;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Transposes the rows and columns of a matrix.
+ /// </summary>
+ /// <param name="matrix">The source matrix.</param>
+ /// <returns>The transposed matrix.</returns>
+ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ var row1 = Sse.LoadVector128(&matrix.M11);
+ var row2 = Sse.LoadVector128(&matrix.M21);
+ var row3 = Sse.LoadVector128(&matrix.M31);
+ var row4 = Sse.LoadVector128(&matrix.M41);
+
+ var l12 = Sse.UnpackLow(row1, row2);
+ var l34 = Sse.UnpackLow(row3, row4);
+ var h12 = Sse.UnpackHigh(row1, row2);
+ var h34 = Sse.UnpackHigh(row3, row4);
+
+ Sse.Store(&matrix.M11, Sse.MoveLowToHigh(l12, l34));
+ Sse.Store(&matrix.M21, Sse.MoveHighToLow(l34, l12));
+ Sse.Store(&matrix.M31, Sse.MoveLowToHigh(h12, h34));
+ Sse.Store(&matrix.M41, Sse.MoveHighToLow(h34, h12));
+
+ return matrix;
+ }
+#endif
+ Matrix4x4 result;
+
+ result.M11 = matrix.M11;
+ result.M12 = matrix.M21;
+ result.M13 = matrix.M31;
+ result.M14 = matrix.M41;
+ result.M21 = matrix.M12;
+ result.M22 = matrix.M22;
+ result.M23 = matrix.M32;
+ result.M24 = matrix.M42;
+ result.M31 = matrix.M13;
+ result.M32 = matrix.M23;
+ result.M33 = matrix.M33;
+ result.M34 = matrix.M43;
+ result.M41 = matrix.M14;
+ result.M42 = matrix.M24;
+ result.M43 = matrix.M34;
+ result.M44 = matrix.M44;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Linearly interpolates between the corresponding values of two matrices.
+ /// </summary>
+ /// <param name="matrix1">The first source matrix.</param>
+ /// <param name="matrix2">The second source matrix.</param>
+ /// <param name="amount">The relative weight of the second source matrix.</param>
+ /// <returns>The interpolated matrix.</returns>
+ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ Vector128<float> amountVec = Vector128.Create(amount);
+ Sse.Store(&matrix1.M11, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M11), Sse.LoadVector128(&matrix2.M11), amountVec));
+ Sse.Store(&matrix1.M21, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M21), Sse.LoadVector128(&matrix2.M21), amountVec));
+ Sse.Store(&matrix1.M31, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M31), Sse.LoadVector128(&matrix2.M31), amountVec));
+ Sse.Store(&matrix1.M41, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M41), Sse.LoadVector128(&matrix2.M41), amountVec));
+ return matrix1;
+ }
+#endif
+ Matrix4x4 result;
+
+ // First row
+ result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount;
+ result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount;
+ result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount;
+ result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount;
+
+ // Second row
+ result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount;
+ result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount;
+ result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount;
+ result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount;
+
+ // Third row
+ result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount;
+ result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount;
+ result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount;
+ result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount;
+
+ // Fourth row
+ result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount;
+ result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount;
+ result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount;
+ result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Returns a new matrix with the negated elements of the given matrix.
+ /// </summary>
+ /// <param name="value">The source matrix.</param>
+ /// <returns>The negated matrix.</returns>
+ public static Matrix4x4 Negate(Matrix4x4 value) => -value;
+
+ /// <summary>
+ /// Adds two matrices together.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The resulting matrix.</returns>
+ public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) => value1 + value2;
+
+ /// <summary>
+ /// Subtracts the second matrix from the first.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The result of the subtraction.</returns>
+ public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 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 Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) => value1 * value2;
+
+ /// <summary>
+ /// Multiplies a matrix by a scalar value.
+ /// </summary>
+ /// <param name="value1">The source matrix.</param>
+ /// <param name="value2">The scaling factor.</param>
+ /// <returns>The scaled matrix.</returns>
+ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) => value1 * value2;
+
+ /// <summary>
+ /// Returns a new matrix with the negated elements of the given matrix.
+ /// </summary>
+ /// <param name="value">The source matrix.</param>
+ /// <returns>The negated matrix.</returns>
+ public static unsafe Matrix4x4 operator -(Matrix4x4 value)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ Vector128<float> zero = Vector128<float>.Zero;
+ Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11)));
+ Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21)));
+ Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31)));
+ Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41)));
+
+ return value;
+ }
+#endif
+ Matrix4x4 m;
+
+ m.M11 = -value.M11;
+ m.M12 = -value.M12;
+ m.M13 = -value.M13;
+ m.M14 = -value.M14;
+ m.M21 = -value.M21;
+ m.M22 = -value.M22;
+ m.M23 = -value.M23;
+ m.M24 = -value.M24;
+ m.M31 = -value.M31;
+ m.M32 = -value.M32;
+ m.M33 = -value.M33;
+ m.M34 = -value.M34;
+ m.M41 = -value.M41;
+ m.M42 = -value.M42;
+ m.M43 = -value.M43;
+ m.M44 = -value.M44;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Adds two matrices together.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The resulting matrix.</returns>
+ public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)));
+ Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)));
+ Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)));
+ Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41)));
+ return value1;
+ }
+#endif
+ Matrix4x4 m;
+
+ m.M11 = value1.M11 + value2.M11;
+ m.M12 = value1.M12 + value2.M12;
+ m.M13 = value1.M13 + value2.M13;
+ m.M14 = value1.M14 + value2.M14;
+ m.M21 = value1.M21 + value2.M21;
+ m.M22 = value1.M22 + value2.M22;
+ m.M23 = value1.M23 + value2.M23;
+ m.M24 = value1.M24 + value2.M24;
+ m.M31 = value1.M31 + value2.M31;
+ m.M32 = value1.M32 + value2.M32;
+ m.M33 = value1.M33 + value2.M33;
+ m.M34 = value1.M34 + value2.M34;
+ m.M41 = value1.M41 + value2.M41;
+ m.M42 = value1.M42 + value2.M42;
+ m.M43 = value1.M43 + value2.M43;
+ m.M44 = value1.M44 + value2.M44;
+
+ return m;
+ }
+
+ /// <summary>
+ /// Subtracts the second matrix from the first.
+ /// </summary>
+ /// <param name="value1">The first source matrix.</param>
+ /// <param name="value2">The second source matrix.</param>
+ /// <returns>The result of the subtraction.</returns>
+ public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)));
+ Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)));
+ Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)));
+ Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41)));
+ return value1;
+ }
+#endif
+ Matrix4x4 m;
+
+ m.M11 = value1.M11 - value2.M11;
+ m.M12 = value1.M12 - value2.M12;
+ m.M13 = value1.M13 - value2.M13;
+ m.M14 = value1.M14 - value2.M14;
+ m.M21 = value1.M21 - value2.M21;
+ m.M22 = value1.M22 - value2.M22;
+ m.M23 = value1.M23 - value2.M23;
+ m.M24 = value1.M24 - value2.M24;
+ m.M31 = value1.M31 - value2.M31;
+ m.M32 = value1.M32 - value2.M32;
+ m.M33 = value1.M33 - value2.M33;
+ m.M34 = value1.M34 - value2.M34;
+ m.M41 = value1.M41 - value2.M41;
+ m.M42 = value1.M42 - value2.M42;
+ m.M43 = value1.M43 - value2.M43;
+ m.M44 = value1.M44 - value2.M44;
+
+ return m;
+ }
+
+ /// <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 unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ var row = Sse.LoadVector128(&value1.M11);
+ Sse.Store(&value1.M11,
+ Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))),
+ Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41)))));
+
+ // 0x00 is _MM_SHUFFLE(0,0,0,0), 0x55 is _MM_SHUFFLE(1,1,1,1), etc.
+ // TODO: Replace with a method once it's added to the API.
+
+ row = Sse.LoadVector128(&value1.M21);
+ Sse.Store(&value1.M21,
+ Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))),
+ Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41)))));
+
+ row = Sse.LoadVector128(&value1.M31);
+ Sse.Store(&value1.M31,
+ Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))),
+ Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41)))));
+
+ row = Sse.LoadVector128(&value1.M41);
+ Sse.Store(&value1.M41,
+ Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))),
+ Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)),
+ Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41)))));
+ return value1;
+ }
+#endif
+ Matrix4x4 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;
+ }
+
+ /// <summary>
+ /// Multiplies a matrix by a scalar value.
+ /// </summary>
+ /// <param name="value1">The source matrix.</param>
+ /// <param name="value2">The scaling factor.</param>
+ /// <returns>The scaled matrix.</returns>
+ public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ Vector128<float> value2Vec = Vector128.Create(value2);
+ Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), value2Vec));
+ Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), value2Vec));
+ Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), value2Vec));
+ Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), value2Vec));
+ return value1;
+ }
+#endif
+ Matrix4x4 m;
+
+ m.M11 = value1.M11 * value2;
+ m.M12 = value1.M12 * value2;
+ m.M13 = value1.M13 * value2;
+ m.M14 = value1.M14 * value2;
+ m.M21 = value1.M21 * value2;
+ m.M22 = value1.M22 * value2;
+ m.M23 = value1.M23 * value2;
+ m.M24 = value1.M24 * value2;
+ m.M31 = value1.M31 * value2;
+ m.M32 = value1.M32 * value2;
+ m.M33 = value1.M33 * value2;
+ m.M34 = value1.M34 * value2;
+ m.M41 = value1.M41 * value2;
+ m.M42 = value1.M42 * value2;
+ m.M43 = value1.M43 * value2;
+ m.M44 = value1.M44 * value2;
+ return m;
+ }
+
+ /// <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 unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ return
+ VectorMath.Equal(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)) &&
+ VectorMath.Equal(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)) &&
+ VectorMath.Equal(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)) &&
+ VectorMath.Equal(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41));
+ }
+#endif
+ 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 unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2)
+ {
+#if HAS_INTRINSICS
+ if (Sse.IsSupported)
+ {
+ return
+ VectorMath.NotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)) ||
+ VectorMath.NotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)) ||
+ VectorMath.NotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)) ||
+ VectorMath.NotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41));
+ }
+#endif
+ 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 readonly bool Equals(Matrix4x4 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 readonly bool Equals(object? obj) => (obj is Matrix4x4 other) && (this == other);
+
+ /// <summary>
+ /// Returns a String representing this matrix instance.
+ /// </summary>
+ /// <returns>The string representation.</returns>
+ public override readonly string ToString()
+ {
+ return string.Format(CultureInfo.CurrentCulture, "{{ {{M11:{0} M12:{1} M13:{2} M14:{3}}} {{M21:{4} M22:{5} M23:{6} M24:{7}}} {{M31:{8} M32:{9} M33:{10} M34:{11}}} {{M41:{12} M42:{13} M43:{14} M44:{15}}} }}",
+ M11, M12, M13, M14,
+ M21, M22, M23, M24,
+ M31, M32, M33, M34,
+ M41, M42, M43, M44);
+ }
+
+ /// <summary>
+ /// Returns the hash code for this instance.
+ /// </summary>
+ /// <returns>The hash code.</returns>
+ public override readonly 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();
+ }
+ }
+ }
+}
--- /dev/null
+// Licensed to the .NET Foundation under one or more agreements.
+// The .NET Foundation licenses this file to you under the MIT license.
+// See the LICENSE file in the project root for more information.
+
+using System.Globalization;
+
+namespace System.Numerics
+{
+ /// <summary>
+ /// A structure encapsulating a four-dimensional vector (x,y,z,w),
+ /// which is used to efficiently rotate an object about the (x,y,z) vector by the angle theta, where w = cos(theta/2).
+ /// </summary>
+ public struct Quaternion : IEquatable<Quaternion>
+ {
+ private const float SlerpEpsilon = 1e-6f;
+
+ /// <summary>
+ /// Specifies the X-value of the vector component of the Quaternion.
+ /// </summary>
+ public float X;
+ /// <summary>
+ /// Specifies the Y-value of the vector component of the Quaternion.
+ /// </summary>
+ public float Y;
+ /// <summary>
+ /// Specifies the Z-value of the vector component of the Quaternion.
+ /// </summary>
+ public float Z;
+ /// <summary>
+ /// Specifies the rotation component of the Quaternion.
+ /// </summary>
+ public float W;
+
+ /// <summary>
+ /// Returns a Quaternion representing no rotation.
+ /// </summary>
+ public static Quaternion Identity
+ {
+ get { return new Quaternion(0, 0, 0, 1); }
+ }
+
+ /// <summary>
+ /// Returns whether the Quaternion is the identity Quaternion.
+ /// </summary>
+ public readonly bool IsIdentity
+ {
+ get { return X == 0f && Y == 0f && Z == 0f && W == 1f; }
+ }
+
+ /// <summary>
+ /// Constructs a Quaternion from the given components.
+ /// </summary>
+ /// <param name="x">The X component of the Quaternion.</param>
+ /// <param name="y">The Y component of the Quaternion.</param>
+ /// <param name="z">The Z component of the Quaternion.</param>
+ /// <param name="w">The W component of the Quaternion.</param>
+ public Quaternion(float x, float y, float z, float w)
+ {
+ this.X = x;
+ this.Y = y;
+ this.Z = z;
+ this.W = w;
+ }
+
+ /// <summary>
+ /// Constructs a Quaternion from the given vector and rotation parts.
+ /// </summary>
+ /// <param name="vectorPart">The vector part of the Quaternion.</param>
+ /// <param name="scalarPart">The rotation part of the Quaternion.</param>
+ public Quaternion(Vector3 vectorPart, float scalarPart)
+ {
+ X = vectorPart.X;
+ Y = vectorPart.Y;
+ Z = vectorPart.Z;
+ W = scalarPart;
+ }
+
+ /// <summary>
+ /// Calculates the length of the Quaternion.
+ /// </summary>
+ /// <returns>The computed length of the Quaternion.</returns>
+ public readonly float Length()
+ {
+ float ls = X * X + Y * Y + Z * Z + W * W;
+
+ return MathF.Sqrt(ls);
+ }
+
+ /// <summary>
+ /// Calculates the length squared of the Quaternion. This operation is cheaper than Length().
+ /// </summary>
+ /// <returns>The length squared of the Quaternion.</returns>
+ public readonly float LengthSquared()
+ {
+ return X * X + Y * Y + Z * Z + W * W;
+ }
+
+ /// <summary>
+ /// Divides each component of the Quaternion by the length of the Quaternion.
+ /// </summary>
+ /// <param name="value">The source Quaternion.</param>
+ /// <returns>The normalized Quaternion.</returns>
+ public static Quaternion Normalize(Quaternion value)
+ {
+ Quaternion ans;
+
+ float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
+
+ float invNorm = 1.0f / MathF.Sqrt(ls);
+
+ ans.X = value.X * invNorm;
+ ans.Y = value.Y * invNorm;
+ ans.Z = value.Z * invNorm;
+ ans.W = value.W * invNorm;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Creates the conjugate of a specified Quaternion.
+ /// </summary>
+ /// <param name="value">The Quaternion of which to return the conjugate.</param>
+ /// <returns>A new Quaternion that is the conjugate of the specified one.</returns>
+ public static Quaternion Conjugate(Quaternion value)
+ {
+ Quaternion ans;
+
+ ans.X = -value.X;
+ ans.Y = -value.Y;
+ ans.Z = -value.Z;
+ ans.W = value.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Returns the inverse of a Quaternion.
+ /// </summary>
+ /// <param name="value">The source Quaternion.</param>
+ /// <returns>The inverted Quaternion.</returns>
+ public static Quaternion Inverse(Quaternion value)
+ {
+ // -1 ( a -v )
+ // q = ( ------------- ------------- )
+ // ( a^2 + |v|^2 , a^2 + |v|^2 )
+
+ Quaternion ans;
+
+ float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
+ float invNorm = 1.0f / ls;
+
+ ans.X = -value.X * invNorm;
+ ans.Y = -value.Y * invNorm;
+ ans.Z = -value.Z * invNorm;
+ ans.W = value.W * invNorm;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Creates a Quaternion from a normalized vector axis and an angle to rotate about the vector.
+ /// </summary>
+ /// <param name="axis">The unit vector to rotate around.
+ /// This vector must be normalized before calling this function or the resulting Quaternion will be incorrect.</param>
+ /// <param name="angle">The angle, in radians, to rotate around the vector.</param>
+ /// <returns>The created Quaternion.</returns>
+ public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
+ {
+ Quaternion ans;
+
+ float halfAngle = angle * 0.5f;
+ float s = MathF.Sin(halfAngle);
+ float c = MathF.Cos(halfAngle);
+
+ ans.X = axis.X * s;
+ ans.Y = axis.Y * s;
+ ans.Z = axis.Z * s;
+ ans.W = c;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Creates a new Quaternion from the given yaw, pitch, and roll, in radians.
+ /// </summary>
+ /// <param name="yaw">The yaw angle, in radians, around the Y-axis.</param>
+ /// <param name="pitch">The pitch angle, in radians, around the X-axis.</param>
+ /// <param name="roll">The roll angle, in radians, around the Z-axis.</param>
+ /// <returns></returns>
+ public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
+ {
+ // Roll first, about axis the object is facing, then
+ // pitch upward, then yaw to face into the new heading
+ float sr, cr, sp, cp, sy, cy;
+
+ float halfRoll = roll * 0.5f;
+ sr = MathF.Sin(halfRoll);
+ cr = MathF.Cos(halfRoll);
+
+ float halfPitch = pitch * 0.5f;
+ sp = MathF.Sin(halfPitch);
+ cp = MathF.Cos(halfPitch);
+
+ float halfYaw = yaw * 0.5f;
+ sy = MathF.Sin(halfYaw);
+ cy = MathF.Cos(halfYaw);
+
+ Quaternion result;
+
+ result.X = cy * sp * cr + sy * cp * sr;
+ result.Y = sy * cp * cr - cy * sp * sr;
+ result.Z = cy * cp * sr - sy * sp * cr;
+ result.W = cy * cp * cr + sy * sp * sr;
+
+ return result;
+ }
+
+ /// <summary>
+ /// Creates a Quaternion from the given rotation matrix.
+ /// </summary>
+ /// <param name="matrix">The rotation matrix.</param>
+ /// <returns>The created Quaternion.</returns>
+ public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix)
+ {
+ float trace = matrix.M11 + matrix.M22 + matrix.M33;
+
+ Quaternion q = new Quaternion();
+
+ if (trace > 0.0f)
+ {
+ float s = MathF.Sqrt(trace + 1.0f);
+ q.W = s * 0.5f;
+ s = 0.5f / s;
+ q.X = (matrix.M23 - matrix.M32) * s;
+ q.Y = (matrix.M31 - matrix.M13) * s;
+ q.Z = (matrix.M12 - matrix.M21) * s;
+ }
+ else
+ {
+ if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
+ {
+ float s = MathF.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
+ float invS = 0.5f / s;
+ q.X = 0.5f * s;
+ q.Y = (matrix.M12 + matrix.M21) * invS;
+ q.Z = (matrix.M13 + matrix.M31) * invS;
+ q.W = (matrix.M23 - matrix.M32) * invS;
+ }
+ else if (matrix.M22 > matrix.M33)
+ {
+ float s = MathF.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
+ float invS = 0.5f / s;
+ q.X = (matrix.M21 + matrix.M12) * invS;
+ q.Y = 0.5f * s;
+ q.Z = (matrix.M32 + matrix.M23) * invS;
+ q.W = (matrix.M31 - matrix.M13) * invS;
+ }
+ else
+ {
+ float s = MathF.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
+ float invS = 0.5f / s;
+ q.X = (matrix.M31 + matrix.M13) * invS;
+ q.Y = (matrix.M32 + matrix.M23) * invS;
+ q.Z = 0.5f * s;
+ q.W = (matrix.M12 - matrix.M21) * invS;
+ }
+ }
+
+ return q;
+ }
+
+ /// <summary>
+ /// Calculates the dot product of two Quaternions.
+ /// </summary>
+ /// <param name="quaternion1">The first source Quaternion.</param>
+ /// <param name="quaternion2">The second source Quaternion.</param>
+ /// <returns>The dot product of the Quaternions.</returns>
+ public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
+ {
+ return quaternion1.X * quaternion2.X +
+ quaternion1.Y * quaternion2.Y +
+ quaternion1.Z * quaternion2.Z +
+ quaternion1.W * quaternion2.W;
+ }
+
+ /// <summary>
+ /// Interpolates between two quaternions, using spherical linear interpolation.
+ /// </summary>
+ /// <param name="quaternion1">The first source Quaternion.</param>
+ /// <param name="quaternion2">The second source Quaternion.</param>
+ /// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
+ /// <returns>The interpolated Quaternion.</returns>
+ public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
+ {
+ float t = amount;
+
+ float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
+ quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
+
+ bool flip = false;
+
+ if (cosOmega < 0.0f)
+ {
+ flip = true;
+ cosOmega = -cosOmega;
+ }
+
+ float s1, s2;
+
+ if (cosOmega > (1.0f - SlerpEpsilon))
+ {
+ // Too close, do straight linear interpolation.
+ s1 = 1.0f - t;
+ s2 = (flip) ? -t : t;
+ }
+ else
+ {
+ float omega = MathF.Acos(cosOmega);
+ float invSinOmega = 1 / MathF.Sin(omega);
+
+ s1 = MathF.Sin((1.0f - t) * omega) * invSinOmega;
+ s2 = (flip)
+ ? -MathF.Sin(t * omega) * invSinOmega
+ : MathF.Sin(t * omega) * invSinOmega;
+ }
+
+ Quaternion ans;
+
+ ans.X = s1 * quaternion1.X + s2 * quaternion2.X;
+ ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y;
+ ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z;
+ ans.W = s1 * quaternion1.W + s2 * quaternion2.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Linearly interpolates between two quaternions.
+ /// </summary>
+ /// <param name="quaternion1">The first source Quaternion.</param>
+ /// <param name="quaternion2">The second source Quaternion.</param>
+ /// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
+ /// <returns>The interpolated Quaternion.</returns>
+ public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
+ {
+ float t = amount;
+ float t1 = 1.0f - t;
+
+ Quaternion r = new Quaternion();
+
+ float dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
+ quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
+
+ if (dot >= 0.0f)
+ {
+ r.X = t1 * quaternion1.X + t * quaternion2.X;
+ r.Y = t1 * quaternion1.Y + t * quaternion2.Y;
+ r.Z = t1 * quaternion1.Z + t * quaternion2.Z;
+ r.W = t1 * quaternion1.W + t * quaternion2.W;
+ }
+ else
+ {
+ r.X = t1 * quaternion1.X - t * quaternion2.X;
+ r.Y = t1 * quaternion1.Y - t * quaternion2.Y;
+ r.Z = t1 * quaternion1.Z - t * quaternion2.Z;
+ r.W = t1 * quaternion1.W - t * quaternion2.W;
+ }
+
+ // Normalize it.
+ float ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W;
+ float invNorm = 1.0f / MathF.Sqrt(ls);
+
+ r.X *= invNorm;
+ r.Y *= invNorm;
+ r.Z *= invNorm;
+ r.W *= invNorm;
+
+ return r;
+ }
+
+ /// <summary>
+ /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
+ /// </summary>
+ /// <param name="value1">The first Quaternion rotation in the series.</param>
+ /// <param name="value2">The second Quaternion rotation in the series.</param>
+ /// <returns>A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation.</returns>
+ public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ // Concatenate rotation is actually q2 * q1 instead of q1 * q2.
+ // So that's why value2 goes q1 and value1 goes q2.
+ float q1x = value2.X;
+ float q1y = value2.Y;
+ float q1z = value2.Z;
+ float q1w = value2.W;
+
+ float q2x = value1.X;
+ float q2y = value1.Y;
+ float q2z = value1.Z;
+ float q2w = value1.W;
+
+ // cross(av, bv)
+ float cx = q1y * q2z - q1z * q2y;
+ float cy = q1z * q2x - q1x * q2z;
+ float cz = q1x * q2y - q1y * q2x;
+
+ float dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+ ans.X = q1x * q2w + q2x * q1w + cx;
+ ans.Y = q1y * q2w + q2y * q1w + cy;
+ ans.Z = q1z * q2w + q2z * q1w + cz;
+ ans.W = q1w * q2w - dot;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Flips the sign of each component of the quaternion.
+ /// </summary>
+ /// <param name="value">The source Quaternion.</param>
+ /// <returns>The negated Quaternion.</returns>
+ public static Quaternion Negate(Quaternion value)
+ {
+ Quaternion ans;
+
+ ans.X = -value.X;
+ ans.Y = -value.Y;
+ ans.Z = -value.Z;
+ ans.W = -value.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Adds two Quaternions element-by-element.
+ /// </summary>
+ /// <param name="value1">The first source Quaternion.</param>
+ /// <param name="value2">The second source Quaternion.</param>
+ /// <returns>The result of adding the Quaternions.</returns>
+ public static Quaternion Add(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X + value2.X;
+ ans.Y = value1.Y + value2.Y;
+ ans.Z = value1.Z + value2.Z;
+ ans.W = value1.W + value2.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Subtracts one Quaternion from another.
+ /// </summary>
+ /// <param name="value1">The first source Quaternion.</param>
+ /// <param name="value2">The second Quaternion, to be subtracted from the first.</param>
+ /// <returns>The result of the subtraction.</returns>
+ public static Quaternion Subtract(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X - value2.X;
+ ans.Y = value1.Y - value2.Y;
+ ans.Z = value1.Z - value2.Z;
+ ans.W = value1.W - value2.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Multiplies two Quaternions together.
+ /// </summary>
+ /// <param name="value1">The Quaternion on the left side of the multiplication.</param>
+ /// <param name="value2">The Quaternion on the right side of the multiplication.</param>
+ /// <returns>The result of the multiplication.</returns>
+ public static Quaternion Multiply(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ float q1x = value1.X;
+ float q1y = value1.Y;
+ float q1z = value1.Z;
+ float q1w = value1.W;
+
+ float q2x = value2.X;
+ float q2y = value2.Y;
+ float q2z = value2.Z;
+ float q2w = value2.W;
+
+ // cross(av, bv)
+ float cx = q1y * q2z - q1z * q2y;
+ float cy = q1z * q2x - q1x * q2z;
+ float cz = q1x * q2y - q1y * q2x;
+
+ float dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+ ans.X = q1x * q2w + q2x * q1w + cx;
+ ans.Y = q1y * q2w + q2y * q1w + cy;
+ ans.Z = q1z * q2w + q2z * q1w + cz;
+ ans.W = q1w * q2w - dot;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Multiplies a Quaternion by a scalar value.
+ /// </summary>
+ /// <param name="value1">The source Quaternion.</param>
+ /// <param name="value2">The scalar value.</param>
+ /// <returns>The result of the multiplication.</returns>
+ public static Quaternion Multiply(Quaternion value1, float value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X * value2;
+ ans.Y = value1.Y * value2;
+ ans.Z = value1.Z * value2;
+ ans.W = value1.W * value2;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Divides a Quaternion by another Quaternion.
+ /// </summary>
+ /// <param name="value1">The source Quaternion.</param>
+ /// <param name="value2">The divisor.</param>
+ /// <returns>The result of the division.</returns>
+ public static Quaternion Divide(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ float q1x = value1.X;
+ float q1y = value1.Y;
+ float q1z = value1.Z;
+ float q1w = value1.W;
+
+ //-------------------------------------
+ // Inverse part.
+ float ls = value2.X * value2.X + value2.Y * value2.Y +
+ value2.Z * value2.Z + value2.W * value2.W;
+ float invNorm = 1.0f / ls;
+
+ float q2x = -value2.X * invNorm;
+ float q2y = -value2.Y * invNorm;
+ float q2z = -value2.Z * invNorm;
+ float q2w = value2.W * invNorm;
+
+ //-------------------------------------
+ // Multiply part.
+
+ // cross(av, bv)
+ float cx = q1y * q2z - q1z * q2y;
+ float cy = q1z * q2x - q1x * q2z;
+ float cz = q1x * q2y - q1y * q2x;
+
+ float dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+ ans.X = q1x * q2w + q2x * q1w + cx;
+ ans.Y = q1y * q2w + q2y * q1w + cy;
+ ans.Z = q1z * q2w + q2z * q1w + cz;
+ ans.W = q1w * q2w - dot;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Flips the sign of each component of the quaternion.
+ /// </summary>
+ /// <param name="value">The source Quaternion.</param>
+ /// <returns>The negated Quaternion.</returns>
+ public static Quaternion operator -(Quaternion value)
+ {
+ Quaternion ans;
+
+ ans.X = -value.X;
+ ans.Y = -value.Y;
+ ans.Z = -value.Z;
+ ans.W = -value.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Adds two Quaternions element-by-element.
+ /// </summary>
+ /// <param name="value1">The first source Quaternion.</param>
+ /// <param name="value2">The second source Quaternion.</param>
+ /// <returns>The result of adding the Quaternions.</returns>
+ public static Quaternion operator +(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X + value2.X;
+ ans.Y = value1.Y + value2.Y;
+ ans.Z = value1.Z + value2.Z;
+ ans.W = value1.W + value2.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Subtracts one Quaternion from another.
+ /// </summary>
+ /// <param name="value1">The first source Quaternion.</param>
+ /// <param name="value2">The second Quaternion, to be subtracted from the first.</param>
+ /// <returns>The result of the subtraction.</returns>
+ public static Quaternion operator -(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X - value2.X;
+ ans.Y = value1.Y - value2.Y;
+ ans.Z = value1.Z - value2.Z;
+ ans.W = value1.W - value2.W;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Multiplies two Quaternions together.
+ /// </summary>
+ /// <param name="value1">The Quaternion on the left side of the multiplication.</param>
+ /// <param name="value2">The Quaternion on the right side of the multiplication.</param>
+ /// <returns>The result of the multiplication.</returns>
+ public static Quaternion operator *(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ float q1x = value1.X;
+ float q1y = value1.Y;
+ float q1z = value1.Z;
+ float q1w = value1.W;
+
+ float q2x = value2.X;
+ float q2y = value2.Y;
+ float q2z = value2.Z;
+ float q2w = value2.W;
+
+ // cross(av, bv)
+ float cx = q1y * q2z - q1z * q2y;
+ float cy = q1z * q2x - q1x * q2z;
+ float cz = q1x * q2y - q1y * q2x;
+
+ float dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+ ans.X = q1x * q2w + q2x * q1w + cx;
+ ans.Y = q1y * q2w + q2y * q1w + cy;
+ ans.Z = q1z * q2w + q2z * q1w + cz;
+ ans.W = q1w * q2w - dot;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Multiplies a Quaternion by a scalar value.
+ /// </summary>
+ /// <param name="value1">The source Quaternion.</param>
+ /// <param name="value2">The scalar value.</param>
+ /// <returns>The result of the multiplication.</returns>
+ public static Quaternion operator *(Quaternion value1, float value2)
+ {
+ Quaternion ans;
+
+ ans.X = value1.X * value2;
+ ans.Y = value1.Y * value2;
+ ans.Z = value1.Z * value2;
+ ans.W = value1.W * value2;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Divides a Quaternion by another Quaternion.
+ /// </summary>
+ /// <param name="value1">The source Quaternion.</param>
+ /// <param name="value2">The divisor.</param>
+ /// <returns>The result of the division.</returns>
+ public static Quaternion operator /(Quaternion value1, Quaternion value2)
+ {
+ Quaternion ans;
+
+ float q1x = value1.X;
+ float q1y = value1.Y;
+ float q1z = value1.Z;
+ float q1w = value1.W;
+
+ //-------------------------------------
+ // Inverse part.
+ float ls = value2.X * value2.X + value2.Y * value2.Y +
+ value2.Z * value2.Z + value2.W * value2.W;
+ float invNorm = 1.0f / ls;
+
+ float q2x = -value2.X * invNorm;
+ float q2y = -value2.Y * invNorm;
+ float q2z = -value2.Z * invNorm;
+ float q2w = value2.W * invNorm;
+
+ //-------------------------------------
+ // Multiply part.
+
+ // cross(av, bv)
+ float cx = q1y * q2z - q1z * q2y;
+ float cy = q1z * q2x - q1x * q2z;
+ float cz = q1x * q2y - q1y * q2x;
+
+ float dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+ ans.X = q1x * q2w + q2x * q1w + cx;
+ ans.Y = q1y * q2w + q2y * q1w + cy;
+ ans.Z = q1z * q2w + q2z * q1w + cz;
+ ans.W = q1w * q2w - dot;
+
+ return ans;
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the two given Quaternions are equal.
+ /// </summary>
+ /// <param name="value1">The first Quaternion to compare.</param>
+ /// <param name="value2">The second Quaternion to compare.</param>
+ /// <returns>True if the Quaternions are equal; False otherwise.</returns>
+ public static bool operator ==(Quaternion value1, Quaternion value2)
+ {
+ return (value1.X == value2.X &&
+ value1.Y == value2.Y &&
+ value1.Z == value2.Z &&
+ value1.W == value2.W);
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the two given Quaternions are not equal.
+ /// </summary>
+ /// <param name="value1">The first Quaternion to compare.</param>
+ /// <param name="value2">The second Quaternion to compare.</param>
+ /// <returns>True if the Quaternions are not equal; False if they are equal.</returns>
+ public static bool operator !=(Quaternion value1, Quaternion value2)
+ {
+ return (value1.X != value2.X ||
+ value1.Y != value2.Y ||
+ value1.Z != value2.Z ||
+ value1.W != value2.W);
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the given Quaternion is equal to this Quaternion instance.
+ /// </summary>
+ /// <param name="other">The Quaternion to compare this instance to.</param>
+ /// <returns>True if the other Quaternion is equal to this instance; False otherwise.</returns>
+ public readonly bool Equals(Quaternion other)
+ {
+ return (X == other.X &&
+ Y == other.Y &&
+ Z == other.Z &&
+ W == other.W);
+ }
+
+ /// <summary>
+ /// Returns a boolean indicating whether the given Object is equal to this Quaternion instance.
+ /// </summary>
+ /// <param name="obj">The Object to compare against.</param>
+ /// <returns>True if the Object is equal to this Quaternion; False otherwise.</returns>
+ public override readonly bool Equals(object? obj)
+ {
+ if (obj is Quaternion)
+ {
+ return Equals((Quaternion)obj);
+ }
+
+ return false;
+ }
+
+ /// <summary>
+ /// Returns a String representing this Quaternion instance.
+ /// </summary>
+ /// <returns>The string representation.</returns>
+ public override readonly string ToString()
+ {
+ return string.Format(CultureInfo.CurrentCulture, "{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W);
+ }
+
+ /// <summary>
+ /// Returns the hash code for this instance.
+ /// </summary>
+ /// <returns>The hash code.</returns>
+ public override readonly int GetHashCode()
+ {
+ return unchecked(X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode());
+ }
+ }
+}
projects / platform / upstream / dotnet / runtime.git / commitdiff