From e64bc548c609455652fcd4107f1f4a2ac3084ff3 Mon Sep 17 00:00:00 2001 From: Tanner Gooding Date: Fri, 11 Sep 2020 12:27:39 -0700 Subject: [PATCH] Removing the Vector*_Intrinsics.cs file, regions from the numerics code, and normalizing the member order (#41898) * Removing the Vector*_Intrinsics.cs file, regions from the numerics code, and normalizing the member order * Updating the doc comment for Quaternion --- .../src/System.Private.CoreLib.Shared.projitems | 20 +- .../src/System/Numerics/Matrix3x2.cs | 819 +++---- .../src/System/Numerics/Matrix4x4.cs | 2528 ++++++++++---------- .../src/System/Numerics/Plane.cs | 216 +- .../src/System/Numerics/Quaternion.cs | 893 +++---- .../src/System/Numerics/Vector2.cs | 651 +++-- .../src/System/Numerics/Vector2_Intrinsics.cs | 295 --- .../src/System/Numerics/Vector3.cs | 692 ++++-- .../src/System/Numerics/Vector3_Intrinsics.cs | 313 --- .../src/System/Numerics/Vector4.cs | 731 ++++-- .../src/System/Numerics/Vector4_Intrinsics.cs | 348 --- 11 files changed, 3358 insertions(+), 4148 deletions(-) delete mode 100644 src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2_Intrinsics.cs delete mode 100644 src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3_Intrinsics.cs delete mode 100644 src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4_Intrinsics.cs diff --git a/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems b/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems index ec80155..2cbc1ae 100644 --- a/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems +++ b/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems @@ -29,17 +29,10 @@ - - - - - - - + + + + @@ -467,11 +460,8 @@ - - - @@ -1977,4 +1967,4 @@ - + \ No newline at end of file diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.cs index a70b3d7..e792c8b 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.cs @@ -6,79 +6,67 @@ using System.Runtime.CompilerServices; namespace System.Numerics { - /// - /// A structure encapsulating a 3x2 matrix. - /// + /// A structure encapsulating a 3x2 matrix. [Intrinsic] public struct Matrix3x2 : IEquatable { private const float RotationEpsilon = 0.001f * MathF.PI / 180f; // 0.1% of a degree - #region Public Fields - /// - /// The first element of the first row - /// + private static readonly Matrix3x2 _identity = new Matrix3x2( + 1f, 0f, + 0f, 1f, + 0f, 0f + ); + + /// The first element of the first row public float M11; - /// - /// The second element of the first row - /// + + /// The second element of the first row public float M12; - /// - /// The first element of the second row - /// + + /// The first element of the second row public float M21; - /// - /// The second element of the second row - /// + + /// The second element of the second row public float M22; - /// - /// The first element of the third row - /// + + /// The first element of the third row public float M31; - /// - /// The second element of the third row - /// + + /// The second element of the third row public float M32; - #endregion Public Fields - private static readonly Matrix3x2 _identity = new Matrix3x2 - ( - 1f, 0f, - 0f, 1f, - 0f, 0f - ); + /// Constructs a Matrix3x2 from the given components. + public Matrix3x2(float m11, float m12, + float m21, float m22, + float m31, float m32) + { + M11 = m11; + M12 = m12; + + M21 = m21; + M22 = m22; + + M31 = m31; + M32 = m32; + } - /// - /// Returns the multiplicative identity matrix. - /// + /// Returns the multiplicative identity matrix. public static Matrix3x2 Identity { - get { return _identity; } + get => _identity; } - /// - /// Returns whether the matrix is the identity matrix. - /// + /// Returns whether the matrix is the identity matrix. 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; - } + get => this == Identity; } - /// - /// Gets or sets the translation component of this matrix. - /// + /// Gets or sets the translation component of this matrix. public Vector2 Translation { - readonly get - { - return new Vector2(M31, M32); - } + readonly get => new Vector2(M31, M32); set { @@ -87,210 +75,143 @@ namespace System.Numerics } } - /// - /// Constructs a Matrix3x2 from the given components. - /// - 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; - } - - /// - /// Creates a translation matrix from the given vector. - /// - /// The translation position. - /// A translation matrix. - public static Matrix3x2 CreateTranslation(Vector2 position) + /// Adds each matrix element in value1 with its corresponding element in value2. + /// The first source matrix. + /// The second source matrix. + /// The matrix containing the summed values. + public static Matrix3x2 operator +(Matrix3x2 value1, Matrix3x2 value2) { - Matrix3x2 result = _identity; - - result.M31 = position.X; - result.M32 = position.Y; + Matrix3x2 m; - return result; - } + m.M11 = value1.M11 + value2.M11; + m.M12 = value1.M12 + value2.M12; - /// - /// Creates a translation matrix from the given X and Y components. - /// - /// The X position. - /// The Y position. - /// A translation matrix. - public static Matrix3x2 CreateTranslation(float xPosition, float yPosition) - { - Matrix3x2 result = _identity; + m.M21 = value1.M21 + value2.M21; + m.M22 = value1.M22 + value2.M22; - result.M31 = xPosition; - result.M32 = yPosition; + m.M31 = value1.M31 + value2.M31; + m.M32 = value1.M32 + value2.M32; - return result; + return m; } - /// - /// Creates a scale matrix from the given X and Y components. - /// - /// Value to scale by on the X-axis. - /// Value to scale by on the Y-axis. - /// A scaling matrix. - public static Matrix3x2 CreateScale(float xScale, float yScale) + /// Returns a boolean indicating whether the given matrices are equal. + /// The first source matrix. + /// The second source matrix. + /// True if the matrices are equal; False otherwise. + public static bool operator ==(Matrix3x2 value1, Matrix3x2 value2) { - Matrix3x2 result = _identity; - - result.M11 = xScale; - result.M22 = yScale; - - return result; + // Check diagonal element first for early out. + return (value1.M11 == value2.M11 + && value1.M22 == value2.M22 + && value1.M12 == value2.M12 + && value1.M21 == value2.M21 + && value1.M31 == value2.M31 + && value1.M32 == value2.M32); } - /// - /// Creates a scale matrix that is offset by a given center point. - /// - /// Value to scale by on the X-axis. - /// Value to scale by on the Y-axis. - /// The center point. - /// A scaling matrix. - public static Matrix3x2 CreateScale(float xScale, float yScale, Vector2 centerPoint) + /// Returns a boolean indicating whether the given matrices are not equal. + /// The first source matrix. + /// The second source matrix. + /// True if the matrices are not equal; False if they are equal. + public static bool operator !=(Matrix3x2 value1, Matrix3x2 value2) { - Matrix3x2 result = _identity; - - float tx = centerPoint.X * (1 - xScale); - float ty = centerPoint.Y * (1 - yScale); - - result.M11 = xScale; - result.M22 = yScale; - result.M31 = tx; - result.M32 = ty; - - return result; + return !(value1 == value2); } - /// - /// Creates a scale matrix from the given vector scale. - /// - /// The scale to use. - /// A scaling matrix. - public static Matrix3x2 CreateScale(Vector2 scales) + /// Multiplies two matrices together and returns the resulting matrix. + /// The first source matrix. + /// The second source matrix. + /// The product matrix. + public static Matrix3x2 operator *(Matrix3x2 value1, Matrix3x2 value2) { - Matrix3x2 result = _identity; - - result.M11 = scales.X; - result.M22 = scales.Y; - - return result; - } + Matrix3x2 m; - /// - /// Creates a scale matrix from the given vector scale with an offset from the given center point. - /// - /// The scale to use. - /// The center offset. - /// A scaling matrix. - public static Matrix3x2 CreateScale(Vector2 scales, Vector2 centerPoint) - { - Matrix3x2 result = _identity; + // First row + m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21; + m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22; - float tx = centerPoint.X * (1 - scales.X); - float ty = centerPoint.Y * (1 - scales.Y); + // Second row + m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21; + m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22; - result.M11 = scales.X; - result.M22 = scales.Y; - result.M31 = tx; - result.M32 = ty; + // 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 result; + return m; } - /// - /// Creates a scale matrix that scales uniformly with the given scale. - /// - /// The uniform scale to use. - /// A scaling matrix. - public static Matrix3x2 CreateScale(float scale) + /// Scales all elements in a matrix by the given scalar factor. + /// The source matrix. + /// The scaling value to use. + /// The resulting matrix. + public static Matrix3x2 operator *(Matrix3x2 value1, float value2) { - Matrix3x2 result = _identity; - - result.M11 = scale; - result.M22 = scale; - - return result; - } + Matrix3x2 m; - /// - /// Creates a scale matrix that scales uniformly with the given scale with an offset from the given center. - /// - /// The uniform scale to use. - /// The center offset. - /// A scaling matrix. - public static Matrix3x2 CreateScale(float scale, Vector2 centerPoint) - { - Matrix3x2 result = _identity; + m.M11 = value1.M11 * value2; + m.M12 = value1.M12 * value2; - float tx = centerPoint.X * (1 - scale); - float ty = centerPoint.Y * (1 - scale); + m.M21 = value1.M21 * value2; + m.M22 = value1.M22 * value2; - result.M11 = scale; - result.M22 = scale; - result.M31 = tx; - result.M32 = ty; + m.M31 = value1.M31 * value2; + m.M32 = value1.M32 * value2; - return result; + return m; } - /// - /// Creates a skew matrix from the given angles in radians. - /// - /// The X angle, in radians. - /// The Y angle, in radians. - /// A skew matrix. - public static Matrix3x2 CreateSkew(float radiansX, float radiansY) + /// Subtracts each matrix element in value2 from its corresponding element in value1. + /// The first source matrix. + /// The second source matrix. + /// The matrix containing the resulting values. + public static Matrix3x2 operator -(Matrix3x2 value1, Matrix3x2 value2) { - Matrix3x2 result = _identity; + Matrix3x2 m; - float xTan = MathF.Tan(radiansX); - float yTan = MathF.Tan(radiansY); + m.M11 = value1.M11 - value2.M11; + m.M12 = value1.M12 - value2.M12; - result.M12 = yTan; - result.M21 = xTan; + m.M21 = value1.M21 - value2.M21; + m.M22 = value1.M22 - value2.M22; - return result; + m.M31 = value1.M31 - value2.M31; + m.M32 = value1.M32 - value2.M32; + + return m; } - /// - /// Creates a skew matrix from the given angles in radians and a center point. - /// - /// The X angle, in radians. - /// The Y angle, in radians. - /// The center point. - /// A skew matrix. - public static Matrix3x2 CreateSkew(float radiansX, float radiansY, Vector2 centerPoint) + /// Negates the given matrix by multiplying all values by -1. + /// The source matrix. + /// The negated matrix. + public static Matrix3x2 operator -(Matrix3x2 value) { - Matrix3x2 result = _identity; + Matrix3x2 m; - float xTan = MathF.Tan(radiansX); - float yTan = MathF.Tan(radiansY); + m.M11 = -value.M11; + m.M12 = -value.M12; - float tx = -centerPoint.Y * xTan; - float ty = -centerPoint.X * yTan; + m.M21 = -value.M21; + m.M22 = -value.M22; - result.M12 = yTan; - result.M21 = xTan; + m.M31 = -value.M31; + m.M32 = -value.M32; - result.M31 = tx; - result.M32 = ty; + return m; + } - return result; + /// Adds each matrix element in value1 with its corresponding element in value2. + /// The first source matrix. + /// The second source matrix. + /// The matrix containing the summed values. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x2 Add(Matrix3x2 value1, Matrix3x2 value2) + { + return value1 + value2; } - /// - /// Creates a rotation matrix using the given rotation in radians. - /// + /// Creates a rotation matrix using the given rotation in radians. /// The amount of rotation, in radians. /// A rotation matrix. public static Matrix3x2 CreateRotation(float radians) @@ -333,7 +254,8 @@ namespace System.Numerics // [ c s ] // [ -s c ] // [ 0 0 ] - Matrix3x2 result = _identity; + Matrix3x2 result = Identity; + result.M11 = c; result.M12 = s; result.M21 = -s; @@ -342,9 +264,7 @@ namespace System.Numerics return result; } - /// - /// Creates a rotation matrix using the given rotation in radians and a center point. - /// + /// Creates a rotation matrix using the given rotation in radians and a center point. /// The amount of rotation, in radians. /// The center point. /// A rotation matrix. @@ -403,352 +323,312 @@ namespace System.Numerics return result; } - /// - /// Calculates the determinant for this matrix. - /// The determinant is calculated by expanding the matrix with a third column whose values are (0,0,1). - /// - /// The determinant. - public readonly float GetDeterminant() + /// Creates a scale matrix from the given vector scale. + /// The scale to use. + /// A scaling matrix. + public static Matrix3x2 CreateScale(Vector2 scales) { - // 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! + Matrix3x2 result = Identity; - return (M11 * M22) - (M21 * M12); + result.M11 = scales.X; + result.M22 = scales.Y; + + return result; } - /// - /// Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter. - /// - /// The source matrix. - /// The output matrix. - /// True if the operation succeeded, False otherwise. - public static bool Invert(Matrix3x2 matrix, out Matrix3x2 result) + /// Creates a scale matrix from the given X and Y components. + /// Value to scale by on the X-axis. + /// Value to scale by on the Y-axis. + /// A scaling matrix. + public static Matrix3x2 CreateScale(float xScale, float yScale) { - float det = (matrix.M11 * matrix.M22) - (matrix.M21 * matrix.M12); + Matrix3x2 result = Identity; - if (MathF.Abs(det) < float.Epsilon) - { - result = new Matrix3x2(float.NaN, float.NaN, float.NaN, float.NaN, float.NaN, float.NaN); - return false; - } + result.M11 = xScale; + result.M22 = yScale; - float invDet = 1.0f / det; + return result; + } - 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; + /// Creates a scale matrix that is offset by a given center point. + /// Value to scale by on the X-axis. + /// Value to scale by on the Y-axis. + /// The center point. + /// A scaling matrix. + public static Matrix3x2 CreateScale(float xScale, float yScale, Vector2 centerPoint) + { + Matrix3x2 result = Identity; - return true; + float tx = centerPoint.X * (1 - xScale); + float ty = centerPoint.Y * (1 - yScale); + + result.M11 = xScale; + result.M22 = yScale; + result.M31 = tx; + result.M32 = ty; + + return result; } - /// - /// Linearly interpolates from matrix1 to matrix2, based on the third parameter. - /// - /// The first source matrix. - /// The second source matrix. - /// The relative weighting of matrix2. - /// The interpolated matrix. - public static Matrix3x2 Lerp(Matrix3x2 matrix1, Matrix3x2 matrix2, float amount) + /// Creates a scale matrix from the given vector scale with an offset from the given center point. + /// The scale to use. + /// The center offset. + /// A scaling matrix. + public static Matrix3x2 CreateScale(Vector2 scales, Vector2 centerPoint) { - Matrix3x2 result; - - // First row - result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; - result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; + Matrix3x2 result = Identity; - // Second row - result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; - result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; + float tx = centerPoint.X * (1 - scales.X); + float ty = centerPoint.Y * (1 - scales.Y); - // Third row - result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; - result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; + result.M11 = scales.X; + result.M22 = scales.Y; + result.M31 = tx; + result.M32 = ty; return result; } - /// - /// Negates the given matrix by multiplying all values by -1. - /// - /// The source matrix. - /// The negated matrix. - public static Matrix3x2 Negate(Matrix3x2 value) + /// Creates a scale matrix that scales uniformly with the given scale. + /// The uniform scale to use. + /// A scaling matrix. + public static Matrix3x2 CreateScale(float scale) { - Matrix3x2 result; + Matrix3x2 result = Identity; - result.M11 = -value.M11; - result.M12 = -value.M12; - result.M21 = -value.M21; - result.M22 = -value.M22; - result.M31 = -value.M31; - result.M32 = -value.M32; + result.M11 = scale; + result.M22 = scale; return result; } - /// - /// Adds each matrix element in value1 with its corresponding element in value2. - /// - /// The first source matrix. - /// The second source matrix. - /// The matrix containing the summed values. - public static Matrix3x2 Add(Matrix3x2 value1, Matrix3x2 value2) + /// Creates a scale matrix that scales uniformly with the given scale with an offset from the given center. + /// The uniform scale to use. + /// The center offset. + /// A scaling matrix. + public static Matrix3x2 CreateScale(float scale, Vector2 centerPoint) { - Matrix3x2 result; + Matrix3x2 result = Identity; - 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; + float tx = centerPoint.X * (1 - scale); + float ty = centerPoint.Y * (1 - scale); + + result.M11 = scale; + result.M22 = scale; + result.M31 = tx; + result.M32 = ty; return result; } - /// - /// Subtracts each matrix element in value2 from its corresponding element in value1. - /// - /// The first source matrix. - /// The second source matrix. - /// The matrix containing the resulting values. - public static Matrix3x2 Subtract(Matrix3x2 value1, Matrix3x2 value2) + /// Creates a skew matrix from the given angles in radians. + /// The X angle, in radians. + /// The Y angle, in radians. + /// A skew matrix. + public static Matrix3x2 CreateSkew(float radiansX, float radiansY) { - Matrix3x2 result; + Matrix3x2 result = Identity; - 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; + float xTan = MathF.Tan(radiansX); + float yTan = MathF.Tan(radiansY); + + result.M12 = yTan; + result.M21 = xTan; return result; } - /// - /// Multiplies two matrices together and returns the resulting matrix. - /// - /// The first source matrix. - /// The second source matrix. - /// The product matrix. - public static Matrix3x2 Multiply(Matrix3x2 value1, Matrix3x2 value2) + /// Creates a skew matrix from the given angles in radians and a center point. + /// The X angle, in radians. + /// The Y angle, in radians. + /// The center point. + /// A skew matrix. + public static Matrix3x2 CreateSkew(float radiansX, float radiansY, Vector2 centerPoint) { - Matrix3x2 result; + Matrix3x2 result = Identity; - // First row - result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21; - result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22; + float xTan = MathF.Tan(radiansX); + float yTan = MathF.Tan(radiansY); - // Second row - result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21; - result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22; + float tx = -centerPoint.Y * xTan; + float ty = -centerPoint.X * yTan; - // 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; + result.M12 = yTan; + result.M21 = xTan; + + result.M31 = tx; + result.M32 = ty; return result; } - /// - /// Scales all elements in a matrix by the given scalar factor. - /// - /// The source matrix. - /// The scaling value to use. - /// The resulting matrix. - public static Matrix3x2 Multiply(Matrix3x2 value1, float value2) + /// Creates a translation matrix from the given vector. + /// The translation position. + /// A translation matrix. + public static Matrix3x2 CreateTranslation(Vector2 position) { - Matrix3x2 result; + Matrix3x2 result = Identity; - 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; + result.M31 = position.X; + result.M32 = position.Y; return result; } - /// - /// Negates the given matrix by multiplying all values by -1. - /// - /// The source matrix. - /// The negated matrix. - public static Matrix3x2 operator -(Matrix3x2 value) + /// Creates a translation matrix from the given X and Y components. + /// The X position. + /// The Y position. + /// A translation matrix. + public static Matrix3x2 CreateTranslation(float xPosition, float yPosition) { - Matrix3x2 m; + Matrix3x2 result = Identity; - m.M11 = -value.M11; - m.M12 = -value.M12; - m.M21 = -value.M21; - m.M22 = -value.M22; - m.M31 = -value.M31; - m.M32 = -value.M32; + result.M31 = xPosition; + result.M32 = yPosition; - return m; + return result; } - /// - /// Adds each matrix element in value1 with its corresponding element in value2. - /// - /// The first source matrix. - /// The second source matrix. - /// The matrix containing the summed values. - public static Matrix3x2 operator +(Matrix3x2 value1, Matrix3x2 value2) + /// Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter. + /// The source matrix. + /// The output matrix. + /// True if the operation succeeded, False otherwise. + public static bool Invert(Matrix3x2 matrix, out Matrix3x2 result) { - Matrix3x2 m; + float det = (matrix.M11 * matrix.M22) - (matrix.M21 * matrix.M12); - 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; + if (MathF.Abs(det) < float.Epsilon) + { + result = new Matrix3x2(float.NaN, float.NaN, float.NaN, float.NaN, float.NaN, float.NaN); + return false; + } - return m; - } + float invDet = 1.0f / det; - /// - /// Subtracts each matrix element in value2 from its corresponding element in value1. - /// - /// The first source matrix. - /// The second source matrix. - /// The matrix containing the resulting values. - public static Matrix3x2 operator -(Matrix3x2 value1, Matrix3x2 value2) - { - Matrix3x2 m; + result.M11 = matrix.M22 * invDet; + result.M12 = -matrix.M12 * invDet; - 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; + result.M21 = -matrix.M21 * invDet; + result.M22 = matrix.M11 * invDet; - return m; + result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * invDet; + result.M32 = (matrix.M31 * matrix.M12 - matrix.M11 * matrix.M32) * invDet; + + return true; } - /// - /// Multiplies two matrices together and returns the resulting matrix. - /// - /// The first source matrix. - /// The second source matrix. - /// The product matrix. - public static Matrix3x2 operator *(Matrix3x2 value1, Matrix3x2 value2) + /// Linearly interpolates from matrix1 to matrix2, based on the third parameter. + /// The first source matrix. + /// The second source matrix. + /// The relative weighting of matrix2. + /// The interpolated matrix. + public static Matrix3x2 Lerp(Matrix3x2 matrix1, Matrix3x2 matrix2, float amount) { - Matrix3x2 m; + Matrix3x2 result; // First row - m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21; - m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22; + result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; + result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; // Second row - m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21; - m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22; + result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; + result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; // 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; + result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; + result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; - return m; + return result; } - /// - /// Scales all elements in a matrix by the given scalar factor. - /// + /// Multiplies two matrices together and returns the resulting matrix. + /// The first source matrix. + /// The second source matrix. + /// The product matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x2 Multiply(Matrix3x2 value1, Matrix3x2 value2) + { + return value1 * value2; + } + + /// Scales all elements in a matrix by the given scalar factor. /// The source matrix. /// The scaling value to use. /// The resulting matrix. - public static Matrix3x2 operator *(Matrix3x2 value1, float value2) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x2 Multiply(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 value1 * value2; + } - return m; + /// Negates the given matrix by multiplying all values by -1. + /// The source matrix. + /// The negated matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x2 Negate(Matrix3x2 value) + { + return -value; } - /// - /// Returns a boolean indicating whether the given matrices are equal. - /// + /// Subtracts each matrix element in value2 from its corresponding element in value1. /// The first source matrix. /// The second source matrix. - /// True if the matrices are equal; False otherwise. - public static bool operator ==(Matrix3x2 value1, Matrix3x2 value2) + /// The matrix containing the resulting values. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3x2 Subtract(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); + return value1 - value2; } - /// - /// Returns a boolean indicating whether the given matrices are not equal. - /// - /// The first source matrix. - /// The second source matrix. - /// True if the matrices are not equal; False if they are equal. - public static bool operator !=(Matrix3x2 value1, Matrix3x2 value2) + /// Returns a boolean indicating whether the given Object is equal to this matrix instance. + /// The Object to compare against. + /// True if the Object is equal to this matrix; False otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public override readonly bool Equals(object? obj) { - 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); + return (obj is Matrix3x2 other) && Equals(other); } - /// - /// Returns a boolean indicating whether the matrix is equal to the other given matrix. - /// + /// Returns a boolean indicating whether the matrix is equal to the other given matrix. /// The other matrix to test equality against. /// True if this matrix is equal to other; False otherwise. 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); + return this == other; } - /// - /// Returns a boolean indicating whether the given Object is equal to this matrix instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this matrix; False otherwise. - public override readonly bool Equals(object? obj) + /// Calculates the determinant for this matrix. + /// The determinant is calculated by expanding the matrix with a third column whose values are (0,0,1). + /// The determinant. + public readonly float GetDeterminant() { - if (obj is Matrix3x2 m) - { - return Equals(m); - } + // 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 false; + return (M11 * M22) - (M21 * M12); } - /// - /// Returns a String representing this matrix instance. - /// + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() + { + return HashCode.Combine(M11, M12, M21, M22, M31, M32); + } + + /// Returns a String representing this matrix instance. /// The string representation. public override readonly string ToString() { @@ -757,14 +637,5 @@ namespace System.Numerics M21, M22, M31, M32); } - - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() - { - return HashCode.Combine(M11, M12, M21, M22, M31, M32); - } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.cs index cfd16ad..8a35020 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.cs @@ -1,19 +1,16 @@ // Licensed to the .NET Foundation under one or more agreements. // The .NET Foundation licenses this file to you under the MIT license. + using Internal.Runtime.CompilerServices; -using System.Diagnostics; using System.Globalization; using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.Arm; using System.Runtime.Intrinsics.X86; namespace System.Numerics { - /// - /// A structure encapsulating a 4x4 matrix. - /// + /// A structure encapsulating a 4x4 matrix. [Intrinsic] public struct Matrix4x4 : IEquatable { @@ -21,156 +18,90 @@ namespace System.Numerics private const float BillboardMinAngle = 1.0f - (0.1f * (MathF.PI / 180.0f)); // 0.1 degrees private const float DecomposeEpsilon = 0.0001f; - #region Public Fields - /// - /// Value at row 1, column 1 of the matrix. - /// + private static readonly Matrix4x4 _identity = new Matrix4x4 + ( + 1f, 0f, 0f, 0f, + 0f, 1f, 0f, 0f, + 0f, 0f, 1f, 0f, + 0f, 0f, 0f, 1f + ); + + /// Value at row 1, column 1 of the matrix. public float M11; - /// - /// Value at row 1, column 2 of the matrix. - /// + + /// Value at row 1, column 2 of the matrix. public float M12; - /// - /// Value at row 1, column 3 of the matrix. - /// + + /// Value at row 1, column 3 of the matrix. public float M13; - /// - /// Value at row 1, column 4 of the matrix. - /// + + /// Value at row 1, column 4 of the matrix. public float M14; - /// - /// Value at row 2, column 1 of the matrix. - /// + /// Value at row 2, column 1 of the matrix. public float M21; - /// - /// Value at row 2, column 2 of the matrix. - /// + + /// Value at row 2, column 2 of the matrix. public float M22; - /// - /// Value at row 2, column 3 of the matrix. - /// + + /// Value at row 2, column 3 of the matrix. public float M23; - /// - /// Value at row 2, column 4 of the matrix. - /// + + /// Value at row 2, column 4 of the matrix. public float M24; - /// - /// Value at row 3, column 1 of the matrix. - /// + /// Value at row 3, column 1 of the matrix. public float M31; - /// - /// Value at row 3, column 2 of the matrix. - /// + + /// Value at row 3, column 2 of the matrix. public float M32; - /// - /// Value at row 3, column 3 of the matrix. - /// + + /// Value at row 3, column 3 of the matrix. public float M33; - /// - /// Value at row 3, column 4 of the matrix. - /// + + /// Value at row 3, column 4 of the matrix. public float M34; - /// - /// Value at row 4, column 1 of the matrix. - /// + /// Value at row 4, column 1 of the matrix. public float M41; - /// - /// Value at row 4, column 2 of the matrix. - /// - public float M42; - /// - /// Value at row 4, column 3 of the matrix. - /// - public float M43; - /// - /// Value at row 4, column 4 of the matrix. - /// - 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 - ); - /// - /// Returns the multiplicative identity matrix. - /// - public static Matrix4x4 Identity - { - get { return _identity; } - } + /// Value at row 4, column 2 of the matrix. + public float M42; - /// - /// Returns whether the matrix is the identity matrix. - /// - 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; - } - } + /// Value at row 4, column 3 of the matrix. + public float M43; - /// - /// Gets or sets the translation component of this matrix. - /// - public Vector3 Translation - { - readonly get - { - return new Vector3(M41, M42, M43); - } - set - { - M41 = value.X; - M42 = value.Y; - M43 = value.Z; - } - } + /// Value at row 4, column 4 of the matrix. + public float M44; - /// - /// Constructs a Matrix4x4 from the given components. - /// + /// Constructs a Matrix4x4 from the given components. 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; + M11 = m11; + M12 = m12; + M13 = m13; + M14 = m14; + + M21 = m21; + M22 = m22; + M23 = m23; + M24 = m24; + + M31 = m31; + M32 = m32; + M33 = m33; + M34 = m34; + + M41 = m41; + M42 = m42; + M43 = m43; + M44 = m44; } - /// - /// Constructs a Matrix4x4 from the given Matrix3x2. - /// + /// Constructs a Matrix4x4 from the given Matrix3x2. /// The source Matrix3x2. public Matrix4x4(Matrix3x2 value) { @@ -178,468 +109,541 @@ namespace System.Numerics 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; } - /// - /// Creates a spherical billboard that rotates around a specified object position. - /// - /// Position of the object the billboard will rotate around. - /// Position of the camera. - /// The up vector of the camera. - /// The forward vector of the camera. - /// The created billboard matrix - public static Matrix4x4 CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraUpVector, Vector3 cameraForwardVector) + /// Returns the multiplicative identity matrix. + public static Matrix4x4 Identity { - Vector3 zaxis = new Vector3( - objectPosition.X - cameraPosition.X, - objectPosition.Y - cameraPosition.Y, - objectPosition.Z - cameraPosition.Z); - - float norm = zaxis.LengthSquared(); + get => _identity; + } - if (norm < BillboardEpsilon) - { - zaxis = -cameraForwardVector; - } - else + /// Returns whether the matrix is the identity matrix. + public readonly bool IsIdentity + { + get { - zaxis = Vector3.Multiply(zaxis, 1.0f / MathF.Sqrt(norm)); + 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; } + } - Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis)); + /// Gets or sets the translation component of this matrix. + public Vector3 Translation + { + readonly get => new Vector3(M41, M42, M43); - Vector3 yaxis = Vector3.Cross(zaxis, xaxis); + set + { + M41 = value.X; + M42 = value.Y; + M43 = value.Z; + } + } - Matrix4x4 result; + /// Adds two matrices together. + /// The first source matrix. + /// The second source matrix. + /// The resulting matrix. + public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) + { + if (AdvSimd.IsSupported) + { + AdvSimd.Store(&value1.M11, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11))); + AdvSimd.Store(&value1.M21, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21))); + AdvSimd.Store(&value1.M31, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31))); + AdvSimd.Store(&value1.M41, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41))); + return value1; + } + else 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; + } - 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; + Matrix4x4 m; - result.M41 = objectPosition.X; - result.M42 = objectPosition.Y; - result.M43 = objectPosition.Z; - result.M44 = 1.0f; + 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 result; + return m; } - /// - /// Creates a cylindrical billboard that rotates around a specified axis. - /// - /// Position of the object the billboard will rotate around. - /// Position of the camera. - /// Axis to rotate the billboard around. - /// Forward vector of the camera. - /// Forward vector of the object. - /// The created billboard matrix. - public static Matrix4x4 CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 rotateAxis, Vector3 cameraForwardVector, Vector3 objectForwardVector) + /// Returns a boolean indicating whether the given two matrices are equal. + /// The first matrix to compare. + /// The second matrix to compare. + /// True if the given matrices are equal; False otherwise. + public static unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2) { - // 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) + if (AdvSimd.Arm64.IsSupported) { - faceDir = -cameraForwardVector; + return VectorMath.Equal(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11)) && + VectorMath.Equal(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21)) && + VectorMath.Equal(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31)) && + VectorMath.Equal(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41)); } - else + else if (Sse.IsSupported) { - faceDir = Vector3.Multiply(faceDir, (1.0f / MathF.Sqrt(norm))); + 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)); } - 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); + 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); + } - if (MathF.Abs(dot) > BillboardMinAngle) + /// Returns a boolean indicating whether the given two matrices are not equal. + /// The first matrix to compare. + /// The second matrix to compare. + /// True if the given matrices are not equal; False if they are equal. + public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + { + if (AdvSimd.Arm64.IsSupported) { - 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)); + return VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11)) || + VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21)) || + VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31)) || + VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41)); } - else + else if (Sse.IsSupported) { - xaxis = Vector3.Normalize(Vector3.Cross(rotateAxis, faceDir)); - zaxis = Vector3.Normalize(Vector3.Cross(xaxis, yaxis)); + 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)); } - Matrix4x4 result; + 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); + } - 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; + /// Multiplies a matrix by another matrix. + /// The first source matrix. + /// The second source matrix. + /// The result of the multiplication. + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) + { + if (AdvSimd.Arm64.IsSupported) + { + Unsafe.SkipInit(out Matrix4x4 result); - result.M41 = objectPosition.X; - result.M42 = objectPosition.Y; - result.M43 = objectPosition.Z; - result.M44 = 1.0f; + // Perform the operation on the first row - return result; - } + Vector128 M11 = AdvSimd.LoadVector128(&value1.M11); - /// - /// Creates a translation matrix. - /// - /// The amount to translate in each axis. - /// The translation matrix. - public static Matrix4x4 CreateTranslation(Vector3 position) - { - Matrix4x4 result = _identity; - result.M41 = position.X; - result.M42 = position.Y; - result.M43 = position.Z; - return result; - } + Vector128 vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M11, 0); + Vector128 vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M11, 1); + Vector128 vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M11, 2); + Vector128 vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M11, 3); - /// - /// Creates a translation matrix. - /// - /// The amount to translate on the X-axis. - /// The amount to translate on the Y-axis. - /// The amount to translate on the Z-axis. - /// The translation matrix. - public static Matrix4x4 CreateTranslation(float xPosition, float yPosition, float zPosition) - { - Matrix4x4 result = _identity; - result.M41 = xPosition; - result.M42 = yPosition; - result.M43 = zPosition; - return result; - } + AdvSimd.Store(&result.M11, AdvSimd.Add(vZ, vW)); - /// - /// Creates a scaling matrix. - /// - /// Value to scale by on the X-axis. - /// Value to scale by on the Y-axis. - /// Value to scale by on the Z-axis. - /// The scaling matrix. - public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale) - { - Matrix4x4 result = _identity; - result.M11 = xScale; - result.M22 = yScale; - result.M33 = zScale; - return result; - } + // Repeat for the other 3 rows - /// - /// Creates a scaling matrix with a center point. - /// - /// Value to scale by on the X-axis. - /// Value to scale by on the Y-axis. - /// Value to scale by on the Z-axis. - /// The center point. - /// The scaling matrix. - public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale, Vector3 centerPoint) - { - Matrix4x4 result = _identity; + Vector128 M21 = AdvSimd.LoadVector128(&value1.M21); - float tx = centerPoint.X * (1 - xScale); - float ty = centerPoint.Y * (1 - yScale); - float tz = centerPoint.Z * (1 - zScale); + vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M21, 0); + vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M21, 1); + vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M21, 2); + vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M21, 3); - result.M11 = xScale; - result.M22 = yScale; - result.M33 = zScale; - result.M41 = tx; - result.M42 = ty; - result.M43 = tz; - return result; - } + AdvSimd.Store(&result.M21, AdvSimd.Add(vZ, vW)); - /// - /// Creates a scaling matrix. - /// - /// The vector containing the amount to scale by on each axis. - /// The scaling matrix. - public static Matrix4x4 CreateScale(Vector3 scales) - { - Matrix4x4 result = _identity; - result.M11 = scales.X; - result.M22 = scales.Y; - result.M33 = scales.Z; - return result; - } + Vector128 M31 = AdvSimd.LoadVector128(&value1.M31); - /// - /// Creates a scaling matrix with a center point. - /// - /// The vector containing the amount to scale by on each axis. - /// The center point. - /// The scaling matrix. - public static Matrix4x4 CreateScale(Vector3 scales, Vector3 centerPoint) - { - Matrix4x4 result = _identity; + vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M31, 0); + vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M31, 1); + vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M31, 2); + vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M31, 3); - float tx = centerPoint.X * (1 - scales.X); - float ty = centerPoint.Y * (1 - scales.Y); - float tz = centerPoint.Z * (1 - scales.Z); + AdvSimd.Store(&result.M31, AdvSimd.Add(vZ, vW)); - result.M11 = scales.X; - result.M22 = scales.Y; - result.M33 = scales.Z; - result.M41 = tx; - result.M42 = ty; - result.M43 = tz; - return result; - } + Vector128 M41 = AdvSimd.LoadVector128(&value1.M41); - /// - /// Creates a uniform scaling matrix that scales equally on each axis. - /// - /// The uniform scaling factor. - /// The scaling matrix. - public static Matrix4x4 CreateScale(float scale) - { - Matrix4x4 result = _identity; + vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M41, 0); + vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M41, 1); + vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M41, 2); + vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M41, 3); - result.M11 = scale; - result.M22 = scale; - result.M33 = scale; + AdvSimd.Store(&result.M41, AdvSimd.Add(vZ, vW)); - return result; - } + return result; + } + else if (Sse.IsSupported) + { + Vector128 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))))); - /// - /// Creates a uniform scaling matrix that scales equally on each axis with a center point. - /// - /// The uniform scaling factor. - /// The center point. - /// The scaling matrix. - public static Matrix4x4 CreateScale(float scale, Vector3 centerPoint) - { - Matrix4x4 result = _identity; + // 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. - float tx = centerPoint.X * (1 - scale); - float ty = centerPoint.Y * (1 - scale); - float tz = centerPoint.Z * (1 - scale); + 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))))); - result.M11 = scale; - result.M22 = scale; - result.M33 = scale; + 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))))); - result.M41 = tx; - result.M42 = ty; - result.M43 = tz; + 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; + } - return result; - } + Matrix4x4 m; - /// - /// Creates a matrix for rotating points around the X-axis. - /// - /// The amount, in radians, by which to rotate around the X-axis. - /// The rotation matrix. - public static Matrix4x4 CreateRotationX(float radians) - { - Matrix4x4 result = _identity; + // 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; - float c = MathF.Cos(radians); - float s = MathF.Sin(radians); + // 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; - // [ 1 0 0 0 ] - // [ 0 c s 0 ] - // [ 0 -s c 0 ] - // [ 0 0 0 1 ] + // 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; - result.M22 = c; - result.M23 = s; - result.M32 = -s; - result.M33 = c; + // 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 result; + return m; } - /// - /// Creates a matrix for rotating points around the X-axis, from a center point. - /// - /// The amount, in radians, by which to rotate around the X-axis. - /// The center point. - /// The rotation matrix. - public static Matrix4x4 CreateRotationX(float radians, Vector3 centerPoint) + /// Multiplies a matrix by a scalar value. + /// The source matrix. + /// The scaling factor. + /// The scaled matrix. + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) { - Matrix4x4 result = _identity; - - 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; + if (AdvSimd.IsSupported) + { + Vector128 value2Vec = Vector128.Create(value2); + AdvSimd.Store(&value1.M11, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M11), value2Vec)); + AdvSimd.Store(&value1.M21, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M21), value2Vec)); + AdvSimd.Store(&value1.M31, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M31), value2Vec)); + AdvSimd.Store(&value1.M41, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M41), value2Vec)); + return value1; + } + else if (Sse.IsSupported) + { + Vector128 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; + } - // [ 1 0 0 0 ] - // [ 0 c s 0 ] - // [ 0 -s c 0 ] - // [ 0 y z 1 ] + Matrix4x4 m; - result.M22 = c; - result.M23 = s; - result.M32 = -s; - result.M33 = c; - result.M42 = y; - result.M43 = z; + 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; + } - return result; - } - - /// - /// Creates a matrix for rotating points around the Y-axis. - /// - /// The amount, in radians, by which to rotate around the Y-axis. - /// The rotation matrix. - public static Matrix4x4 CreateRotationY(float radians) + /// Subtracts the second matrix from the first. + /// The first source matrix. + /// The second source matrix. + /// The result of the subtraction. + public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 result = _identity; + if (AdvSimd.IsSupported) + { + AdvSimd.Store(&value1.M11, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11))); + AdvSimd.Store(&value1.M21, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21))); + AdvSimd.Store(&value1.M31, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31))); + AdvSimd.Store(&value1.M41, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41))); + return value1; + } + else 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; + } - float c = MathF.Cos(radians); - float s = MathF.Sin(radians); + Matrix4x4 m; - // [ c 0 -s 0 ] - // [ 0 1 0 0 ] - // [ s 0 c 0 ] - // [ 0 0 0 1 ] - result.M11 = c; - result.M13 = -s; - result.M31 = s; - result.M33 = c; + 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 result; + return m; } - /// - /// Creates a matrix for rotating points around the Y-axis, from a center point. - /// - /// The amount, in radians, by which to rotate around the Y-axis. - /// The center point. - /// The rotation matrix. - public static Matrix4x4 CreateRotationY(float radians, Vector3 centerPoint) + /// Returns a new matrix with the negated elements of the given matrix. + /// The source matrix. + /// The negated matrix. + public static unsafe Matrix4x4 operator -(Matrix4x4 value) { - Matrix4x4 result = _identity; + if (AdvSimd.IsSupported) + { + AdvSimd.Store(&value.M11, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M11))); + AdvSimd.Store(&value.M21, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M21))); + AdvSimd.Store(&value.M31, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M31))); + AdvSimd.Store(&value.M41, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M41))); + return value; + } + else if (Sse.IsSupported) + { + Vector128 zero = Vector128.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; + } - float c = MathF.Cos(radians); - float s = MathF.Sin(radians); + Matrix4x4 m; - float x = centerPoint.X * (1 - c) - centerPoint.Z * s; - float z = centerPoint.Z * (1 - c) + centerPoint.X * s; + 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; - // [ c 0 -s 0 ] - // [ 0 1 0 0 ] - // [ s 0 c 0 ] - // [ x 0 z 1 ] - result.M11 = c; - result.M13 = -s; - result.M31 = s; - result.M33 = c; - result.M41 = x; - result.M43 = z; + return m; + } - return result; + /// Adds two matrices together. + /// The first source matrix. + /// The second source matrix. + /// The resulting matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) + { + return value1 + value2; } - /// - /// Creates a matrix for rotating points around the Z-axis. - /// - /// The amount, in radians, by which to rotate around the Z-axis. - /// The rotation matrix. - public static Matrix4x4 CreateRotationZ(float radians) + /// Creates a spherical billboard that rotates around a specified object position. + /// Position of the object the billboard will rotate around. + /// Position of the camera. + /// The up vector of the camera. + /// The forward vector of the camera. + /// The created billboard matrix + public static Matrix4x4 CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraUpVector, Vector3 cameraForwardVector) { - Matrix4x4 result = _identity; + Vector3 zaxis = objectPosition - cameraPosition; + float norm = zaxis.LengthSquared(); - float c = MathF.Cos(radians); - float s = MathF.Sin(radians); + if (norm < BillboardEpsilon) + { + zaxis = -cameraForwardVector; + } + else + { + zaxis = Vector3.Multiply(zaxis, 1.0f / MathF.Sqrt(norm)); + } - // [ c s 0 0 ] - // [ -s c 0 0 ] - // [ 0 0 1 0 ] - // [ 0 0 0 1 ] - result.M11 = c; - result.M12 = s; - result.M21 = -s; - result.M22 = c; + 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; } - /// - /// Creates a matrix for rotating points around the Z-axis, from a center point. - /// - /// The amount, in radians, by which to rotate around the Z-axis. - /// The center point. - /// The rotation matrix. - public static Matrix4x4 CreateRotationZ(float radians, Vector3 centerPoint) + /// Creates a cylindrical billboard that rotates around a specified axis. + /// Position of the object the billboard will rotate around. + /// Position of the camera. + /// Axis to rotate the billboard around. + /// Forward vector of the camera. + /// Forward vector of the object. + /// The created billboard matrix. + public static Matrix4x4 CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 rotateAxis, Vector3 cameraForwardVector, Vector3 objectForwardVector) { - Matrix4x4 result = _identity; + // Treat the case when object and camera positions are too close. + Vector3 faceDir = objectPosition - cameraPosition; + float norm = faceDir.LengthSquared(); - float c = MathF.Cos(radians); - float s = MathF.Sin(radians); + if (norm < BillboardEpsilon) + { + faceDir = -cameraForwardVector; + } + else + { + faceDir = Vector3.Multiply(faceDir, (1.0f / MathF.Sqrt(norm))); + } - float x = centerPoint.X * (1 - c) + centerPoint.Y * s; - float y = centerPoint.Y * (1 - c) - centerPoint.X * s; + Vector3 yaxis = rotateAxis; + Vector3 xaxis; + Vector3 zaxis; - // [ c s 0 0 ] - // [ -s c 0 0 ] - // [ 0 0 1 0 ] - // [ x y 0 1 ] - result.M11 = c; - result.M12 = s; - result.M21 = -s; - result.M22 = c; - result.M41 = x; - result.M42 = y; + // 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; } - /// - /// Creates a matrix that rotates around an arbitrary vector. - /// + /// Creates a matrix that rotates around an arbitrary vector. /// The axis to rotate around. /// The angle to rotate around the given axis, in radians. /// The rotation matrix. @@ -675,7 +679,7 @@ namespace System.Numerics float xx = x * x, yy = y * y, zz = z * z; float xy = x * y, xz = x * z, yz = y * z; - Matrix4x4 result = _identity; + Matrix4x4 result = Identity; result.M11 = xx + ca * (1.0f - xx); result.M12 = xy - ca * xy + sa * z; @@ -692,19 +696,133 @@ namespace System.Numerics return result; } - /// - /// Creates a perspective projection matrix based on a field of view, aspect ratio, and near and far view plane distances. - /// - /// Field of view in the y direction, in radians. - /// Aspect ratio, defined as view space width divided by height. - /// Distance to the near view plane. + /// Creates a rotation matrix from the given Quaternion rotation value. + /// The source Quaternion. + /// The rotation matrix. + public static Matrix4x4 CreateFromQuaternion(Quaternion quaternion) + { + Matrix4x4 result = Identity; + + 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.M21 = 2.0f * (xy - wz); + result.M22 = 1.0f - 2.0f * (zz + xx); + result.M23 = 2.0f * (yz + wx); + + result.M31 = 2.0f * (xz + wy); + result.M32 = 2.0f * (yz - wx); + result.M33 = 1.0f - 2.0f * (yy + xx); + + return result; + } + + /// Creates a rotation matrix from the specified yaw, pitch, and roll. + /// Angle of rotation, in radians, around the Y-axis. + /// Angle of rotation, in radians, around the X-axis. + /// Angle of rotation, in radians, around the Z-axis. + /// The rotation matrix. + public static Matrix4x4 CreateFromYawPitchRoll(float yaw, float pitch, float roll) + { + Quaternion q = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll); + return CreateFromQuaternion(q); + } + + /// Creates a view matrix. + /// The position of the camera. + /// The target towards which the camera is pointing. + /// The direction that is "up" from the camera's point of view. + /// The view matrix. + 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 = Identity; + + result.M11 = xaxis.X; + result.M12 = yaxis.X; + result.M13 = zaxis.X; + + result.M21 = xaxis.Y; + result.M22 = yaxis.Y; + result.M23 = zaxis.Y; + + result.M31 = xaxis.Z; + result.M32 = yaxis.Z; + result.M33 = zaxis.Z; + + result.M41 = -Vector3.Dot(xaxis, cameraPosition); + result.M42 = -Vector3.Dot(yaxis, cameraPosition); + result.M43 = -Vector3.Dot(zaxis, cameraPosition); + + return result; + } + + /// Creates an orthographic perspective matrix from the given view volume dimensions. + /// Width of the view volume. + /// Height of the view volume. + /// Minimum Z-value of the view volume. + /// Maximum Z-value of the view volume. + /// The orthographic projection matrix. + public static Matrix4x4 CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) + { + Matrix4x4 result = Identity; + + result.M11 = 2.0f / width; + result.M22 = 2.0f / height; + result.M33 = 1.0f / (zNearPlane - zFarPlane); + result.M43 = zNearPlane / (zNearPlane - zFarPlane); + + return result; + } + + /// Builds a customized, orthographic projection matrix. + /// Minimum X-value of the view volume. + /// Maximum X-value of the view volume. + /// Minimum Y-value of the view volume. + /// Maximum Y-value of the view volume. + /// Minimum Z-value of the view volume. + /// Maximum Z-value of the view volume. + /// The orthographic projection matrix. + public static Matrix4x4 CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) + { + Matrix4x4 result = Identity; + + result.M11 = 2.0f / (right - left); + + result.M22 = 2.0f / (top - bottom); + + result.M33 = 1.0f / (zNearPlane - zFarPlane); + + result.M41 = (left + right) / (left - right); + result.M42 = (top + bottom) / (bottom - top); + result.M43 = zNearPlane / (zNearPlane - zFarPlane); + + return result; + } + + /// Creates a perspective projection matrix from the given view volume dimensions. + /// Width of the view volume at the near view plane. + /// Height of the view volume at the near view plane. + /// Distance to the near view plane. /// Distance to the far view plane. /// The perspective projection matrix. - public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance) + public static Matrix4x4 CreatePerspective(float width, float height, 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)); @@ -714,20 +832,17 @@ namespace System.Numerics 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.M11 = 2.0f * nearPlaneDistance / width; result.M12 = result.M13 = result.M14 = 0.0f; - result.M22 = yScale; + result.M22 = 2.0f * nearPlaneDistance / height; result.M21 = result.M23 = result.M24 = 0.0f; - result.M31 = result.M32 = 0.0f; float 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; @@ -736,16 +851,17 @@ namespace System.Numerics return result; } - /// - /// Creates a perspective projection matrix from the given view volume dimensions. - /// - /// Width of the view volume at the near view plane. - /// Height of the view volume at the near view plane. + /// Creates a perspective projection matrix based on a field of view, aspect ratio, and near and far view plane distances. + /// Field of view in the y direction, in radians. + /// Aspect ratio, defined as view space width divided by height. /// Distance to the near view plane. /// Distance to the far view plane. /// The perspective projection matrix. - public static Matrix4x4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance) + 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)); @@ -755,17 +871,20 @@ namespace System.Numerics 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 = 2.0f * nearPlaneDistance / width; + result.M11 = xScale; result.M12 = result.M13 = result.M14 = 0.0f; - result.M22 = 2.0f * nearPlaneDistance / height; + result.M22 = yScale; result.M21 = result.M23 = result.M24 = 0.0f; + result.M31 = result.M32 = 0.0f; float 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; @@ -774,9 +893,7 @@ namespace System.Numerics return result; } - /// - /// Creates a customized, perspective projection matrix. - /// + /// Creates a customized, perspective projection matrix. /// Minimum x-value of the view volume at the near view plane. /// Maximum x-value of the view volume at the near view plane. /// Minimum y-value of the view volume at the near view plane. @@ -815,173 +932,302 @@ namespace System.Numerics return result; } - /// - /// Creates an orthographic perspective matrix from the given view volume dimensions. - /// - /// Width of the view volume. - /// Height of the view volume. - /// Minimum Z-value of the view volume. - /// Maximum Z-value of the view volume. - /// The orthographic projection matrix. - public static Matrix4x4 CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) + /// Creates a Matrix that reflects the coordinate system about a specified Plane. + /// The Plane about which to create a reflection. + /// A new matrix expressing the reflection. + public static Matrix4x4 CreateReflection(Plane value) { - Matrix4x4 result = _identity; + value = Plane.Normalize(value); - result.M11 = 2.0f / width; - result.M22 = 2.0f / height; - result.M33 = 1.0f / (zNearPlane - zFarPlane); - result.M43 = zNearPlane / (zNearPlane - zFarPlane); + float a = value.Normal.X; + float b = value.Normal.Y; + float c = value.Normal.Z; - return result; - } + float fa = -2.0f * a; + float fb = -2.0f * b; + float fc = -2.0f * c; - /// - /// Builds a customized, orthographic projection matrix. - /// - /// Minimum X-value of the view volume. - /// Maximum X-value of the view volume. - /// Minimum Y-value of the view volume. - /// Maximum Y-value of the view volume. - /// Minimum Z-value of the view volume. - /// Maximum Z-value of the view volume. - /// The orthographic projection matrix. - public static Matrix4x4 CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) - { - Matrix4x4 result = _identity; + Matrix4x4 result = Identity; - result.M11 = 2.0f / (right - left); + result.M11 = fa * a + 1.0f; + result.M12 = fb * a; + result.M13 = fc * a; - result.M22 = 2.0f / (top - bottom); + result.M21 = fa * b; + result.M22 = fb * b + 1.0f; + result.M23 = fc * b; - result.M33 = 1.0f / (zNearPlane - zFarPlane); + result.M31 = fa * c; + result.M32 = fb * c; + result.M33 = fc * c + 1.0f; - result.M41 = (left + right) / (left - right); - result.M42 = (top + bottom) / (bottom - top); - result.M43 = zNearPlane / (zNearPlane - zFarPlane); + result.M41 = fa * value.D; + result.M42 = fb * value.D; + result.M43 = fc * value.D; return result; } - /// - /// Creates a view matrix. - /// - /// The position of the camera. - /// The target towards which the camera is pointing. - /// The direction that is "up" from the camera's point of view. - /// The view matrix. - public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) + /// Creates a matrix for rotating points around the X-axis. + /// The amount, in radians, by which to rotate around the X-axis. + /// The rotation matrix. + public static Matrix4x4 CreateRotationX(float radians) { - Vector3 zaxis = Vector3.Normalize(cameraPosition - cameraTarget); - Vector3 xaxis = Vector3.Normalize(Vector3.Cross(cameraUpVector, zaxis)); - Vector3 yaxis = Vector3.Cross(zaxis, xaxis); - - Matrix4x4 result = _identity; - - result.M11 = xaxis.X; - result.M12 = yaxis.X; - result.M13 = zaxis.X; + Matrix4x4 result = Identity; - result.M21 = xaxis.Y; - result.M22 = yaxis.Y; - result.M23 = zaxis.Y; + float c = MathF.Cos(radians); + float s = MathF.Sin(radians); - result.M31 = xaxis.Z; - result.M32 = yaxis.Z; - result.M33 = zaxis.Z; + // [ 1 0 0 0 ] + // [ 0 c s 0 ] + // [ 0 -s c 0 ] + // [ 0 0 0 1 ] - result.M41 = -Vector3.Dot(xaxis, cameraPosition); - result.M42 = -Vector3.Dot(yaxis, cameraPosition); - result.M43 = -Vector3.Dot(zaxis, cameraPosition); + result.M22 = c; + result.M23 = s; + result.M32 = -s; + result.M33 = c; return result; } - /// - /// Creates a world matrix with the specified parameters. - /// - /// The position of the object; used in translation operations. - /// Forward direction of the object. - /// Upward direction of the object; usually [0, 1, 0]. - /// The world matrix. - public static Matrix4x4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up) + /// Creates a matrix for rotating points around the X-axis, from a center point. + /// The amount, in radians, by which to rotate around the X-axis. + /// The center point. + /// The rotation matrix. + public static Matrix4x4 CreateRotationX(float radians, Vector3 centerPoint) { - Vector3 zaxis = Vector3.Normalize(-forward); - Vector3 xaxis = Vector3.Normalize(Vector3.Cross(up, zaxis)); - Vector3 yaxis = Vector3.Cross(zaxis, xaxis); + Matrix4x4 result = Identity; - Matrix4x4 result = _identity; + float c = MathF.Cos(radians); + float s = MathF.Sin(radians); - result.M11 = xaxis.X; - result.M12 = xaxis.Y; - result.M13 = xaxis.Z; + float y = centerPoint.Y * (1 - c) + centerPoint.Z * s; + float z = centerPoint.Z * (1 - c) - centerPoint.Y * s; - result.M21 = yaxis.X; - result.M22 = yaxis.Y; - result.M23 = yaxis.Z; + // [ 1 0 0 0 ] + // [ 0 c s 0 ] + // [ 0 -s c 0 ] + // [ 0 y z 1 ] - result.M31 = zaxis.X; - result.M32 = zaxis.Y; - result.M33 = zaxis.Z; + result.M22 = c; + result.M23 = s; + result.M32 = -s; + result.M33 = c; + result.M42 = y; + result.M43 = z; - result.M41 = position.X; - result.M42 = position.Y; - result.M43 = position.Z; + return result; + } + + /// Creates a matrix for rotating points around the Y-axis. + /// The amount, in radians, by which to rotate around the Y-axis. + /// The rotation matrix. + public static Matrix4x4 CreateRotationY(float radians) + { + Matrix4x4 result = Identity; + + 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.M13 = -s; + result.M31 = s; + result.M33 = c; return result; } - /// - /// Creates a rotation matrix from the given Quaternion rotation value. - /// - /// The source Quaternion. + /// Creates a matrix for rotating points around the Y-axis, from a center point. + /// The amount, in radians, by which to rotate around the Y-axis. + /// The center point. /// The rotation matrix. - public static Matrix4x4 CreateFromQuaternion(Quaternion quaternion) + public static Matrix4x4 CreateRotationY(float radians, Vector3 centerPoint) { - Matrix4x4 result = _identity; + Matrix4x4 result = Identity; - float xx = quaternion.X * quaternion.X; - float yy = quaternion.Y * quaternion.Y; - float zz = quaternion.Z * quaternion.Z; + float c = MathF.Cos(radians); + float s = MathF.Sin(radians); - 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; + float x = centerPoint.X * (1 - c) - centerPoint.Z * s; + float z = centerPoint.Z * (1 - c) + centerPoint.X * s; - result.M11 = 1.0f - 2.0f * (yy + zz); - result.M12 = 2.0f * (xy + wz); - result.M13 = 2.0f * (xz - wy); + // [ c 0 -s 0 ] + // [ 0 1 0 0 ] + // [ s 0 c 0 ] + // [ x 0 z 1 ] + result.M11 = c; + result.M13 = -s; + result.M31 = s; + result.M33 = c; + result.M41 = x; + result.M43 = z; - result.M21 = 2.0f * (xy - wz); - result.M22 = 1.0f - 2.0f * (zz + xx); - result.M23 = 2.0f * (yz + wx); + return result; + } - result.M31 = 2.0f * (xz + wy); - result.M32 = 2.0f * (yz - wx); - result.M33 = 1.0f - 2.0f * (yy + xx); + /// Creates a matrix for rotating points around the Z-axis. + /// The amount, in radians, by which to rotate around the Z-axis. + /// The rotation matrix. + public static Matrix4x4 CreateRotationZ(float radians) + { + Matrix4x4 result = Identity; + + 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.M21 = -s; + result.M22 = c; return result; } - /// - /// Creates a rotation matrix from the specified yaw, pitch, and roll. - /// - /// Angle of rotation, in radians, around the Y-axis. - /// Angle of rotation, in radians, around the X-axis. - /// Angle of rotation, in radians, around the Z-axis. + /// Creates a matrix for rotating points around the Z-axis, from a center point. + /// The amount, in radians, by which to rotate around the Z-axis. + /// The center point. /// The rotation matrix. - public static Matrix4x4 CreateFromYawPitchRoll(float yaw, float pitch, float roll) + public static Matrix4x4 CreateRotationZ(float radians, Vector3 centerPoint) { - Quaternion q = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll); + Matrix4x4 result = Identity; + + 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.M21 = -s; + result.M22 = c; + result.M41 = x; + result.M42 = y; + + return result; + } + + /// Creates a scaling matrix. + /// Value to scale by on the X-axis. + /// Value to scale by on the Y-axis. + /// Value to scale by on the Z-axis. + /// The scaling matrix. + public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale) + { + Matrix4x4 result = Identity; + result.M11 = xScale; + result.M22 = yScale; + result.M33 = zScale; + return result; + } + + /// Creates a scaling matrix with a center point. + /// Value to scale by on the X-axis. + /// Value to scale by on the Y-axis. + /// Value to scale by on the Z-axis. + /// The center point. + /// The scaling matrix. + public static Matrix4x4 CreateScale(float xScale, float yScale, float zScale, Vector3 centerPoint) + { + Matrix4x4 result = Identity; + + float tx = centerPoint.X * (1 - xScale); + float ty = centerPoint.Y * (1 - yScale); + float tz = centerPoint.Z * (1 - zScale); + + result.M11 = xScale; + result.M22 = yScale; + result.M33 = zScale; + result.M41 = tx; + result.M42 = ty; + result.M43 = tz; + return result; + } + + /// Creates a scaling matrix. + /// The vector containing the amount to scale by on each axis. + /// The scaling matrix. + public static Matrix4x4 CreateScale(Vector3 scales) + { + Matrix4x4 result = Identity; + result.M11 = scales.X; + result.M22 = scales.Y; + result.M33 = scales.Z; + return result; + } + + /// Creates a scaling matrix with a center point. + /// The vector containing the amount to scale by on each axis. + /// The center point. + /// The scaling matrix. + public static Matrix4x4 CreateScale(Vector3 scales, Vector3 centerPoint) + { + Matrix4x4 result = Identity; + + 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.M22 = scales.Y; + result.M33 = scales.Z; + result.M41 = tx; + result.M42 = ty; + result.M43 = tz; + return result; + } + + /// Creates a uniform scaling matrix that scales equally on each axis. + /// The uniform scaling factor. + /// The scaling matrix. + public static Matrix4x4 CreateScale(float scale) + { + Matrix4x4 result = Identity; + + result.M11 = scale; + result.M22 = scale; + result.M33 = scale; + + return result; + } + + /// Creates a uniform scaling matrix that scales equally on each axis with a center point. + /// The uniform scaling factor. + /// The center point. + /// The scaling matrix. + public static Matrix4x4 CreateScale(float scale, Vector3 centerPoint) + { + Matrix4x4 result = Identity; - return Matrix4x4.CreateFromQuaternion(q); + float tx = centerPoint.X * (1 - scale); + float ty = centerPoint.Y * (1 - scale); + float tz = centerPoint.Z * (1 - scale); + + result.M11 = scale; + result.M22 = scale; + result.M33 = scale; + + result.M41 = tx; + result.M42 = ty; + result.M43 = tz; + + return result; } - /// - /// Creates a Matrix that flattens geometry into a specified Plane as if casting a shadow from a specified light source. - /// + /// Creates a Matrix that flattens geometry into a specified Plane as if casting a shadow from a specified light source. /// The direction from which the light that will cast the shadow is coming. /// The Plane onto which the new matrix should flatten geometry so as to cast a shadow. /// A new Matrix that can be used to flatten geometry onto the specified plane from the specified direction. @@ -995,7 +1241,7 @@ namespace System.Numerics float c = -p.Normal.Z; float d = -p.D; - Matrix4x4 result = _identity; + Matrix4x4 result = Identity; result.M11 = a * lightDirection.X + dot; result.M21 = b * lightDirection.X; @@ -1017,117 +1263,65 @@ namespace System.Numerics return result; } - /// - /// Creates a Matrix that reflects the coordinate system about a specified Plane. - /// - /// The Plane about which to create a reflection. - /// A new matrix expressing the reflection. - public static Matrix4x4 CreateReflection(Plane value) + /// Creates a translation matrix. + /// The amount to translate in each axis. + /// The translation matrix. + public static Matrix4x4 CreateTranslation(Vector3 position) { - value = Plane.Normalize(value); + Matrix4x4 result = Identity; + result.M41 = position.X; + result.M42 = position.Y; + result.M43 = position.Z; + return result; + } - float a = value.Normal.X; - float b = value.Normal.Y; - float c = value.Normal.Z; + /// Creates a translation matrix. + /// The amount to translate on the X-axis. + /// The amount to translate on the Y-axis. + /// The amount to translate on the Z-axis. + /// The translation matrix. + public static Matrix4x4 CreateTranslation(float xPosition, float yPosition, float zPosition) + { + Matrix4x4 result = Identity; + result.M41 = xPosition; + result.M42 = yPosition; + result.M43 = zPosition; + return result; + } - float fa = -2.0f * a; - float fb = -2.0f * b; - float fc = -2.0f * c; + /// Creates a world matrix with the specified parameters. + /// The position of the object; used in translation operations. + /// Forward direction of the object. + /// Upward direction of the object; usually [0, 1, 0]. + /// The world matrix. + 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 = _identity; + Matrix4x4 result = Identity; - result.M11 = fa * a + 1.0f; - result.M12 = fb * a; - result.M13 = fc * a; + result.M11 = xaxis.X; + result.M12 = xaxis.Y; + result.M13 = xaxis.Z; - result.M21 = fa * b; - result.M22 = fb * b + 1.0f; - result.M23 = fc * b; + result.M21 = yaxis.X; + result.M22 = yaxis.Y; + result.M23 = yaxis.Z; - result.M31 = fa * c; - result.M32 = fb * c; - result.M33 = fc * c + 1.0f; + result.M31 = zaxis.X; + result.M32 = zaxis.Y; + result.M33 = zaxis.Z; - result.M41 = fa * value.D; - result.M42 = fb * value.D; - result.M43 = fc * value.D; + result.M41 = position.X; + result.M42 = position.Y; + result.M43 = position.Z; return result; } - /// - /// Calculates the determinant of the matrix. - /// - /// The determinant of the matrix. - 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); - } - - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static Vector128 Permute(Vector128 value, byte control) - { - if (Avx.IsSupported) - { - return Avx.Permute(value, control); - } - else if (Sse.IsSupported) - { - return Sse.Shuffle(value, value, control); - } - else - { - // Redundant test so we won't prejit remainder of this method on platforms without AdvSimd. - throw new PlatformNotSupportedException(); - } - } - - /// - /// Attempts to calculate the inverse of the given matrix. If successful, result will contain the inverted matrix. - /// + /// Attempts to calculate the inverse of the given matrix. If successful, result will contain the inverted matrix. /// The source matrix to invert. /// If successful, contains the inverted matrix. /// True if the source matrix could be inverted; False otherwise. @@ -1474,24 +1668,65 @@ namespace System.Numerics } } - private struct CanonicalBasis + /// Multiplies a matrix by another matrix. + /// The first source matrix. + /// The second source matrix. + /// The result of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) { - public Vector3 Row0; - public Vector3 Row1; - public Vector3 Row2; - }; + return value1 * value2; + } - private struct VectorBasis + /// Multiplies a matrix by a scalar value. + /// The source matrix. + /// The scaling factor. + /// The scaled matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) { - public unsafe Vector3* Element0; - public unsafe Vector3* Element1; - public unsafe Vector3* Element2; + return value1 * value2; } - /// - /// 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. - /// + /// Returns a new matrix with the negated elements of the given matrix. + /// The source matrix. + /// The negated matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 Negate(Matrix4x4 value) + { + return -value; + } + + /// Subtracts the second matrix from the first. + /// The first source matrix. + /// The second source matrix. + /// The result of the subtraction. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) + { + return value1 - value2; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static Vector128 Permute(Vector128 value, byte control) + { + if (Avx.IsSupported) + { + return Avx.Permute(value, control); + } + else if (Sse.IsSupported) + { + return Sse.Shuffle(value, value, control); + } + else + { + // Redundant test so we won't prejit remainder of this method on platforms without AdvSimd. + throw new PlatformNotSupportedException(); + } + } + + /// 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. /// The source matrix. /// The scaling component of the transformation matrix. /// The rotation component of the transformation matrix. @@ -1511,7 +1746,7 @@ namespace System.Numerics VectorBasis vectorBasis; Vector3** pVectorBasis = (Vector3**)&vectorBasis; - Matrix4x4 matTemp = Matrix4x4.Identity; + Matrix4x4 matTemp = Identity; CanonicalBasis canonicalBasis = default; Vector3* pCanonicalBasis = &canonicalBasis.Row0; @@ -1688,9 +1923,62 @@ namespace System.Numerics return result; } - /// - /// Transforms the given matrix by applying the given Quaternion rotation. - /// + /// Linearly interpolates between the corresponding values of two matrices. + /// The first source matrix. + /// The second source matrix. + /// The relative weight of the second source matrix. + /// The interpolated matrix. + public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) + { + if (AdvSimd.IsSupported) + { + Vector128 amountVec = Vector128.Create(amount); + AdvSimd.Store(&matrix1.M11, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M11), AdvSimd.LoadVector128(&matrix2.M11), amountVec)); + AdvSimd.Store(&matrix1.M21, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M21), AdvSimd.LoadVector128(&matrix2.M21), amountVec)); + AdvSimd.Store(&matrix1.M31, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M31), AdvSimd.LoadVector128(&matrix2.M31), amountVec)); + AdvSimd.Store(&matrix1.M41, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M41), AdvSimd.LoadVector128(&matrix2.M41), amountVec)); + return matrix1; + } + else if (Sse.IsSupported) + { + Vector128 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; + } + + 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; + } + + /// Transforms the given matrix by applying the given Quaternion rotation. /// The source matrix to transform. /// The rotation to apply. /// The transformed matrix. @@ -1725,598 +2013,171 @@ namespace System.Numerics 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; - } - - /// - /// Transposes the rows and columns of a matrix. - /// - /// The source matrix. - /// The transposed matrix. - public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) - { - if (AdvSimd.Arm64.IsSupported) - { - // This implementation is based on the DirectX Math Library XMMatrixTranspose method - // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl - - Vector128 M11 = AdvSimd.LoadVector128(&matrix.M11); - Vector128 M31 = AdvSimd.LoadVector128(&matrix.M31); - - Vector128 P00 = AdvSimd.Arm64.ZipLow(M11, M31); - Vector128 P01 = AdvSimd.Arm64.ZipHigh(M11, M31); - - Vector128 M21 = AdvSimd.LoadVector128(&matrix.M21); - Vector128 M41 = AdvSimd.LoadVector128(&matrix.M41); - - Vector128 P10 = AdvSimd.Arm64.ZipLow(M21, M41); - Vector128 P11 = AdvSimd.Arm64.ZipHigh(M21, M41); - - AdvSimd.Store(&matrix.M11, AdvSimd.Arm64.ZipLow(P00, P10)); - AdvSimd.Store(&matrix.M21, AdvSimd.Arm64.ZipHigh(P00, P10)); - AdvSimd.Store(&matrix.M31, AdvSimd.Arm64.ZipLow(P01, P11)); - AdvSimd.Store(&matrix.M41, AdvSimd.Arm64.ZipHigh(P01, P11)); - - return matrix; - } - else if (Sse.IsSupported) - { - Vector128 row1 = Sse.LoadVector128(&matrix.M11); - Vector128 row2 = Sse.LoadVector128(&matrix.M21); - Vector128 row3 = Sse.LoadVector128(&matrix.M31); - Vector128 row4 = Sse.LoadVector128(&matrix.M41); - - Vector128 l12 = Sse.UnpackLow(row1, row2); - Vector128 l34 = Sse.UnpackLow(row3, row4); - Vector128 h12 = Sse.UnpackHigh(row1, row2); - Vector128 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; - } - - 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; - } - - /// - /// Linearly interpolates between the corresponding values of two matrices. - /// - /// The first source matrix. - /// The second source matrix. - /// The relative weight of the second source matrix. - /// The interpolated matrix. - public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) - { - if (AdvSimd.IsSupported) - { - Vector128 amountVec = Vector128.Create(amount); - AdvSimd.Store(&matrix1.M11, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M11), AdvSimd.LoadVector128(&matrix2.M11), amountVec)); - AdvSimd.Store(&matrix1.M21, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M21), AdvSimd.LoadVector128(&matrix2.M21), amountVec)); - AdvSimd.Store(&matrix1.M31, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M31), AdvSimd.LoadVector128(&matrix2.M31), amountVec)); - AdvSimd.Store(&matrix1.M41, VectorMath.Lerp(AdvSimd.LoadVector128(&matrix1.M41), AdvSimd.LoadVector128(&matrix2.M41), amountVec)); - return matrix1; - } - else if (Sse.IsSupported) - { - Vector128 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; - } - - 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; - } - - /// - /// Returns a new matrix with the negated elements of the given matrix. - /// - /// The source matrix. - /// The negated matrix. - public static Matrix4x4 Negate(Matrix4x4 value) => -value; - - /// - /// Adds two matrices together. - /// - /// The first source matrix. - /// The second source matrix. - /// The resulting matrix. - public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) => value1 + value2; - - /// - /// Subtracts the second matrix from the first. - /// - /// The first source matrix. - /// The second source matrix. - /// The result of the subtraction. - public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) => value1 - value2; - - /// - /// Multiplies a matrix by another matrix. - /// - /// The first source matrix. - /// The second source matrix. - /// The result of the multiplication. - public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) => value1 * value2; - - /// - /// Multiplies a matrix by a scalar value. - /// - /// The source matrix. - /// The scaling factor. - /// The scaled matrix. - public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) => value1 * value2; - - /// - /// Returns a new matrix with the negated elements of the given matrix. - /// - /// The source matrix. - /// The negated matrix. - public static unsafe Matrix4x4 operator -(Matrix4x4 value) - { - if (AdvSimd.IsSupported) - { - AdvSimd.Store(&value.M11, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M11))); - AdvSimd.Store(&value.M21, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M21))); - AdvSimd.Store(&value.M31, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M31))); - AdvSimd.Store(&value.M41, AdvSimd.Negate(AdvSimd.LoadVector128(&value.M41))); - return value; - } - else if (Sse.IsSupported) - { - Vector128 zero = Vector128.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; - } - - 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; - } - - /// - /// Adds two matrices together. - /// - /// The first source matrix. - /// The second source matrix. - /// The resulting matrix. - public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) - { - if (AdvSimd.IsSupported) - { - AdvSimd.Store(&value1.M11, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11))); - AdvSimd.Store(&value1.M21, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21))); - AdvSimd.Store(&value1.M31, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31))); - AdvSimd.Store(&value1.M41, AdvSimd.Add(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41))); - return value1; - } - else 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; - } - - 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; - } - - /// - /// Subtracts the second matrix from the first. - /// - /// The first source matrix. - /// The second source matrix. - /// The result of the subtraction. - public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) - { - if (AdvSimd.IsSupported) - { - AdvSimd.Store(&value1.M11, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11))); - AdvSimd.Store(&value1.M21, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21))); - AdvSimd.Store(&value1.M31, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31))); - AdvSimd.Store(&value1.M41, AdvSimd.Subtract(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41))); - return value1; - } - else 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; - } - - 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; - } - - /// - /// Multiplies a matrix by another matrix. - /// - /// The first source matrix. - /// The second source matrix. - /// The result of the multiplication. - public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) - { - if (AdvSimd.Arm64.IsSupported) - { - Unsafe.SkipInit(out Matrix4x4 result); - - // Perform the operation on the first row - - Vector128 M11 = AdvSimd.LoadVector128(&value1.M11); - - Vector128 vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M11, 0); - Vector128 vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M11, 1); - Vector128 vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M11, 2); - Vector128 vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M11, 3); - - AdvSimd.Store(&result.M11, AdvSimd.Add(vZ, vW)); - - // Repeat for the other 3 rows - - Vector128 M21 = AdvSimd.LoadVector128(&value1.M21); - - vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M21, 0); - vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M21, 1); - vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M21, 2); - vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M21, 3); - - AdvSimd.Store(&result.M21, AdvSimd.Add(vZ, vW)); - - Vector128 M31 = AdvSimd.LoadVector128(&value1.M31); - - vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M31, 0); - vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M31, 1); - vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M31, 2); - vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M31, 3); - - AdvSimd.Store(&result.M31, AdvSimd.Add(vZ, vW)); - - Vector128 M41 = AdvSimd.LoadVector128(&value1.M41); - - vX = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M11), M41, 0); - vY = AdvSimd.MultiplyBySelectedScalar(AdvSimd.LoadVector128(&value2.M21), M41, 1); - vZ = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vX, AdvSimd.LoadVector128(&value2.M31), M41, 2); - vW = AdvSimd.Arm64.FusedMultiplyAddBySelectedScalar(vY, AdvSimd.LoadVector128(&value2.M41), M41, 3); - - AdvSimd.Store(&result.M41, AdvSimd.Add(vZ, vW)); - - return result; - } - else if (Sse.IsSupported) - { - Vector128 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; - } - - 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; + // 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 - 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; + 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 - 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; + 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 - 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; + 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 m; + return result; } - /// - /// Multiplies a matrix by a scalar value. - /// - /// The source matrix. - /// The scaling factor. - /// The scaled matrix. - public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) + /// Transposes the rows and columns of a matrix. + /// The source matrix. + /// The transposed matrix. + public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) { - if (AdvSimd.IsSupported) - { - Vector128 value2Vec = Vector128.Create(value2); - AdvSimd.Store(&value1.M11, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M11), value2Vec)); - AdvSimd.Store(&value1.M21, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M21), value2Vec)); - AdvSimd.Store(&value1.M31, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M31), value2Vec)); - AdvSimd.Store(&value1.M41, AdvSimd.Multiply(AdvSimd.LoadVector128(&value1.M41), value2Vec)); - return value1; - } - else if (Sse.IsSupported) + if (AdvSimd.Arm64.IsSupported) { - Vector128 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; - } + // This implementation is based on the DirectX Math Library XMMatrixTranspose method + // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMatrix.inl - Matrix4x4 m; + Vector128 M11 = AdvSimd.LoadVector128(&matrix.M11); + Vector128 M31 = AdvSimd.LoadVector128(&matrix.M31); - 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; - } + Vector128 P00 = AdvSimd.Arm64.ZipLow(M11, M31); + Vector128 P01 = AdvSimd.Arm64.ZipHigh(M11, M31); - /// - /// Returns a boolean indicating whether the given two matrices are equal. - /// - /// The first matrix to compare. - /// The second matrix to compare. - /// True if the given matrices are equal; False otherwise. - public static unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2) - { - if (AdvSimd.Arm64.IsSupported) - { - return VectorMath.Equal(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11)) && - VectorMath.Equal(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21)) && - VectorMath.Equal(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31)) && - VectorMath.Equal(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41)); + Vector128 M21 = AdvSimd.LoadVector128(&matrix.M21); + Vector128 M41 = AdvSimd.LoadVector128(&matrix.M41); + + Vector128 P10 = AdvSimd.Arm64.ZipLow(M21, M41); + Vector128 P11 = AdvSimd.Arm64.ZipHigh(M21, M41); + + AdvSimd.Store(&matrix.M11, AdvSimd.Arm64.ZipLow(P00, P10)); + AdvSimd.Store(&matrix.M21, AdvSimd.Arm64.ZipHigh(P00, P10)); + AdvSimd.Store(&matrix.M31, AdvSimd.Arm64.ZipLow(P01, P11)); + AdvSimd.Store(&matrix.M41, AdvSimd.Arm64.ZipHigh(P01, P11)); + + return matrix; } else 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)); + Vector128 row1 = Sse.LoadVector128(&matrix.M11); + Vector128 row2 = Sse.LoadVector128(&matrix.M21); + Vector128 row3 = Sse.LoadVector128(&matrix.M31); + Vector128 row4 = Sse.LoadVector128(&matrix.M41); + + Vector128 l12 = Sse.UnpackLow(row1, row2); + Vector128 l34 = Sse.UnpackLow(row3, row4); + Vector128 h12 = Sse.UnpackHigh(row1, row2); + Vector128 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; } - 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); + 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; } - /// - /// Returns a boolean indicating whether the given two matrices are not equal. - /// - /// The first matrix to compare. - /// The second matrix to compare. - /// True if the given matrices are not equal; False if they are equal. - public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + /// Returns a boolean indicating whether the given Object is equal to this matrix instance. + /// The Object to compare against. + /// True if the Object is equal to this matrix; False otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public override readonly bool Equals(object? obj) { - if (AdvSimd.Arm64.IsSupported) - { - return VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M11), AdvSimd.LoadVector128(&value2.M11)) || - VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M21), AdvSimd.LoadVector128(&value2.M21)) || - VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M31), AdvSimd.LoadVector128(&value2.M31)) || - VectorMath.NotEqual(AdvSimd.LoadVector128(&value1.M41), AdvSimd.LoadVector128(&value2.M41)); - } - else 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)); - } - - 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); + return (obj is Matrix4x4 other) && Equals(other); } - /// - /// Returns a boolean indicating whether this matrix instance is equal to the other given matrix. - /// + /// Returns a boolean indicating whether this matrix instance is equal to the other given matrix. /// The matrix to compare this instance to. /// True if the matrices are equal; False otherwise. - public readonly bool Equals(Matrix4x4 other) => this == other; - - /// - /// Returns a boolean indicating whether the given Object is equal to this matrix instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this matrix; False otherwise. - public override readonly bool Equals(object? obj) => (obj is Matrix4x4 other) && (this == other); + public readonly bool Equals(Matrix4x4 other) + { + return this == other; + } - /// - /// Returns a String representing this matrix instance. - /// - /// The string representation. - public override readonly string ToString() + /// Calculates the determinant of the matrix. + /// The determinant of the matrix. + public readonly float GetDeterminant() { - 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); + // | 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); } - /// - /// Returns the hash code for this instance. - /// + /// Returns the hash code for this instance. /// The hash code. public override readonly int GetHashCode() { @@ -2344,5 +2205,30 @@ namespace System.Numerics return hash.ToHashCode(); } + + /// Returns a String representing this matrix instance. + /// The string representation. + 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); + } + + 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; + } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs index 631f6da..b2e8c41 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Plane.cs @@ -6,26 +6,19 @@ using System.Runtime.CompilerServices; namespace System.Numerics { - /// - /// A structure encapsulating a 3D Plane - /// + /// A structure encapsulating a 3D Plane [Intrinsic] public struct Plane : IEquatable { private const float NormalizeEpsilon = 1.192092896e-07f; // smallest such that 1.0+NormalizeEpsilon != 1.0 - /// - /// The normal vector of the Plane. - /// + /// The normal vector of the Plane. public Vector3 Normal; - /// - /// The distance of the Plane along its normal from the origin. - /// + + /// The distance of the Plane along its normal from the origin. public float D; - /// - /// Constructs a Plane from the X, Y, and Z components of its normal, and its distance from the origin on that normal. - /// + /// Constructs a Plane from the X, Y, and Z components of its normal, and its distance from the origin on that normal. /// The X-component of the normal. /// The Y-component of the normal. /// The Z-component of the normal. @@ -33,23 +26,19 @@ namespace System.Numerics public Plane(float x, float y, float z, float d) { Normal = new Vector3(x, y, z); - this.D = d; + D = d; } - /// - /// Constructs a Plane from the given normal and distance along the normal from the origin. - /// + /// Constructs a Plane from the given normal and distance along the normal from the origin. /// The Plane's normal vector. /// The Plane's distance from the origin along its normal vector. public Plane(Vector3 normal, float d) { - this.Normal = normal; - this.D = d; + Normal = normal; + D = d; } - /// - /// Constructs a Plane from the given Vector4. - /// + /// Constructs a Plane from the given Vector4. /// A vector whose first 3 elements describe the normal vector, /// and whose W component defines the distance along that normal from the origin. public Plane(Vector4 value) @@ -58,9 +47,7 @@ namespace System.Numerics D = value.W; } - /// - /// Creates a Plane that contains the three given points. - /// + /// Creates a Plane that contains the three given points. /// The first point defining the Plane. /// The second point defining the Plane. /// The third point defining the Plane. @@ -112,9 +99,59 @@ namespace System.Numerics } } - /// - /// Creates a new Plane whose normal vector is the source Plane's normal vector normalized. - /// + /// Calculates the dot product of a Plane and Vector4. + /// The Plane. + /// The Vector4. + /// The dot product. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dot(Plane plane, Vector4 value) + { + return plane.Normal.X * value.X + + plane.Normal.Y * value.Y + + plane.Normal.Z * value.Z + + plane.D * value.W; + } + + /// Returns the dot product of a specified Vector3 and the normal vector of this Plane plus the distance (D) value of the Plane. + /// The plane. + /// The Vector3. + /// The resulting value. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float DotCoordinate(Plane plane, Vector3 value) + { + if (Vector.IsHardwareAccelerated) + { + return Vector3.Dot(plane.Normal, value) + plane.D; + } + else + { + return plane.Normal.X * value.X + + plane.Normal.Y * value.Y + + plane.Normal.Z * value.Z + + plane.D; + } + } + + /// Returns the dot product of a specified Vector3 and the Normal vector of this Plane. + /// The plane. + /// The Vector3. + /// The resulting dot product. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float DotNormal(Plane plane, Vector3 value) + { + if (Vector.IsHardwareAccelerated) + { + return Vector3.Dot(plane.Normal, value); + } + else + { + return plane.Normal.X * value.X + + plane.Normal.Y * value.Y + + plane.Normal.Z * value.Z; + } + } + + /// Creates a new Plane whose normal vector is the source Plane's normal vector normalized. /// The source Plane. /// The normalized Plane. [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -152,9 +189,7 @@ namespace System.Numerics } } - /// - /// Transforms a normalized Plane by a Matrix. - /// + /// Transforms a normalized Plane by a Matrix. /// The normalized Plane to transform. /// This Plane must already be normalized, so that its Normal vector is of unit length, before this method is called. /// The transformation matrix to apply to the Plane. @@ -173,9 +208,7 @@ namespace System.Numerics x * m.M41 + y * m.M42 + z * m.M43 + w * m.M44); } - /// - /// Transforms a normalized Plane by a Quaternion rotation. - /// + /// Transforms a normalized Plane by a Quaternion rotation. /// The normalized Plane to transform. /// This Plane must already be normalized, so that its Normal vector is of unit length, before this method is called. /// The Quaternion rotation to apply to the Plane. @@ -219,67 +252,7 @@ namespace System.Numerics plane.D); } - /// - /// Calculates the dot product of a Plane and Vector4. - /// - /// The Plane. - /// The Vector4. - /// The dot product. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dot(Plane plane, Vector4 value) - { - return plane.Normal.X * value.X + - plane.Normal.Y * value.Y + - plane.Normal.Z * value.Z + - plane.D * value.W; - } - - /// - /// Returns the dot product of a specified Vector3 and the normal vector of this Plane plus the distance (D) value of the Plane. - /// - /// The plane. - /// The Vector3. - /// The resulting value. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float DotCoordinate(Plane plane, Vector3 value) - { - if (Vector.IsHardwareAccelerated) - { - return Vector3.Dot(plane.Normal, value) + plane.D; - } - else - { - return plane.Normal.X * value.X + - plane.Normal.Y * value.Y + - plane.Normal.Z * value.Z + - plane.D; - } - } - - /// - /// Returns the dot product of a specified Vector3 and the Normal vector of this Plane. - /// - /// The plane. - /// The Vector3. - /// The resulting dot product. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float DotNormal(Plane plane, Vector3 value) - { - if (Vector.IsHardwareAccelerated) - { - return Vector3.Dot(plane.Normal, value); - } - else - { - return plane.Normal.X * value.X + - plane.Normal.Y * value.Y + - plane.Normal.Z * value.Z; - } - } - - /// - /// Returns a boolean indicating whether the two given Planes are equal. - /// + /// Returns a boolean indicating whether the two given Planes are equal. /// The first Plane to compare. /// The second Plane to compare. /// True if the Planes are equal; False otherwise. @@ -292,24 +265,26 @@ namespace System.Numerics value1.D == value2.D); } - /// - /// Returns a boolean indicating whether the two given Planes are not equal. - /// + /// Returns a boolean indicating whether the two given Planes are not equal. /// The first Plane to compare. /// The second Plane to compare. /// True if the Planes are not equal; False if they are equal. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool operator !=(Plane value1, Plane value2) { - return (value1.Normal.X != value2.Normal.X || - value1.Normal.Y != value2.Normal.Y || - value1.Normal.Z != value2.Normal.Z || - value1.D != value2.D); + return !(value1 == value2); } - /// - /// Returns a boolean indicating whether the given Plane is equal to this Plane instance. - /// + /// Returns a boolean indicating whether the given Object is equal to this Plane instance. + /// The Object to compare against. + /// True if the Object is equal to this Plane; False otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public override readonly bool Equals(object? obj) + { + return (obj is Plane other) && Equals(other); + } + + /// Returns a boolean indicating whether the given Plane is equal to this Plane instance. /// The Plane to compare this instance to. /// True if the other Plane is equal to this instance; False otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -317,7 +292,7 @@ namespace System.Numerics { if (Vector.IsHardwareAccelerated) { - return this.Normal.Equals(other.Normal) && this.D == other.D; + return Normal.Equals(other.Normal) && D == other.D; } else { @@ -328,40 +303,19 @@ namespace System.Numerics } } - /// - /// Returns a boolean indicating whether the given Object is equal to this Plane instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this Plane; False otherwise. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public override readonly bool Equals(object? obj) + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() { - if (obj is Plane) - { - return Equals((Plane)obj); - } - - return false; + return Normal.GetHashCode() + D.GetHashCode(); } - /// - /// Returns a String representing this Plane instance. - /// + /// Returns a String representing this Plane instance. /// The string representation. public override readonly string ToString() { CultureInfo ci = CultureInfo.CurrentCulture; - return string.Format(ci, "{{Normal:{0} D:{1}}}", Normal.ToString(), D.ToString(ci)); } - - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() - { - return Normal.GetHashCode() + D.GetHashCode(); - } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs index b7a5bc3..e1096b7 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs @@ -6,66 +6,38 @@ using System.Runtime.CompilerServices; namespace System.Numerics { - /// - /// 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). - /// + /// Represents a vector that is used to encode three-dimensional physical rotations. [Intrinsic] public struct Quaternion : IEquatable { private const float SlerpEpsilon = 1e-6f; - /// - /// Specifies the X-value of the vector component of the Quaternion. - /// + /// Specifies the X-value of the vector component of the Quaternion. public float X; - /// - /// Specifies the Y-value of the vector component of the Quaternion. - /// + + /// Specifies the Y-value of the vector component of the Quaternion. public float Y; - /// - /// Specifies the Z-value of the vector component of the Quaternion. - /// - public float Z; - /// - /// Specifies the rotation component of the Quaternion. - /// - public float W; - /// - /// Returns a Quaternion representing no rotation. - /// - public static Quaternion Identity - { - get { return new Quaternion(0, 0, 0, 1); } - } + /// Specifies the Z-value of the vector component of the Quaternion. + public float Z; - /// - /// Returns whether the Quaternion is the identity Quaternion. - /// - public readonly bool IsIdentity - { - get { return X == 0f && Y == 0f && Z == 0f && W == 1f; } - } + /// Specifies the rotation component of the Quaternion. + public float W; - /// - /// Constructs a Quaternion from the given components. - /// + /// Constructs a Quaternion from the given components. /// The X component of the Quaternion. /// The Y component of the Quaternion. /// The Z component of the Quaternion. /// The W component of the Quaternion. public Quaternion(float x, float y, float z, float w) { - this.X = x; - this.Y = y; - this.Z = z; - this.W = w; + X = x; + Y = y; + Z = z; + W = w; } - /// - /// Constructs a Quaternion from the given vector and rotation parts. - /// + /// Constructs a Quaternion from the given vector and rotation parts. /// The vector part of the Quaternion. /// The rotation part of the Quaternion. public Quaternion(Vector3 vectorPart, float scalarPart) @@ -76,91 +48,238 @@ namespace System.Numerics W = scalarPart; } - /// - /// Calculates the length of the Quaternion. - /// - /// The computed length of the Quaternion. - public readonly float Length() + /// Returns a Quaternion representing no rotation. + public static Quaternion Identity { - float ls = X * X + Y * Y + Z * Z + W * W; + get => new Quaternion(0, 0, 0, 1); + } - return MathF.Sqrt(ls); + /// Returns whether the Quaternion is the identity Quaternion. + public readonly bool IsIdentity + { + get => this == Identity; } - /// - /// Calculates the length squared of the Quaternion. This operation is cheaper than Length(). - /// - /// The length squared of the Quaternion. - public readonly float LengthSquared() + /// Adds two Quaternions element-by-element. + /// The first source Quaternion. + /// The second source Quaternion. + /// The result of adding the Quaternions. + public static Quaternion operator +(Quaternion value1, Quaternion value2) { - return X * X + Y * Y + Z * Z + W * W; + 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; } - /// - /// Divides each component of the Quaternion by the length of the Quaternion. - /// - /// The source Quaternion. - /// The normalized Quaternion. - public static Quaternion Normalize(Quaternion value) + /// Divides a Quaternion by another Quaternion. + /// The source Quaternion. + /// The divisor. + /// The result of the division. + public static Quaternion operator /(Quaternion value1, Quaternion value2) { Quaternion ans; - float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; + float q1x = value1.X; + float q1y = value1.Y; + float q1z = value1.Z; + float q1w = value1.W; - float invNorm = 1.0f / MathF.Sqrt(ls); + //------------------------------------- + // 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; - ans.X = value.X * invNorm; - ans.Y = value.Y * invNorm; - ans.Z = value.Z * invNorm; - ans.W = value.W * invNorm; + 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; } - /// - /// Creates the conjugate of a specified Quaternion. - /// - /// The Quaternion of which to return the conjugate. - /// A new Quaternion that is the conjugate of the specified one. - public static Quaternion Conjugate(Quaternion value) + /// Returns a boolean indicating whether the two given Quaternions are equal. + /// The first Quaternion to compare. + /// The second Quaternion to compare. + /// True if the Quaternions are equal; False otherwise. + 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); + } + + /// Returns a boolean indicating whether the two given Quaternions are not equal. + /// The first Quaternion to compare. + /// The second Quaternion to compare. + /// True if the Quaternions are not equal; False if they are equal. + public static bool operator !=(Quaternion value1, Quaternion value2) + { + return !(value1 == value2); + } + + /// Multiplies two Quaternions together. + /// The Quaternion on the left side of the multiplication. + /// The Quaternion on the right side of the multiplication. + /// The result of the multiplication. + 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; + } + + /// Multiplies a Quaternion by a scalar value. + /// The source Quaternion. + /// The scalar value. + /// The result of the multiplication. + 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; + } + + /// Subtracts one Quaternion from another. + /// The first source Quaternion. + /// The second Quaternion, to be subtracted from the first. + /// The result of the subtraction. + 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; + } + + /// Flips the sign of each component of the quaternion. + /// The source Quaternion. + /// The negated Quaternion. + public static Quaternion operator -(Quaternion value) { Quaternion ans; ans.X = -value.X; ans.Y = -value.Y; ans.Z = -value.Z; - ans.W = value.W; + ans.W = -value.W; return ans; } - /// - /// Returns the inverse of a Quaternion. - /// - /// The source Quaternion. - /// The inverted Quaternion. - public static Quaternion Inverse(Quaternion value) + /// Adds two Quaternions element-by-element. + /// The first source Quaternion. + /// The second source Quaternion. + /// The result of adding the Quaternions. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Quaternion Add(Quaternion value1, Quaternion value2) { - // -1 ( a -v ) - // q = ( ------------- ------------- ) - // ( a^2 + |v|^2 , a^2 + |v|^2 ) + return value1 + value2; + } + /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation. + /// The first Quaternion rotation in the series. + /// The second Quaternion rotation in the series. + /// A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation. + public static Quaternion Concatenate(Quaternion value1, Quaternion value2) + { 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; + // 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; - ans.X = -value.X * invNorm; - ans.Y = -value.Y * invNorm; - ans.Z = -value.Z * invNorm; - ans.W = value.W * invNorm; + 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; + } + + /// Creates the conjugate of a specified Quaternion. + /// The Quaternion of which to return the conjugate. + /// A new Quaternion that is the conjugate of the specified one. + 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; } - /// - /// Creates a Quaternion from a normalized vector axis and an angle to rotate about the vector. - /// + /// Creates a Quaternion from a normalized vector axis and an angle to rotate about the vector. /// The unit vector to rotate around. /// This vector must be normalized before calling this function or the resulting Quaternion will be incorrect. /// The angle, in radians, to rotate around the vector. @@ -181,44 +300,7 @@ namespace System.Numerics return ans; } - /// - /// Creates a new Quaternion from the given yaw, pitch, and roll, in radians. - /// - /// The yaw angle, in radians, around the Y-axis. - /// The pitch angle, in radians, around the X-axis. - /// The roll angle, in radians, around the Z-axis. - /// - 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; - } - - /// - /// Creates a Quaternion from the given rotation matrix. - /// + /// Creates a Quaternion from the given rotation matrix. /// The rotation matrix. /// The created Quaternion. public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix) @@ -270,74 +352,84 @@ namespace System.Numerics return q; } - /// - /// Calculates the dot product of two Quaternions. - /// - /// The first source Quaternion. - /// The second source Quaternion. - /// The dot product of the Quaternions. - public static float Dot(Quaternion quaternion1, Quaternion quaternion2) + /// Creates a new Quaternion from the given yaw, pitch, and roll, in radians. + /// The yaw angle, in radians, around the Y-axis. + /// The pitch angle, in radians, around the X-axis. + /// The roll angle, in radians, around the Z-axis. + /// + public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) { - return quaternion1.X * quaternion2.X + - quaternion1.Y * quaternion2.Y + - quaternion1.Z * quaternion2.Z + - quaternion1.W * quaternion2.W; - } + // 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; - /// - /// Interpolates between two quaternions, using spherical linear interpolation. - /// - /// The first source Quaternion. - /// The second source Quaternion. - /// The relative weight of the second source Quaternion in the interpolation. - /// The interpolated Quaternion. - public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) - { - float t = amount; + float halfRoll = roll * 0.5f; + sr = MathF.Sin(halfRoll); + cr = MathF.Cos(halfRoll); - float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + - quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; + float halfPitch = pitch * 0.5f; + sp = MathF.Sin(halfPitch); + cp = MathF.Cos(halfPitch); - bool flip = false; + float halfYaw = yaw * 0.5f; + sy = MathF.Sin(halfYaw); + cy = MathF.Cos(halfYaw); - if (cosOmega < 0.0f) - { - flip = true; - cosOmega = -cosOmega; - } + Quaternion result; - float s1, s2; + 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; - 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); + return result; + } - s1 = MathF.Sin((1.0f - t) * omega) * invSinOmega; - s2 = (flip) - ? -MathF.Sin(t * omega) * invSinOmega - : MathF.Sin(t * omega) * invSinOmega; - } + /// Divides a Quaternion by another Quaternion. + /// The source Quaternion. + /// The divisor. + /// The result of the division. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Quaternion Divide(Quaternion value1, Quaternion value2) + { + return value1 / value2; + } + + /// Calculates the dot product of two Quaternions. + /// The first source Quaternion. + /// The second source Quaternion. + /// The dot product of the Quaternions. + 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; + } + + /// Returns the inverse of a Quaternion. + /// The source Quaternion. + /// The inverted Quaternion. + public static Quaternion Inverse(Quaternion value) + { + // -1 ( a -v ) + // q = ( ------------- ------------- ) + // ( a^2 + |v|^2 , a^2 + |v|^2 ) 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; + 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; } - /// - /// Linearly interpolates between two quaternions. - /// + /// Linearly interpolates between two quaternions. /// The first source Quaternion. /// The second source Quaternion. /// The relative weight of the second source Quaternion in the interpolation. @@ -379,415 +471,156 @@ namespace System.Numerics return r; } - /// - /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation. - /// - /// The first Quaternion rotation in the series. - /// The second Quaternion rotation in the series. - /// A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation. - 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; - } - - /// - /// Flips the sign of each component of the quaternion. - /// - /// The source Quaternion. - /// The negated Quaternion. - 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; - } - - /// - /// Adds two Quaternions element-by-element. - /// - /// The first source Quaternion. - /// The second source Quaternion. - /// The result of adding the Quaternions. - 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; - } - - /// - /// Subtracts one Quaternion from another. - /// - /// The first source Quaternion. - /// The second Quaternion, to be subtracted from the first. - /// The result of the subtraction. - 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; - } - - /// - /// Multiplies two Quaternions together. - /// + /// Multiplies two Quaternions together. /// The Quaternion on the left side of the multiplication. /// The Quaternion on the right side of the multiplication. /// The result of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] 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; + return value1 * value2; } - /// - /// Multiplies a Quaternion by a scalar value. - /// + /// Multiplies a Quaternion by a scalar value. /// The source Quaternion. /// The scalar value. /// The result of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] 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; - } - - /// - /// Divides a Quaternion by another Quaternion. - /// - /// The source Quaternion. - /// The divisor. - /// The result of the division. - 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; + return value1 * value2; } - /// - /// Flips the sign of each component of the quaternion. - /// + /// Flips the sign of each component of the quaternion. /// The source Quaternion. /// The negated Quaternion. - public static Quaternion operator -(Quaternion value) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + 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; + return -value; } - /// - /// Adds two Quaternions element-by-element. - /// - /// The first source Quaternion. - /// The second source Quaternion. - /// The result of adding the Quaternions. - public static Quaternion operator +(Quaternion value1, Quaternion value2) + /// Divides each component of the Quaternion by the length of the Quaternion. + /// The source Quaternion. + /// The normalized Quaternion. + public static Quaternion Normalize(Quaternion value) { 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; - } + float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; - /// - /// Subtracts one Quaternion from another. - /// - /// The first source Quaternion. - /// The second Quaternion, to be subtracted from the first. - /// The result of the subtraction. - public static Quaternion operator -(Quaternion value1, Quaternion value2) - { - Quaternion ans; + float invNorm = 1.0f / MathF.Sqrt(ls); - ans.X = value1.X - value2.X; - ans.Y = value1.Y - value2.Y; - ans.Z = value1.Z - value2.Z; - ans.W = value1.W - value2.W; + ans.X = value.X * invNorm; + ans.Y = value.Y * invNorm; + ans.Z = value.Z * invNorm; + ans.W = value.W * invNorm; return ans; } - /// - /// Multiplies two Quaternions together. - /// - /// The Quaternion on the left side of the multiplication. - /// The Quaternion on the right side of the multiplication. - /// The result of the multiplication. - public static Quaternion operator *(Quaternion value1, Quaternion value2) + /// Interpolates between two quaternions, using spherical linear interpolation. + /// The first source Quaternion. + /// The second source Quaternion. + /// The relative weight of the second source Quaternion in the interpolation. + /// The interpolated Quaternion. + public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) { - 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 t = amount; - float dot = q1x * q2x + q1y * q2y + q1z * q2z; + float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; - 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; + bool flip = false; - return ans; - } + if (cosOmega < 0.0f) + { + flip = true; + cosOmega = -cosOmega; + } - /// - /// Multiplies a Quaternion by a scalar value. - /// - /// The source Quaternion. - /// The scalar value. - /// The result of the multiplication. - public static Quaternion operator *(Quaternion value1, float value2) - { - Quaternion ans; + float s1, s2; - ans.X = value1.X * value2; - ans.Y = value1.Y * value2; - ans.Z = value1.Z * value2; - ans.W = value1.W * value2; + 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); - return ans; - } + s1 = MathF.Sin((1.0f - t) * omega) * invSinOmega; + s2 = (flip) + ? -MathF.Sin(t * omega) * invSinOmega + : MathF.Sin(t * omega) * invSinOmega; + } - /// - /// Divides a Quaternion by another Quaternion. - /// - /// The source Quaternion. - /// The divisor. - /// The result of the division. - 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; + 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; } - /// - /// Returns a boolean indicating whether the two given Quaternions are equal. - /// - /// The first Quaternion to compare. - /// The second Quaternion to compare. - /// True if the Quaternions are equal; False otherwise. - public static bool operator ==(Quaternion value1, Quaternion value2) + /// Subtracts one Quaternion from another. + /// The first source Quaternion. + /// The second Quaternion, to be subtracted from the first. + /// The result of the subtraction. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Quaternion Subtract(Quaternion value1, Quaternion value2) { - return (value1.X == value2.X && - value1.Y == value2.Y && - value1.Z == value2.Z && - value1.W == value2.W); + return value1 - value2; } - /// - /// Returns a boolean indicating whether the two given Quaternions are not equal. - /// - /// The first Quaternion to compare. - /// The second Quaternion to compare. - /// True if the Quaternions are not equal; False if they are equal. - public static bool operator !=(Quaternion value1, Quaternion value2) + /// Returns a boolean indicating whether the given Object is equal to this Quaternion instance. + /// The Object to compare against. + /// True if the Object is equal to this Quaternion; False otherwise. + public override readonly bool Equals(object? obj) { - return (value1.X != value2.X || - value1.Y != value2.Y || - value1.Z != value2.Z || - value1.W != value2.W); + return (obj is Quaternion other) && Equals(other); } - /// - /// Returns a boolean indicating whether the given Quaternion is equal to this Quaternion instance. - /// + /// Returns a boolean indicating whether the given Quaternion is equal to this Quaternion instance. /// The Quaternion to compare this instance to. /// True if the other Quaternion is equal to this instance; False otherwise. public readonly bool Equals(Quaternion other) { - return (X == other.X && - Y == other.Y && - Z == other.Z && - W == other.W); + return this == other; } - /// - /// Returns a boolean indicating whether the given Object is equal to this Quaternion instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this Quaternion; False otherwise. - public override readonly bool Equals(object? obj) + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() { - if (obj is Quaternion) - { - return Equals((Quaternion)obj); - } + return unchecked(X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode()); + } + + /// Calculates the length of the Quaternion. + /// The computed length of the Quaternion. + public readonly float Length() + { + float lengthSquared = LengthSquared(); + return MathF.Sqrt(lengthSquared); + } - return false; + /// Calculates the length squared of the Quaternion. This operation is cheaper than Length(). + /// The length squared of the Quaternion. + public readonly float LengthSquared() + { + return X * X + Y * Y + Z * Z + W * W; } - /// - /// Returns a String representing this Quaternion instance. - /// + /// Returns a String representing this Quaternion instance. /// The string representation. public override readonly string ToString() { return string.Format(CultureInfo.CurrentCulture, "{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W); } - - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() - { - return unchecked(X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode()); - } } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs index 7b03544..02a245b 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2.cs @@ -7,145 +7,223 @@ using System.Text; namespace System.Numerics { - /// - /// A structure encapsulating two single precision floating point values and provides hardware accelerated methods. - /// + /// A structure encapsulating two single precision floating point values and provides hardware accelerated methods. [Intrinsic] public partial struct Vector2 : IEquatable, IFormattable { - #region Public Static Properties - /// - /// Returns the vector (0,0). - /// + /// The X component of the vector. + public float X; + + /// The Y component of the vector. + public float Y; + + /// Constructs a vector whose elements are all the single specified value. + /// The element to fill the vector with. + [Intrinsic] + public Vector2(float value) : this(value, value) + { + } + + /// Constructs a vector with the given individual elements. + /// The X component. + /// The Y component. + [Intrinsic] + public Vector2(float x, float y) + { + X = x; + Y = y; + } + + /// Returns the vector (0,0). public static Vector2 Zero { [Intrinsic] - get - { - return default; - } + get => default; } - /// - /// Returns the vector (1,1). - /// + + /// Returns the vector (1,1). public static Vector2 One { [Intrinsic] - get - { - return new Vector2(1.0f, 1.0f); - } + get => new Vector2(1.0f); } - /// - /// Returns the vector (1,0). - /// - public static Vector2 UnitX { get { return new Vector2(1.0f, 0.0f); } } - /// - /// Returns the vector (0,1). - /// - public static Vector2 UnitY { get { return new Vector2(0.0f, 1.0f); } } - #endregion Public Static Properties - - #region Public instance methods - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() + + /// Returns the vector (1,0). + public static Vector2 UnitX { - return HashCode.Combine(this.X, this.Y); + get => new Vector2(1.0f, 0.0f); } - /// - /// Returns a boolean indicating whether the given Object is equal to this Vector2 instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this Vector2; False otherwise. + /// Returns the vector (0,1). + public static Vector2 UnitY + { + get => new Vector2(0.0f, 1.0f); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public override readonly bool Equals(object? obj) + public static Vector2 operator +(Vector2 left, Vector2 right) { - return (obj is Vector2 other) && Equals(other); + return new Vector2( + left.X + right.X, + left.Y + right.Y + ); } - /// - /// Returns a String representing this Vector2 instance. - /// - /// The string representation. - public override readonly string ToString() + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator /(Vector2 left, Vector2 right) { - return ToString("G", CultureInfo.CurrentCulture); + return new Vector2( + left.X / right.X, + left.Y / right.Y + ); } - /// - /// Returns a String representing this Vector2 instance, using the specified format to format individual elements. - /// - /// The format of individual elements. - /// The string representation. - public readonly string ToString(string? format) + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator /(Vector2 value1, float value2) { - return ToString(format, CultureInfo.CurrentCulture); + return value1 / new Vector2(value2); } - /// - /// Returns a String representing this Vector2 instance, using the specified format to format individual elements - /// and the given IFormatProvider. - /// - /// The format of individual elements. - /// The format provider to use when formatting elements. - /// The string representation. - public readonly string ToString(string? format, IFormatProvider? formatProvider) + /// Returns a boolean indicating whether the two given vectors are equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are equal; False otherwise. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator ==(Vector2 left, Vector2 right) { - StringBuilder sb = new StringBuilder(); - string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; - sb.Append('<'); - sb.Append(this.X.ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(this.Y.ToString(format, formatProvider)); - sb.Append('>'); - return sb.ToString(); + return (left.X == right.X) + && (left.Y == right.Y); } - /// - /// Returns the length of the vector. - /// - /// The vector's length. + /// Returns a boolean indicating whether the two given vectors are not equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are not equal; False if they are equal. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Length() + public static bool operator !=(Vector2 left, Vector2 right) { - float ls = Vector2.Dot(this, this); - return MathF.Sqrt(ls); + return !(left == right); } - /// - /// Returns the length of the vector squared. This operation is cheaper than Length(). - /// - /// The vector's length squared. + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float LengthSquared() + public static Vector2 operator *(Vector2 left, Vector2 right) + { + return new Vector2( + left.X * right.X, + left.Y * right.Y + ); + } + + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator *(Vector2 left, float right) + { + return left * new Vector2(right); + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator *(float left, Vector2 right) + { + return right * left; + } + + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator -(Vector2 left, Vector2 right) { - return Vector2.Dot(this, this); + return new Vector2( + left.X - right.X, + left.Y - right.Y + ); } - #endregion Public Instance Methods - #region Public Static Methods - /// - /// Returns the Euclidean distance between the two given points. - /// + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 operator -(Vector2 value) + { + return Zero - value; + } + + /// Returns a vector whose elements are the absolute values of each of the source vector's elements. + /// The source vector. + /// The absolute value vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Abs(Vector2 value) + { + return new Vector2( + MathF.Abs(value.X), + MathF.Abs(value.Y) + ); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Add(Vector2 left, Vector2 right) + { + return left + right; + } + + /// Restricts a vector between a min and max value. + /// The source vector. + /// The minimum value. + /// The maximum value. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) + { + // We must follow HLSL behavior in the case user specified min value is bigger than max value. + return Min(Max(value1, min), max); + } + + /// Returns the Euclidean distance between the two given points. /// The first point. /// The second point. /// The distance. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Distance(Vector2 value1, Vector2 value2) { - Vector2 difference = value1 - value2; - float ls = Vector2.Dot(difference, difference); - return MathF.Sqrt(ls); + float distanceSquared = DistanceSquared(value1, value2); + return MathF.Sqrt(distanceSquared); } - /// - /// Returns the Euclidean distance squared between the two given points. - /// + /// Returns the Euclidean distance squared between the two given points. /// The first point. /// The second point. /// The distance squared. @@ -153,50 +231,42 @@ namespace System.Numerics public static float DistanceSquared(Vector2 value1, Vector2 value2) { Vector2 difference = value1 - value2; - return Vector2.Dot(difference, difference); + return Dot(difference, difference); } - /// - /// Returns a vector with the same direction as the given vector, but with a length of 1. - /// - /// The vector to normalize. - /// The normalized vector. + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Normalize(Vector2 value) + public static Vector2 Divide(Vector2 left, Vector2 right) { - float length = value.Length(); - return value / length; + return left / right; } - /// - /// Returns the reflection of a vector off a surface that has the specified normal. - /// - /// The source vector. - /// The normal of the surface being reflected off. - /// The reflected vector. + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Reflect(Vector2 vector, Vector2 normal) + public static Vector2 Divide(Vector2 left, float divisor) { - float dot = Vector2.Dot(vector, normal); - return vector - (2 * dot * normal); + return left / divisor; } - /// - /// Restricts a vector between a min and max value. - /// - /// The source vector. - /// The minimum value. - /// The maximum value. + /// Returns the dot product of two vectors. + /// The first vector. + /// The second vector. + /// The dot product. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) + public static float Dot(Vector2 value1, Vector2 value2) { - // We must follow HLSL behavior in the case user specified min value is bigger than max value. - return Vector2.Min(Vector2.Max(value1, min), max); + return (value1.X * value2.X) + + (value1.Y * value2.Y); } - /// - /// Linearly interpolates between two vectors based on the given weighting. - /// + /// Linearly interpolates between two vectors based on the given weighting. /// The first source vector. /// The second source vector. /// Value between 0 and 1 indicating the weight of the second source vector. @@ -207,65 +277,143 @@ namespace System.Numerics return (value1 * (1.0f - amount)) + (value2 * amount); } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. - /// The transformation matrix. - /// The transformed vector. + /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors + /// The first source vector + /// The second source vector + /// The maximized vector + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Transform(Vector2 position, Matrix3x2 matrix) + public static Vector2 Max(Vector2 value1, Vector2 value2) { return new Vector2( - position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M31, - position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M32); + (value1.X > value2.X) ? value1.X : value2.X, + (value1.Y > value2.Y) ? value1.Y : value2.Y + ); } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. - /// The transformation matrix. - /// The transformed vector. + /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. + /// The first source vector. + /// The second source vector. + /// The minimized vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Transform(Vector2 position, Matrix4x4 matrix) + public static Vector2 Min(Vector2 value1, Vector2 value2) { return new Vector2( - position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M41, - position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M42); + (value1.X < value2.X) ? value1.X : value2.X, + (value1.Y < value2.Y) ? value1.Y : value2.Y + ); } - /// - /// Transforms a vector normal by the given matrix. - /// - /// The source vector. + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Multiply(Vector2 left, Vector2 right) + { + return left * right; + } + + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Multiply(Vector2 left, float right) + { + return left * right; + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Multiply(float left, Vector2 right) + { + return left * right; + } + + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Negate(Vector2 value) + { + return -value; + } + + /// Returns a vector with the same direction as the given vector, but with a length of 1. + /// The vector to normalize. + /// The normalized vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Normalize(Vector2 value) + { + return value / value.Length(); + } + + /// Returns the reflection of a vector off a surface that has the specified normal. + /// The source vector. + /// The normal of the surface being reflected off. + /// The reflected vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Reflect(Vector2 vector, Vector2 normal) + { + float dot = Dot(vector, normal); + return vector - (2 * dot * normal); + } + + /// Returns a vector whose elements are the square root of each of the source vector's elements. + /// The source vector. + /// The square root vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 SquareRoot(Vector2 value) + { + return new Vector2( + MathF.Sqrt(value.X), + MathF.Sqrt(value.Y) + ); + } + + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Subtract(Vector2 left, Vector2 right) + { + return left - right; + } + + /// Transforms a vector by the given matrix. + /// The source vector. /// The transformation matrix. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 TransformNormal(Vector2 normal, Matrix3x2 matrix) + public static Vector2 Transform(Vector2 position, Matrix3x2 matrix) { return new Vector2( - normal.X * matrix.M11 + normal.Y * matrix.M21, - normal.X * matrix.M12 + normal.Y * matrix.M22); + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M31, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M32 + ); } - /// - /// Transforms a vector normal by the given matrix. - /// - /// The source vector. + /// Transforms a vector by the given matrix. + /// The source vector. /// The transformation matrix. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 TransformNormal(Vector2 normal, Matrix4x4 matrix) + public static Vector2 Transform(Vector2 position, Matrix4x4 matrix) { return new Vector2( - normal.X * matrix.M11 + normal.Y * matrix.M21, - normal.X * matrix.M12 + normal.Y * matrix.M22); + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42 + ); } - /// - /// Transforms a vector by the given Quaternion rotation value. - /// + /// Transforms a vector by the given Quaternion rotation value. /// The source vector to be rotated. /// The rotation to apply. /// The transformed vector. @@ -284,108 +432,145 @@ namespace System.Numerics return new Vector2( value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2), - value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2)); + value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + ); } - #endregion Public Static Methods - - #region Public operator methods - // all the below methods should be inlined as they are - // implemented over JIT intrinsics - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. + /// Transforms a vector normal by the given matrix. + /// The source vector. + /// The transformation matrix. + /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Add(Vector2 left, Vector2 right) + public static Vector2 TransformNormal(Vector2 normal, Matrix3x2 matrix) { - return left + right; + return new Vector2( + (normal.X * matrix.M11) + (normal.Y * matrix.M21), + (normal.X * matrix.M12) + (normal.Y * matrix.M22) + ); } - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. + /// Transforms a vector normal by the given matrix. + /// The source vector. + /// The transformation matrix. + /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Subtract(Vector2 left, Vector2 right) + public static Vector2 TransformNormal(Vector2 normal, Matrix4x4 matrix) { - return left - right; + return new Vector2( + (normal.X * matrix.M11) + (normal.Y * matrix.M21), + (normal.X * matrix.M12) + (normal.Y * matrix.M22) + ); } - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. + /// Copies the contents of the vector into the given array. + /// The destination array. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Multiply(Vector2 left, Vector2 right) + public readonly void CopyTo(float[] array) { - return left * right; + CopyTo(array, 0); } - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Multiply(Vector2 left, float right) + /// Copies the contents of the vector into the given array, starting from the given index. + /// If array is null. + /// If array is multidimensional. + /// If index is greater than end of the array or index is less than zero. + /// If number of elements in source vector is greater than those available in destination array or if there are not enough elements to copy. + [Intrinsic] + public readonly void CopyTo(float[] array, int index) { - return left * right; + if (array is null) + { + // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. + throw new NullReferenceException(SR.Arg_NullArgumentNullRef); + } + + if ((index < 0) || (index >= array.Length)) + { + throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); + } + + if ((array.Length - index) < 2) + { + throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); + } + + array[index] = X; + array[index + 1] = Y; } - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. + /// Returns a boolean indicating whether the given Object is equal to this Vector2 instance. + /// The Object to compare against. + /// True if the Object is equal to this Vector2; False otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Multiply(float left, Vector2 right) + public override readonly bool Equals(object? obj) { - return left * right; + return (obj is Vector2 other) && Equals(other); } - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Divide(Vector2 left, Vector2 right) + /// Returns a boolean indicating whether the given Vector2 is equal to this Vector2 instance. + /// The Vector2 to compare this instance to. + /// True if the other Vector2 is equal to this instance; False otherwise. + [Intrinsic] + public readonly bool Equals(Vector2 other) { - return left / right; + return this == other; } - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() + { + return HashCode.Combine(X, Y); + } + + /// Returns the length of the vector. + /// The vector's length. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Divide(Vector2 left, float divisor) + public readonly float Length() { - return left / divisor; + float lengthSquared = LengthSquared(); + return MathF.Sqrt(lengthSquared); } - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. + /// Returns the length of the vector squared. This operation is cheaper than Length(). + /// The vector's length squared. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Negate(Vector2 value) + public readonly float LengthSquared() { - return -value; + return Dot(this, this); + } + + /// Returns a String representing this Vector2 instance. + /// The string representation. + public override readonly string ToString() + { + return ToString("G", CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector2 instance, using the specified format to format individual elements. + /// The format of individual elements. + /// The string representation. + public readonly string ToString(string? format) + { + return ToString(format, CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector2 instance, using the specified format to format individual elements and the given IFormatProvider. + /// The format of individual elements. + /// The format provider to use when formatting elements. + /// The string representation. + public readonly string ToString(string? format, IFormatProvider? formatProvider) + { + StringBuilder sb = new StringBuilder(); + string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; + sb.Append('<'); + sb.Append(X.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Y.ToString(format, formatProvider)); + sb.Append('>'); + return sb.ToString(); } - #endregion Public operator methods } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2_Intrinsics.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2_Intrinsics.cs deleted file mode 100644 index ae8c412..0000000 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector2_Intrinsics.cs +++ /dev/null @@ -1,295 +0,0 @@ -// Licensed to the .NET Foundation under one or more agreements. -// The .NET Foundation licenses this file to you under the MIT license. - -using System.Runtime.CompilerServices; - -namespace System.Numerics -{ - // This file contains the definitions for all of the JIT intrinsic methods and properties that are recognized by the current x64 JIT compiler. - // The implementation defined here is used in any circumstance where the JIT fails to recognize these members as intrinsic. - // The JIT recognizes these methods and properties by name and signature: if either is changed, the JIT will no longer recognize the member. - // Some methods declared here are not strictly intrinsic, but delegate to an intrinsic method. For example, only one overload of CopyTo() - - public partial struct Vector2 - { - /// - /// The X component of the vector. - /// - public float X; - /// - /// The Y component of the vector. - /// - public float Y; - - #region Constructors - /// - /// Constructs a vector whose elements are all the single specified value. - /// - /// The element to fill the vector with. - [Intrinsic] - public Vector2(float value) : this(value, value) { } - - /// - /// Constructs a vector with the given individual elements. - /// - /// The X component. - /// The Y component. - [Intrinsic] - public Vector2(float x, float y) - { - X = x; - Y = y; - } - #endregion Constructors - - #region Public Instance Methods - /// - /// Copies the contents of the vector into the given array. - /// - /// The destination array. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void CopyTo(float[] array) - { - CopyTo(array, 0); - } - - /// - /// Copies the contents of the vector into the given array, starting from the given index. - /// - /// If array is null. - /// If array is multidimensional. - /// If index is greater than end of the array or index is less than zero. - /// If number of elements in source vector is greater than those available in destination array - /// or if there are not enough elements to copy. - [Intrinsic] - public readonly void CopyTo(float[] array, int index) - { - if (array == null) - { - // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. - throw new NullReferenceException(SR.Arg_NullArgumentNullRef); - } - if (index < 0 || index >= array.Length) - { - throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); - } - if ((array.Length - index) < 2) - { - throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); - } - array[index] = X; - array[index + 1] = Y; - } - - /// - /// Returns a boolean indicating whether the given Vector2 is equal to this Vector2 instance. - /// - /// The Vector2 to compare this instance to. - /// True if the other Vector2 is equal to this instance; False otherwise. - [Intrinsic] - public readonly bool Equals(Vector2 other) - { - return this == other; - } - #endregion Public Instance Methods - - #region Public Static Methods - /// - /// Returns the dot product of two vectors. - /// - /// The first vector. - /// The second vector. - /// The dot product. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dot(Vector2 value1, Vector2 value2) - { - return value1.X * value2.X + - value1.Y * value2.Y; - } - - /// - /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. - /// - /// The first source vector. - /// The second source vector. - /// The minimized vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Min(Vector2 value1, Vector2 value2) - { - return new Vector2( - (value1.X < value2.X) ? value1.X : value2.X, - (value1.Y < value2.Y) ? value1.Y : value2.Y); - } - - /// - /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors - /// - /// The first source vector - /// The second source vector - /// The maximized vector - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Max(Vector2 value1, Vector2 value2) - { - return new Vector2( - (value1.X > value2.X) ? value1.X : value2.X, - (value1.Y > value2.Y) ? value1.Y : value2.Y); - } - - /// - /// Returns a vector whose elements are the absolute values of each of the source vector's elements. - /// - /// The source vector. - /// The absolute value vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 Abs(Vector2 value) - { - return new Vector2(MathF.Abs(value.X), MathF.Abs(value.Y)); - } - - /// - /// Returns a vector whose elements are the square root of each of the source vector's elements. - /// - /// The source vector. - /// The square root vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 SquareRoot(Vector2 value) - { - return new Vector2(MathF.Sqrt(value.X), MathF.Sqrt(value.Y)); - } - #endregion Public Static Methods - - #region Public Static Operators - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator +(Vector2 left, Vector2 right) - { - return new Vector2(left.X + right.X, left.Y + right.Y); - } - - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator -(Vector2 left, Vector2 right) - { - return new Vector2(left.X - right.X, left.Y - right.Y); - } - - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator *(Vector2 left, Vector2 right) - { - return new Vector2(left.X * right.X, left.Y * right.Y); - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator *(float left, Vector2 right) - { - return new Vector2(left, left) * right; - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator *(Vector2 left, float right) - { - return left * new Vector2(right, right); - } - - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator /(Vector2 left, Vector2 right) - { - return new Vector2(left.X / right.X, left.Y / right.Y); - } - - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator /(Vector2 value1, float value2) - { - return value1 / new Vector2(value2); - } - - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector2 operator -(Vector2 value) - { - return Zero - value; - } - - /// - /// Returns a boolean indicating whether the two given vectors are equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are equal; False otherwise. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator ==(Vector2 left, Vector2 right) - { - return left.X == right.X && - left.Y == right.Y; - } - - /// - /// Returns a boolean indicating whether the two given vectors are not equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are not equal; False if they are equal. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator !=(Vector2 left, Vector2 right) - { - return !(left == right); - } - #endregion Public Static Operators - } -} diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs index b156f42..1c9a1d6 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs @@ -13,147 +13,259 @@ namespace System.Numerics [Intrinsic] public partial struct Vector3 : IEquatable, IFormattable { - #region Public Static Properties - /// - /// Returns the vector (0,0,0). - /// + /// The X component of the vector. + public float X; + + /// The Y component of the vector. + public float Y; + + /// The Z component of the vector. + public float Z; + + /// Constructs a vector whose elements are all the single specified value. + /// The element to fill the vector with. + [Intrinsic] + public Vector3(float value) : this(value, value, value) + { + } + + /// Constructs a Vector3 from the given Vector2 and a third value. + /// The Vector to extract X and Y components from. + /// The Z component. + [Intrinsic] + public Vector3(Vector2 value, float z) : this(value.X, value.Y, z) + { + } + + /// Constructs a vector with the given individual elements. + /// The X component. + /// The Y component. + /// The Z component. + [Intrinsic] + public Vector3(float x, float y, float z) + { + X = x; + Y = y; + Z = z; + } + + /// Returns the vector (0,0,0). public static Vector3 Zero { [Intrinsic] - get - { - return default; - } + get => default; } - /// - /// Returns the vector (1,1,1). - /// + + /// Returns the vector (1,1,1). public static Vector3 One { [Intrinsic] - get - { - return new Vector3(1.0f, 1.0f, 1.0f); - } + get => new Vector3(1.0f); } - /// - /// Returns the vector (1,0,0). - /// - public static Vector3 UnitX { get { return new Vector3(1.0f, 0.0f, 0.0f); } } - /// - /// Returns the vector (0,1,0). - /// - public static Vector3 UnitY { get { return new Vector3(0.0f, 1.0f, 0.0f); } } - /// - /// Returns the vector (0,0,1). - /// - public static Vector3 UnitZ { get { return new Vector3(0.0f, 0.0f, 1.0f); } } - #endregion Public Static Properties - - #region Public Instance Methods - - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() + + /// Returns the vector (1,0,0). + public static Vector3 UnitX { - return HashCode.Combine(this.X, this.Y, this.Z); + get => new Vector3(1.0f, 0.0f, 0.0f); } - /// - /// Returns a boolean indicating whether the given Object is equal to this Vector3 instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this Vector3; False otherwise. + /// Returns the vector (0,1,0). + public static Vector3 UnitY + { + get => new Vector3(0.0f, 1.0f, 0.0f); + } + + /// Returns the vector (0,0,1). + public static Vector3 UnitZ + { + get => new Vector3(0.0f, 0.0f, 1.0f); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public override readonly bool Equals(object? obj) + public static Vector3 operator +(Vector3 left, Vector3 right) { - return (obj is Vector3 other) && Equals(other); + return new Vector3( + left.X + right.X, + left.Y + right.Y, + left.Z + right.Z + ); } - /// - /// Returns a String representing this Vector3 instance. - /// - /// The string representation. - public override readonly string ToString() + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator /(Vector3 left, Vector3 right) { - return ToString("G", CultureInfo.CurrentCulture); + return new Vector3( + left.X / right.X, + left.Y / right.Y, + left.Z / right.Z + ); } - /// - /// Returns a String representing this Vector3 instance, using the specified format to format individual elements. - /// - /// The format of individual elements. - /// The string representation. - public readonly string ToString(string? format) + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator /(Vector3 value1, float value2) { - return ToString(format, CultureInfo.CurrentCulture); + return value1 / new Vector3(value2); } - /// - /// Returns a String representing this Vector3 instance, using the specified format to format individual elements - /// and the given IFormatProvider. - /// - /// The format of individual elements. - /// The format provider to use when formatting elements. - /// The string representation. - public readonly string ToString(string? format, IFormatProvider? formatProvider) + /// Returns a boolean indicating whether the two given vectors are equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are equal; False otherwise. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator ==(Vector3 left, Vector3 right) { - StringBuilder sb = new StringBuilder(); - string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; - sb.Append('<'); - sb.Append(((IFormattable)this.X).ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(((IFormattable)this.Y).ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(((IFormattable)this.Z).ToString(format, formatProvider)); - sb.Append('>'); - return sb.ToString(); + return (left.X == right.X) + && (left.Y == right.Y) + && (left.Z == right.Z); } - /// - /// Returns the length of the vector. - /// - /// The vector's length. + /// Returns a boolean indicating whether the two given vectors are not equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are not equal; False if they are equal. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Length() + public static bool operator !=(Vector3 left, Vector3 right) { - float ls = Vector3.Dot(this, this); - return MathF.Sqrt(ls); + return !(left == right); } - /// - /// Returns the length of the vector squared. This operation is cheaper than Length(). - /// - /// The vector's length squared. + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float LengthSquared() + public static Vector3 operator *(Vector3 left, Vector3 right) + { + return new Vector3( + left.X * right.X, + left.Y * right.Y, + left.Z * right.Z + ); + } + + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator *(Vector3 left, float right) + { + return left * new Vector3(right); + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator *(float left, Vector3 right) { - return Vector3.Dot(this, this); + return right * left; } - #endregion Public Instance Methods - #region Public Static Methods - /// - /// Returns the Euclidean distance between the two given points. - /// + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator -(Vector3 left, Vector3 right) + { + return new Vector3( + left.X - right.X, + left.Y - right.Y, + left.Z - right.Z + ); + } + + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 operator -(Vector3 value) + { + return Zero - value; + } + + /// Returns a vector whose elements are the absolute values of each of the source vector's elements. + /// The source vector. + /// The absolute value vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Abs(Vector3 value) + { + return new Vector3( + MathF.Abs(value.X), + MathF.Abs(value.Y), + MathF.Abs(value.Z) + ); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Add(Vector3 left, Vector3 right) + { + return left + right; + } + + /// Restricts a vector between a min and max value. + /// The source vector. + /// The minimum value. + /// The maximum value. + /// The restricted vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) + { + // We must follow HLSL behavior in the case user specified min value is bigger than max value. + return Min(Max(value1, min), max); + } + + /// Computes the cross product of two vectors. + /// The first vector. + /// The second vector. + /// The cross product. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Cross(Vector3 vector1, Vector3 vector2) + { + return new Vector3( + (vector1.Y * vector2.Z) - (vector1.Z * vector2.Y), + (vector1.Z * vector2.X) - (vector1.X * vector2.Z), + (vector1.X * vector2.Y) - (vector1.Y * vector2.X) + ); + } + + /// Returns the Euclidean distance between the two given points. /// The first point. /// The second point. /// The distance. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Distance(Vector3 value1, Vector3 value2) { - Vector3 difference = value1 - value2; - float ls = Vector3.Dot(difference, difference); - return MathF.Sqrt(ls); + float distanceSquared = DistanceSquared(value1, value2); + return MathF.Sqrt(distanceSquared); } - /// - /// Returns the Euclidean distance squared between the two given points. - /// + /// Returns the Euclidean distance squared between the two given points. /// The first point. /// The second point. /// The distance squared. @@ -161,112 +273,180 @@ namespace System.Numerics public static float DistanceSquared(Vector3 value1, Vector3 value2) { Vector3 difference = value1 - value2; - return Vector3.Dot(difference, difference); + return Dot(difference, difference); } - /// - /// Returns a vector with the same direction as the given vector, but with a length of 1. - /// - /// The vector to normalize. - /// The normalized vector. + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Normalize(Vector3 value) + public static Vector3 Divide(Vector3 left, Vector3 right) + { + return left / right; + } + + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Divide(Vector3 left, float divisor) { - float length = value.Length(); - return value / length; + return left / divisor; } - /// - /// Computes the cross product of two vectors. - /// + /// Returns the dot product of two vectors. /// The first vector. /// The second vector. - /// The cross product. + /// The dot product. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Cross(Vector3 vector1, Vector3 vector2) + public static float Dot(Vector3 vector1, Vector3 vector2) { - return new Vector3( - vector1.Y * vector2.Z - vector1.Z * vector2.Y, - vector1.Z * vector2.X - vector1.X * vector2.Z, - vector1.X * vector2.Y - vector1.Y * vector2.X); + return (vector1.X * vector2.X) + + (vector1.Y * vector2.Y) + + (vector1.Z * vector2.Z); } - /// - /// Returns the reflection of a vector off a surface that has the specified normal. - /// - /// The source vector. - /// The normal of the surface being reflected off. - /// The reflected vector. + /// Linearly interpolates between two vectors based on the given weighting. + /// The first source vector. + /// The second source vector. + /// Value between 0 and 1 indicating the weight of the second source vector. + /// The interpolated vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Reflect(Vector3 vector, Vector3 normal) + public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) { - float dot = Vector3.Dot(vector, normal); - Vector3 temp = normal * dot * 2f; - return vector - temp; + return (value1 * (1f - amount)) + (value2 * amount); } - /// - /// Restricts a vector between a min and max value. - /// - /// The source vector. - /// The minimum value. - /// The maximum value. - /// The restricted vector. + /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors. + /// The first source vector. + /// The second source vector. + /// The maximized vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) + public static Vector3 Max(Vector3 value1, Vector3 value2) { - // We must follow HLSL behavior in the case user specified min value is bigger than max value. - return Vector3.Min(Vector3.Max(value1, min), max); + return new Vector3( + (value1.X > value2.X) ? value1.X : value2.X, + (value1.Y > value2.Y) ? value1.Y : value2.Y, + (value1.Z > value2.Z) ? value1.Z : value2.Z + ); } - /// - /// Linearly interpolates between two vectors based on the given weighting. - /// + /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. /// The first source vector. /// The second source vector. - /// Value between 0 and 1 indicating the weight of the second source vector. - /// The interpolated vector. + /// The minimized vector. + [Intrinsic] + public static Vector3 Min(Vector3 value1, Vector3 value2) + { + return new Vector3( + (value1.X < value2.X) ? value1.X : value2.X, + (value1.Y < value2.Y) ? value1.Y : value2.Y, + (value1.Z < value2.Z) ? value1.Z : value2.Z + ); + } + + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) + public static Vector3 Multiply(Vector3 left, Vector3 right) { - Vector3 firstInfluence = value1 * (1f - amount); - Vector3 secondInfluence = value2 * amount; - return firstInfluence + secondInfluence; + return left * right; } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. - /// The transformation matrix. - /// The transformed vector. + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Transform(Vector3 position, Matrix4x4 matrix) + public static Vector3 Multiply(Vector3 left, float right) + { + return left * right; + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Multiply(float left, Vector3 right) + { + return left * right; + } + + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Negate(Vector3 value) + { + return -value; + } + + /// Returns a vector with the same direction as the given vector, but with a length of 1. + /// The vector to normalize. + /// The normalized vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Normalize(Vector3 value) + { + return value / value.Length(); + } + + /// Returns the reflection of a vector off a surface that has the specified normal. + /// The source vector. + /// The normal of the surface being reflected off. + /// The reflected vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Reflect(Vector3 vector, Vector3 normal) + { + float dot = Dot(vector, normal); + return vector - (2 * dot * normal); + } + + /// Returns a vector whose elements are the square root of each of the source vector's elements. + /// The source vector. + /// The square root vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 SquareRoot(Vector3 value) { return new Vector3( - position.X * matrix.M11 + position.Y * matrix.M21 + position.Z * matrix.M31 + matrix.M41, - position.X * matrix.M12 + position.Y * matrix.M22 + position.Z * matrix.M32 + matrix.M42, - position.X * matrix.M13 + position.Y * matrix.M23 + position.Z * matrix.M33 + matrix.M43); + MathF.Sqrt(value.X), + MathF.Sqrt(value.Y), + MathF.Sqrt(value.Z) + ); } - /// - /// Transforms a vector normal by the given matrix. - /// - /// The source vector. + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector3 Subtract(Vector3 left, Vector3 right) + { + return left - right; + } + + /// Transforms a vector by the given matrix. + /// The source vector. /// The transformation matrix. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 TransformNormal(Vector3 normal, Matrix4x4 matrix) + public static Vector3 Transform(Vector3 position, Matrix4x4 matrix) { return new Vector3( - normal.X * matrix.M11 + normal.Y * matrix.M21 + normal.Z * matrix.M31, - normal.X * matrix.M12 + normal.Y * matrix.M22 + normal.Z * matrix.M32, - normal.X * matrix.M13 + normal.Y * matrix.M23 + normal.Z * matrix.M33); + (position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43 + ); } - /// - /// Transforms a vector by the given Quaternion rotation value. - /// + /// Transforms a vector by the given Quaternion rotation value. /// The source vector to be rotated. /// The rotation to apply. /// The transformed vector. @@ -290,109 +470,137 @@ namespace System.Numerics return new Vector3( value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2), value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2), - value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2)); + value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2) + ); } - #endregion Public Static Methods - - #region Public operator methods - - // All these methods should be inlined as they are implemented - // over JIT intrinsics - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. + /// Transforms a vector normal by the given matrix. + /// The source vector. + /// The transformation matrix. + /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Add(Vector3 left, Vector3 right) + public static Vector3 TransformNormal(Vector3 normal, Matrix4x4 matrix) { - return left + right; + return new Vector3( + (normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31), + (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32), + (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33) + ); } - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. + /// Copies the contents of the vector into the given array. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Subtract(Vector3 left, Vector3 right) + public readonly void CopyTo(float[] array) { - return left - right; + CopyTo(array, 0); } - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. + /// Copies the contents of the vector into the given array, starting from index. + /// If array is null. + /// If array is multidimensional. + /// If index is greater than end of the array or index is less than zero. + /// If number of elements in source vector is greater than those available in destination array. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Multiply(Vector3 left, Vector3 right) + public readonly void CopyTo(float[] array, int index) { - return left * right; + if (array is null) + { + // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. + throw new NullReferenceException(SR.Arg_NullArgumentNullRef); + } + + if ((index < 0) || (index >= array.Length)) + { + throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); + } + + if ((array.Length - index) < 3) + { + throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); + } + + array[index] = X; + array[index + 1] = Y; + array[index + 2] = Z; } - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. + /// Returns a boolean indicating whether the given Object is equal to this Vector3 instance. + /// The Object to compare against. + /// True if the Object is equal to this Vector3; False otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Multiply(Vector3 left, float right) + public override readonly bool Equals(object? obj) { - return left * right; + return (obj is Vector3 other) && Equals(other); } - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Multiply(float left, Vector3 right) + /// Returns a boolean indicating whether the given Vector3 is equal to this Vector3 instance. + /// The Vector3 to compare this instance to. + /// True if the other Vector3 is equal to this instance; False otherwise. + [Intrinsic] + public readonly bool Equals(Vector3 other) { - return left * right; + return this == other; } - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Divide(Vector3 left, Vector3 right) + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() { - return left / right; + return HashCode.Combine(X, Y, Z); } - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. + /// Returns the length of the vector. + /// The vector's length. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Divide(Vector3 left, float divisor) + public readonly float Length() { - return left / divisor; + float lengthSquared = LengthSquared(); + return MathF.Sqrt(lengthSquared); } - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. + /// Returns the length of the vector squared. This operation is cheaper than Length(). + /// The vector's length squared. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Negate(Vector3 value) + public readonly float LengthSquared() { - return -value; + return Dot(this, this); + } + + /// Returns a String representing this Vector3 instance. + /// The string representation. + public override readonly string ToString() + { + return ToString("G", CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector3 instance, using the specified format to format individual elements. + /// The format of individual elements. + /// The string representation. + public readonly string ToString(string? format) + { + return ToString(format, CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector3 instance, using the specified format to format individual elements and the given IFormatProvider. + /// The format of individual elements. + /// The format provider to use when formatting elements. + /// The string representation. + public readonly string ToString(string? format, IFormatProvider? formatProvider) + { + StringBuilder sb = new StringBuilder(); + string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; + sb.Append('<'); + sb.Append(X.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Y.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Z.ToString(format, formatProvider)); + sb.Append('>'); + return sb.ToString(); } - #endregion Public operator methods } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3_Intrinsics.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3_Intrinsics.cs deleted file mode 100644 index 51c8046..0000000 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3_Intrinsics.cs +++ /dev/null @@ -1,313 +0,0 @@ -// Licensed to the .NET Foundation under one or more agreements. -// The .NET Foundation licenses this file to you under the MIT license. - -using System.Runtime.CompilerServices; - -namespace System.Numerics -{ - // This file contains the definitions for all of the JIT intrinsic methods and properties that are recognized by the current x64 JIT compiler. - // The implementation defined here is used in any circumstance where the JIT fails to recognize these members as intrinsic. - // The JIT recognizes these methods and properties by name and signature: if either is changed, the JIT will no longer recognize the member. - // Some methods declared here are not strictly intrinsic, but delegate to an intrinsic method. For example, only one overload of CopyTo() - // is actually recognized by the JIT, but both are here for simplicity. - - public partial struct Vector3 - { - /// - /// The X component of the vector. - /// - public float X; - /// - /// The Y component of the vector. - /// - public float Y; - /// - /// The Z component of the vector. - /// - public float Z; - - #region Constructors - /// - /// Constructs a vector whose elements are all the single specified value. - /// - /// The element to fill the vector with. - [Intrinsic] - public Vector3(float value) : this(value, value, value) { } - - /// - /// Constructs a Vector3 from the given Vector2 and a third value. - /// - /// The Vector to extract X and Y components from. - /// The Z component. - [Intrinsic] - public Vector3(Vector2 value, float z) : this(value.X, value.Y, z) { } - - /// - /// Constructs a vector with the given individual elements. - /// - /// The X component. - /// The Y component. - /// The Z component. - [Intrinsic] - public Vector3(float x, float y, float z) - { - X = x; - Y = y; - Z = z; - } - #endregion Constructors - - #region Public Instance Methods - /// - /// Copies the contents of the vector into the given array. - /// - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void CopyTo(float[] array) - { - CopyTo(array, 0); - } - - /// - /// Copies the contents of the vector into the given array, starting from index. - /// - /// If array is null. - /// If array is multidimensional. - /// If index is greater than end of the array or index is less than zero. - /// If number of elements in source vector is greater than those available in destination array. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void CopyTo(float[] array, int index) - { - if (array == null) - { - // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. - throw new NullReferenceException(SR.Arg_NullArgumentNullRef); - } - if (index < 0 || index >= array.Length) - { - throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); - } - if ((array.Length - index) < 3) - { - throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); - } - array[index] = X; - array[index + 1] = Y; - array[index + 2] = Z; - } - - /// - /// Returns a boolean indicating whether the given Vector3 is equal to this Vector3 instance. - /// - /// The Vector3 to compare this instance to. - /// True if the other Vector3 is equal to this instance; False otherwise. - [Intrinsic] - public readonly bool Equals(Vector3 other) - { - return this == other; - } - #endregion Public Instance Methods - - #region Public Static Methods - /// - /// Returns the dot product of two vectors. - /// - /// The first vector. - /// The second vector. - /// The dot product. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dot(Vector3 vector1, Vector3 vector2) - { - return vector1.X * vector2.X + - vector1.Y * vector2.Y + - vector1.Z * vector2.Z; - } - - /// - /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. - /// - /// The first source vector. - /// The second source vector. - /// The minimized vector. - [Intrinsic] - public static Vector3 Min(Vector3 value1, Vector3 value2) - { - return new Vector3( - (value1.X < value2.X) ? value1.X : value2.X, - (value1.Y < value2.Y) ? value1.Y : value2.Y, - (value1.Z < value2.Z) ? value1.Z : value2.Z); - } - - /// - /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors. - /// - /// The first source vector. - /// The second source vector. - /// The maximized vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Max(Vector3 value1, Vector3 value2) - { - return new Vector3( - (value1.X > value2.X) ? value1.X : value2.X, - (value1.Y > value2.Y) ? value1.Y : value2.Y, - (value1.Z > value2.Z) ? value1.Z : value2.Z); - } - - /// - /// Returns a vector whose elements are the absolute values of each of the source vector's elements. - /// - /// The source vector. - /// The absolute value vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 Abs(Vector3 value) - { - return new Vector3(MathF.Abs(value.X), MathF.Abs(value.Y), MathF.Abs(value.Z)); - } - - /// - /// Returns a vector whose elements are the square root of each of the source vector's elements. - /// - /// The source vector. - /// The square root vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 SquareRoot(Vector3 value) - { - return new Vector3(MathF.Sqrt(value.X), MathF.Sqrt(value.Y), MathF.Sqrt(value.Z)); - } - #endregion Public Static Methods - - #region Public Static Operators - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator +(Vector3 left, Vector3 right) - { - return new Vector3(left.X + right.X, left.Y + right.Y, left.Z + right.Z); - } - - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator -(Vector3 left, Vector3 right) - { - return new Vector3(left.X - right.X, left.Y - right.Y, left.Z - right.Z); - } - - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator *(Vector3 left, Vector3 right) - { - return new Vector3(left.X * right.X, left.Y * right.Y, left.Z * right.Z); - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator *(Vector3 left, float right) - { - return left * new Vector3(right); - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator *(float left, Vector3 right) - { - return new Vector3(left) * right; - } - - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator /(Vector3 left, Vector3 right) - { - return new Vector3(left.X / right.X, left.Y / right.Y, left.Z / right.Z); - } - - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator /(Vector3 value1, float value2) - { - return value1 / new Vector3(value2); - } - - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector3 operator -(Vector3 value) - { - return Zero - value; - } - - /// - /// Returns a boolean indicating whether the two given vectors are equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are equal; False otherwise. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator ==(Vector3 left, Vector3 right) - { - return left.X == right.X && - left.Y == right.Y && - left.Z == right.Z; - } - - /// - /// Returns a boolean indicating whether the two given vectors are not equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are not equal; False if they are equal. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator !=(Vector3 left, Vector3 right) - { - return !(left == right); - } - #endregion Public Static Operators - } -} diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs index a34c8b0..8958bcb 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4.cs @@ -7,159 +7,275 @@ using System.Text; namespace System.Numerics { - /// - /// A structure encapsulating four single precision floating point values and provides hardware accelerated methods. - /// + /// A structure encapsulating four single precision floating point values and provides hardware accelerated methods. [Intrinsic] public partial struct Vector4 : IEquatable, IFormattable { - #region Public Static Properties - /// - /// Returns the vector (0,0,0,0). - /// + /// The X component of the vector. + public float X; + + /// The Y component of the vector. + public float Y; + + /// The Z component of the vector. + public float Z; + + /// The W component of the vector. + public float W; + + /// Constructs a vector whose elements are all the single specified value. + /// The element to fill the vector with. + [Intrinsic] + public Vector4(float value) : this(value, value, value, value) + { + } + + /// Constructs a Vector4 from the given Vector2 and a Z and W component. + /// The vector to use as the X and Y components. + /// The Z component. + /// The W component. + [Intrinsic] + public Vector4(Vector2 value, float z, float w) : this(value.X, value.Y, z, w) + { + } + + /// Constructs a Vector4 from the given Vector3 and a W component. + /// The vector to use as the X, Y, and Z components. + /// The W component. + [Intrinsic] + public Vector4(Vector3 value, float w) : this(value.X, value.Y, value.Z, w) + { + } + + /// Constructs a vector with the given individual elements. + /// W component. + /// X component. + /// Y component. + /// Z component. + [Intrinsic] + public Vector4(float x, float y, float z, float w) + { + X = x; + Y = y; + Z = z; + W = w; + } + + /// Returns the vector (0,0,0,0). public static Vector4 Zero { [Intrinsic] - get - { - return default; - } + get => default; } - /// - /// Returns the vector (1,1,1,1). - /// + + /// Returns the vector (1,1,1,1). public static Vector4 One { [Intrinsic] - get - { - return new Vector4(1.0f, 1.0f, 1.0f, 1.0f); - } + get => new Vector4(1.0f); } - /// - /// Returns the vector (1,0,0,0). - /// - public static Vector4 UnitX { get { return new Vector4(1.0f, 0.0f, 0.0f, 0.0f); } } - /// - /// Returns the vector (0,1,0,0). - /// - public static Vector4 UnitY { get { return new Vector4(0.0f, 1.0f, 0.0f, 0.0f); } } - /// - /// Returns the vector (0,0,1,0). - /// - public static Vector4 UnitZ { get { return new Vector4(0.0f, 0.0f, 1.0f, 0.0f); } } - /// - /// Returns the vector (0,0,0,1). - /// - public static Vector4 UnitW { get { return new Vector4(0.0f, 0.0f, 0.0f, 1.0f); } } - #endregion Public Static Properties - - #region Public instance methods - /// - /// Returns the hash code for this instance. - /// - /// The hash code. - public override readonly int GetHashCode() + + /// Returns the vector (1,0,0,0). + public static Vector4 UnitX { - return HashCode.Combine(this.X, this.Y, this.Z, this.W); + get => new Vector4(1.0f, 0.0f, 0.0f, 0.0f); } - /// - /// Returns a boolean indicating whether the given Object is equal to this Vector4 instance. - /// - /// The Object to compare against. - /// True if the Object is equal to this Vector4; False otherwise. + /// Returns the vector (0,1,0,0). + public static Vector4 UnitY + { + get => new Vector4(0.0f, 1.0f, 0.0f, 0.0f); + } + + /// Returns the vector (0,0,1,0). + public static Vector4 UnitZ + { + get => new Vector4(0.0f, 0.0f, 1.0f, 0.0f); + } + + /// Returns the vector (0,0,0,1). + public static Vector4 UnitW + { + get => new Vector4(0.0f, 0.0f, 0.0f, 1.0f); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public override readonly bool Equals(object? obj) + public static Vector4 operator +(Vector4 left, Vector4 right) { - return (obj is Vector4 other) && Equals(other); + return new Vector4( + left.X + right.X, + left.Y + right.Y, + left.Z + right.Z, + left.W + right.W + ); } - /// - /// Returns a String representing this Vector4 instance. - /// - /// The string representation. - public override readonly string ToString() + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator /(Vector4 left, Vector4 right) { - return ToString("G", CultureInfo.CurrentCulture); + return new Vector4( + left.X / right.X, + left.Y / right.Y, + left.Z / right.Z, + left.W / right.W + ); } - /// - /// Returns a String representing this Vector4 instance, using the specified format to format individual elements. - /// - /// The format of individual elements. - /// The string representation. - public readonly string ToString(string? format) + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator /(Vector4 value1, float value2) { - return ToString(format, CultureInfo.CurrentCulture); + return value1 / new Vector4(value2); } - /// - /// Returns a String representing this Vector4 instance, using the specified format to format individual elements - /// and the given IFormatProvider. - /// - /// The format of individual elements. - /// The format provider to use when formatting elements. - /// The string representation. - public readonly string ToString(string? format, IFormatProvider? formatProvider) + /// Returns a boolean indicating whether the two given vectors are equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are equal; False otherwise. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator ==(Vector4 left, Vector4 right) { - StringBuilder sb = new StringBuilder(); - string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; - sb.Append('<'); - sb.Append(this.X.ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(this.Y.ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(this.Z.ToString(format, formatProvider)); - sb.Append(separator); - sb.Append(' '); - sb.Append(this.W.ToString(format, formatProvider)); - sb.Append('>'); - return sb.ToString(); + return (left.X == right.X) + && (left.Y == right.Y) + && (left.Z == right.Z) + && (left.W == right.W); } - /// - /// Returns the length of the vector. This operation is cheaper than Length(). - /// - /// The vector's length. + /// Returns a boolean indicating whether the two given vectors are not equal. + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are not equal; False if they are equal. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Length() + public static bool operator !=(Vector4 left, Vector4 right) { - float ls = Vector4.Dot(this, this); - return MathF.Sqrt(ls); + return !(left == right); } - /// - /// Returns the length of the vector squared. - /// - /// The vector's length squared. + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float LengthSquared() + public static Vector4 operator *(Vector4 left, Vector4 right) + { + return new Vector4( + left.X * right.X, + left.Y * right.Y, + left.Z * right.Z, + left.W * right.W + ); + } + + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator *(Vector4 left, float right) + { + return left * new Vector4(right); + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator *(float left, Vector4 right) + { + return right * left; + } + + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator -(Vector4 left, Vector4 right) + { + return new Vector4( + left.X - right.X, + left.Y - right.Y, + left.Z - right.Z, + left.W - right.W + ); + } + + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 operator -(Vector4 value) + { + return Zero - value; + } + + /// Returns a vector whose elements are the absolute values of each of the source vector's elements. + /// The source vector. + /// The absolute value vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Abs(Vector4 value) + { + return new Vector4( + MathF.Abs(value.X), + MathF.Abs(value.Y), + MathF.Abs(value.Z), + MathF.Abs(value.W) + ); + } + + /// Adds two vectors together. + /// The first source vector. + /// The second source vector. + /// The summed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Add(Vector4 left, Vector4 right) + { + return left + right; + } + + /// Restricts a vector between a min and max value. + /// The source vector. + /// The minimum value. + /// The maximum value. + /// The restricted vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) { - return Vector4.Dot(this, this); + // We must follow HLSL behavior in the case user specified min value is bigger than max value. + return Min(Max(value1, min), max); } - #endregion Public Instance Methods - #region Public Static Methods - /// - /// Returns the Euclidean distance between the two given points. - /// + /// Returns the Euclidean distance between the two given points. /// The first point. /// The second point. /// The distance. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float Distance(Vector4 value1, Vector4 value2) { - Vector4 difference = value1 - value2; - float ls = Vector4.Dot(difference, difference); - return MathF.Sqrt(ls); + float distanceSquared = DistanceSquared(value1, value2); + return MathF.Sqrt(distanceSquared); } - /// - /// Returns the Euclidean distance squared between the two given points. - /// + /// Returns the Euclidean distance squared between the two given points. /// The first point. /// The second point. /// The distance squared. @@ -167,38 +283,44 @@ namespace System.Numerics public static float DistanceSquared(Vector4 value1, Vector4 value2) { Vector4 difference = value1 - value2; - return Vector4.Dot(difference, difference); + return Dot(difference, difference); } - /// - /// Returns a vector with the same direction as the given vector, but with a length of 1. - /// - /// The vector to normalize. - /// The normalized vector. + /// Divides the first vector by the second. + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Normalize(Vector4 vector) + public static Vector4 Divide(Vector4 left, Vector4 right) { - float length = vector.Length(); - return vector / length; + return left / right; } - /// - /// Restricts a vector between a min and max value. - /// - /// The source vector. - /// The minimum value. - /// The maximum value. - /// The restricted vector. + /// Divides the vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The result of the division. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) + public static Vector4 Divide(Vector4 left, float divisor) { - // We must follow HLSL behavior in the case user specified min value is bigger than max value. - return Vector4.Min(Vector4.Max(value1, min), max); + return left / divisor; } - /// - /// Linearly interpolates between two vectors based on the given weighting. - /// + /// Returns the dot product of two vectors. + /// The first vector. + /// The second vector. + /// The dot product. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dot(Vector4 vector1, Vector4 vector2) + { + return (vector1.X * vector2.X) + + (vector1.Y * vector2.Y) + + (vector1.Z * vector2.Z) + + (vector1.W * vector2.W); + } + + /// Linearly interpolates between two vectors based on the given weighting. /// The first source vector. /// The second source vector. /// Value between 0 and 1 indicating the weight of the second source vector. @@ -209,57 +331,127 @@ namespace System.Numerics return (value1 * (1.0f - amount)) + (value2 * amount); } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. - /// The transformation matrix. - /// The transformed vector. + /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors. + /// The first source vector. + /// The second source vector. + /// The maximized vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Transform(Vector2 position, Matrix4x4 matrix) + public static Vector4 Max(Vector4 value1, Vector4 value2) { return new Vector4( - position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M41, - position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M42, - position.X * matrix.M13 + position.Y * matrix.M23 + matrix.M43, - position.X * matrix.M14 + position.Y * matrix.M24 + matrix.M44); + (value1.X > value2.X) ? value1.X : value2.X, + (value1.Y > value2.Y) ? value1.Y : value2.Y, + (value1.Z > value2.Z) ? value1.Z : value2.Z, + (value1.W > value2.W) ? value1.W : value2.W + ); } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. - /// The transformation matrix. - /// The transformed vector. + /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. + /// The first source vector. + /// The second source vector. + /// The minimized vector. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Transform(Vector3 position, Matrix4x4 matrix) + public static Vector4 Min(Vector4 value1, Vector4 value2) { return new Vector4( - position.X * matrix.M11 + position.Y * matrix.M21 + position.Z * matrix.M31 + matrix.M41, - position.X * matrix.M12 + position.Y * matrix.M22 + position.Z * matrix.M32 + matrix.M42, - position.X * matrix.M13 + position.Y * matrix.M23 + position.Z * matrix.M33 + matrix.M43, - position.X * matrix.M14 + position.Y * matrix.M24 + position.Z * matrix.M34 + matrix.M44); + (value1.X < value2.X) ? value1.X : value2.X, + (value1.Y < value2.Y) ? value1.Y : value2.Y, + (value1.Z < value2.Z) ? value1.Z : value2.Z, + (value1.W < value2.W) ? value1.W : value2.W + ); } - /// - /// Transforms a vector by the given matrix. - /// - /// The source vector. + /// Multiplies two vectors together. + /// The first source vector. + /// The second source vector. + /// The product vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Multiply(Vector4 left, Vector4 right) + { + return left * right; + } + + /// Multiplies a vector by the given scalar. + /// The source vector. + /// The scalar value. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Multiply(Vector4 left, float right) + { + return left * right; + } + + /// Multiplies a vector by the given scalar. + /// The scalar value. + /// The source vector. + /// The scaled vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Multiply(float left, Vector4 right) + { + return left * right; + } + + /// Negates a given vector. + /// The source vector. + /// The negated vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Negate(Vector4 value) + { + return -value; + } + + /// Returns a vector with the same direction as the given vector, but with a length of 1. + /// The vector to normalize. + /// The normalized vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Normalize(Vector4 vector) + { + return vector / vector.Length(); + } + + /// Returns a vector whose elements are the square root of each of the source vector's elements. + /// The source vector. + /// The square root vector. + [Intrinsic] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 SquareRoot(Vector4 value) + { + return new Vector4( + MathF.Sqrt(value.X), + MathF.Sqrt(value.Y), + MathF.Sqrt(value.Z), + MathF.Sqrt(value.W) + ); + } + + /// Subtracts the second vector from the first. + /// The first source vector. + /// The second source vector. + /// The difference vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Subtract(Vector4 left, Vector4 right) + { + return left - right; + } + + /// Transforms a vector by the given matrix. + /// The source vector. /// The transformation matrix. /// The transformed vector. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Transform(Vector4 vector, Matrix4x4 matrix) + public static Vector4 Transform(Vector2 position, Matrix4x4 matrix) { return new Vector4( - vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31 + vector.W * matrix.M41, - vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32 + vector.W * matrix.M42, - vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33 + vector.W * matrix.M43, - vector.X * matrix.M14 + vector.Y * matrix.M24 + vector.Z * matrix.M34 + vector.W * matrix.M44); + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44 + ); } - /// - /// Transforms a vector by the given Quaternion rotation value. - /// + /// Transforms a vector by the given Quaternion rotation value. /// The source vector to be rotated. /// The rotation to apply. /// The transformed vector. @@ -284,12 +476,26 @@ namespace System.Numerics value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2), value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2), value.X * (xz2 - wy2) + value.Y * (yz2 + wx2), - 1.0f); + 1.0f + ); + } + + /// Transforms a vector by the given matrix. + /// The source vector. + /// The transformation matrix. + /// The transformed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Transform(Vector3 position, Matrix4x4 matrix) + { + return new Vector4( + (position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44 + ); } - /// - /// Transforms a vector by the given Quaternion rotation value. - /// + /// Transforms a vector by the given Quaternion rotation value. /// The source vector to be rotated. /// The rotation to apply. /// The transformed vector. @@ -314,12 +520,26 @@ namespace System.Numerics value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2), value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2), value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2), - 1.0f); + 1.0f + ); } - /// - /// Transforms a vector by the given Quaternion rotation value. - /// + /// Transforms a vector by the given matrix. + /// The source vector. + /// The transformation matrix. + /// The transformed vector. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector4 Transform(Vector4 vector, Matrix4x4 matrix) + { + return new Vector4( + (vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), + (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42), + (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43), + (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44) + ); + } + + /// Transforms a vector by the given Quaternion rotation value. /// The source vector to be rotated. /// The rotation to apply. /// The transformed vector. @@ -346,106 +566,125 @@ namespace System.Numerics value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2), value.W); } - #endregion Public Static Methods - - #region Public operator methods - // All these methods should be inlines as they are implemented - // over JIT intrinsics - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. + /// Copies the contents of the vector into the given array. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Add(Vector4 left, Vector4 right) + public readonly void CopyTo(float[] array) { - return left + right; + CopyTo(array, 0); } - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. + /// Copies the contents of the vector into the given array, starting from index. + /// If array is null. + /// If array is multidimensional. + /// If index is greater than end of the array or index is less than zero. + /// If number of elements in source vector is greater than those available in destination array. + [Intrinsic] [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Subtract(Vector4 left, Vector4 right) + public readonly void CopyTo(float[] array, int index) { - return left - right; + if (array is null) + { + // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. + throw new NullReferenceException(SR.Arg_NullArgumentNullRef); + } + + if ((index < 0) || (index >= array.Length)) + { + throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); + } + + if ((array.Length - index) < 4) + { + throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); + } + + array[index] = X; + array[index + 1] = Y; + array[index + 2] = Z; + array[index + 3] = W; } - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Multiply(Vector4 left, Vector4 right) + /// Returns a boolean indicating whether the given Vector4 is equal to this Vector4 instance. + /// The Vector4 to compare this instance to. + /// True if the other Vector4 is equal to this instance; False otherwise. + [Intrinsic] + public readonly bool Equals(Vector4 other) { - return left * right; + return this == other; } - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. + /// Returns a boolean indicating whether the given Object is equal to this Vector4 instance. + /// The Object to compare against. + /// True if the Object is equal to this Vector4; False otherwise. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Multiply(Vector4 left, float right) + public override readonly bool Equals(object? obj) { - return left * new Vector4(right, right, right, right); + return (obj is Vector4 other) && Equals(other); } - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Multiply(float left, Vector4 right) + /// Returns the hash code for this instance. + /// The hash code. + public override readonly int GetHashCode() { - return new Vector4(left, left, left, left) * right; + return HashCode.Combine(X, Y, Z, W); } - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. + /// Returns the length of the vector. This operation is cheaper than Length(). + /// The vector's length. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Divide(Vector4 left, Vector4 right) + public readonly float Length() { - return left / right; + float lengthSquared = LengthSquared(); + return MathF.Sqrt(lengthSquared); } - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. + /// Returns the length of the vector squared. + /// The vector's length squared. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Divide(Vector4 left, float divisor) + public readonly float LengthSquared() { - return left / divisor; + return Dot(this, this); } - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Negate(Vector4 value) + /// Returns a String representing this Vector4 instance. + /// The string representation. + public override readonly string ToString() { - return -value; + return ToString("G", CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector4 instance, using the specified format to format individual elements. + /// The format of individual elements. + /// The string representation. + public readonly string ToString(string? format) + { + return ToString(format, CultureInfo.CurrentCulture); + } + + /// Returns a String representing this Vector4 instance, using the specified format to format individual elements + /// and the given IFormatProvider. + /// The format of individual elements. + /// The format provider to use when formatting elements. + /// The string representation. + public readonly string ToString(string? format, IFormatProvider? formatProvider) + { + StringBuilder sb = new StringBuilder(); + string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; + sb.Append('<'); + sb.Append(X.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Y.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Z.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(W.ToString(format, formatProvider)); + sb.Append('>'); + return sb.ToString(); } - #endregion Public operator methods } } diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4_Intrinsics.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4_Intrinsics.cs deleted file mode 100644 index 444bd04..0000000 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector4_Intrinsics.cs +++ /dev/null @@ -1,348 +0,0 @@ -// Licensed to the .NET Foundation under one or more agreements. -// The .NET Foundation licenses this file to you under the MIT license. - -using System.Runtime.CompilerServices; - -namespace System.Numerics -{ - // This file contains the definitions for all of the JIT intrinsic methods and properties that are recognized by the current x64 JIT compiler. - // The implementation defined here is used in any circumstance where the JIT fails to recognize these members as intrinsic. - // The JIT recognizes these methods and properties by name and signature: if either is changed, the JIT will no longer recognize the member. - // Some methods declared here are not strictly intrinsic, but delegate to an intrinsic method. For example, only one overload of CopyTo() - - public partial struct Vector4 - { - /// - /// The X component of the vector. - /// - public float X; - /// - /// The Y component of the vector. - /// - public float Y; - /// - /// The Z component of the vector. - /// - public float Z; - /// - /// The W component of the vector. - /// - public float W; - - #region Constructors - - /// - /// Constructs a vector whose elements are all the single specified value. - /// - /// The element to fill the vector with. - [Intrinsic] - public Vector4(float value) - : this(value, value, value, value) - { - } - /// - /// Constructs a vector with the given individual elements. - /// - /// W component. - /// X component. - /// Y component. - /// Z component. - [Intrinsic] - public Vector4(float x, float y, float z, float w) - { - W = w; - X = x; - Y = y; - Z = z; - } - - /// - /// Constructs a Vector4 from the given Vector2 and a Z and W component. - /// - /// The vector to use as the X and Y components. - /// The Z component. - /// The W component. - [Intrinsic] - public Vector4(Vector2 value, float z, float w) - { - X = value.X; - Y = value.Y; - Z = z; - W = w; - } - - /// - /// Constructs a Vector4 from the given Vector3 and a W component. - /// - /// The vector to use as the X, Y, and Z components. - /// The W component. - [Intrinsic] - public Vector4(Vector3 value, float w) - { - X = value.X; - Y = value.Y; - Z = value.Z; - W = w; - } - #endregion Constructors - - #region Public Instance Methods - /// - /// Copies the contents of the vector into the given array. - /// - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void CopyTo(float[] array) - { - CopyTo(array, 0); - } - - /// - /// Copies the contents of the vector into the given array, starting from index. - /// - /// If array is null. - /// If array is multidimensional. - /// If index is greater than end of the array or index is less than zero. - /// If number of elements in source vector is greater than those available in destination array. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly void CopyTo(float[] array, int index) - { - if (array == null) - { - // Match the JIT's exception type here. For perf, a NullReference is thrown instead of an ArgumentNull. - throw new NullReferenceException(SR.Arg_NullArgumentNullRef); - } - if (index < 0 || index >= array.Length) - { - throw new ArgumentOutOfRangeException(nameof(index), SR.Format(SR.Arg_ArgumentOutOfRangeException, index)); - } - if ((array.Length - index) < 4) - { - throw new ArgumentException(SR.Format(SR.Arg_ElementsInSourceIsGreaterThanDestination, index)); - } - array[index] = X; - array[index + 1] = Y; - array[index + 2] = Z; - array[index + 3] = W; - } - - /// - /// Returns a boolean indicating whether the given Vector4 is equal to this Vector4 instance. - /// - /// The Vector4 to compare this instance to. - /// True if the other Vector4 is equal to this instance; False otherwise. - [Intrinsic] - public readonly bool Equals(Vector4 other) - { - return this == other; - } - #endregion Public Instance Methods - - #region Public Static Methods - /// - /// Returns the dot product of two vectors. - /// - /// The first vector. - /// The second vector. - /// The dot product. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dot(Vector4 vector1, Vector4 vector2) - { - return vector1.X * vector2.X + - vector1.Y * vector2.Y + - vector1.Z * vector2.Z + - vector1.W * vector2.W; - } - - /// - /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. - /// - /// The first source vector. - /// The second source vector. - /// The minimized vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Min(Vector4 value1, Vector4 value2) - { - return new Vector4( - (value1.X < value2.X) ? value1.X : value2.X, - (value1.Y < value2.Y) ? value1.Y : value2.Y, - (value1.Z < value2.Z) ? value1.Z : value2.Z, - (value1.W < value2.W) ? value1.W : value2.W); - } - - /// - /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors. - /// - /// The first source vector. - /// The second source vector. - /// The maximized vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Max(Vector4 value1, Vector4 value2) - { - return new Vector4( - (value1.X > value2.X) ? value1.X : value2.X, - (value1.Y > value2.Y) ? value1.Y : value2.Y, - (value1.Z > value2.Z) ? value1.Z : value2.Z, - (value1.W > value2.W) ? value1.W : value2.W); - } - - /// - /// Returns a vector whose elements are the absolute values of each of the source vector's elements. - /// - /// The source vector. - /// The absolute value vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 Abs(Vector4 value) - { - return new Vector4(MathF.Abs(value.X), MathF.Abs(value.Y), MathF.Abs(value.Z), MathF.Abs(value.W)); - } - - /// - /// Returns a vector whose elements are the square root of each of the source vector's elements. - /// - /// The source vector. - /// The square root vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 SquareRoot(Vector4 value) - { - return new Vector4(MathF.Sqrt(value.X), MathF.Sqrt(value.Y), MathF.Sqrt(value.Z), MathF.Sqrt(value.W)); - } - #endregion Public Static Methods - - #region Public static operators - /// - /// Adds two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The summed vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator +(Vector4 left, Vector4 right) - { - return new Vector4(left.X + right.X, left.Y + right.Y, left.Z + right.Z, left.W + right.W); - } - - /// - /// Subtracts the second vector from the first. - /// - /// The first source vector. - /// The second source vector. - /// The difference vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator -(Vector4 left, Vector4 right) - { - return new Vector4(left.X - right.X, left.Y - right.Y, left.Z - right.Z, left.W - right.W); - } - - /// - /// Multiplies two vectors together. - /// - /// The first source vector. - /// The second source vector. - /// The product vector. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator *(Vector4 left, Vector4 right) - { - return new Vector4(left.X * right.X, left.Y * right.Y, left.Z * right.Z, left.W * right.W); - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator *(Vector4 left, float right) - { - return left * new Vector4(right); - } - - /// - /// Multiplies a vector by the given scalar. - /// - /// The scalar value. - /// The source vector. - /// The scaled vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator *(float left, Vector4 right) - { - return new Vector4(left) * right; - } - - /// - /// Divides the first vector by the second. - /// - /// The first source vector. - /// The second source vector. - /// The vector resulting from the division. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator /(Vector4 left, Vector4 right) - { - return new Vector4(left.X / right.X, left.Y / right.Y, left.Z / right.Z, left.W / right.W); - } - - /// - /// Divides the vector by the given scalar. - /// - /// The source vector. - /// The scalar value. - /// The result of the division. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator /(Vector4 value1, float value2) - { - return value1 / new Vector4(value2); - } - - /// - /// Negates a given vector. - /// - /// The source vector. - /// The negated vector. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static Vector4 operator -(Vector4 value) - { - return Zero - value; - } - - /// - /// Returns a boolean indicating whether the two given vectors are equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are equal; False otherwise. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator ==(Vector4 left, Vector4 right) - { - return left.X == right.X - && left.Y == right.Y - && left.Z == right.Z - && left.W == right.W; - } - - /// - /// Returns a boolean indicating whether the two given vectors are not equal. - /// - /// The first vector to compare. - /// The second vector to compare. - /// True if the vectors are not equal; False if they are equal. - [Intrinsic] - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool operator !=(Vector4 left, Vector4 right) - { - return !(left == right); - } - #endregion Public static operators - } -} -- 2.7.4