Perform some additional cleanup of the System.Numerics.Vector and Matrix types

Author: tannergooding

src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix3x2.Impl.cs

@@ -72,8 +72,7 @@ readonly get
                     {
                         ThrowHelper.ThrowArgumentOutOfRangeException();
                     }
-
-                    return Unsafe.Add(ref Unsafe.AsRef(in this.X), row)[column];
+                    return Unsafe.Add(ref Unsafe.AsRef(in X), row)[column];
                 }
 
                 [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -83,7 +82,7 @@ readonly get
                     {
                         ThrowHelper.ThrowArgumentOutOfRangeException();
                     }
-                    Unsafe.Add(ref this.X, row)[column] = value;
+                    Unsafe.Add(ref X, row)[column] = value;
                 }
             }
 
@@ -514,7 +513,7 @@ public readonly float GetDeterminant()
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public override readonly int GetHashCode() => HashCode.Combine(X, Y, Z);
 
-            bool IEquatable<Impl>.Equals(Impl other) => Equals(in other);
+            readonly bool IEquatable<Impl>.Equals(Impl other) => Equals(in other);
         }
     }
 }

src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs

@@ -14,8 +14,6 @@ namespace System.Numerics
     [Intrinsic]
     public struct Plane : IEquatable<Plane>
     {
-        private const float NormalizeEpsilon = 1.192092896e-07f; // smallest such that 1.0+NormalizeEpsilon != 1.0
-
         /// <summary>The normal vector of the plane.</summary>
         public Vector3 Normal;
 
@@ -74,49 +72,15 @@ public Plane(Vector4 value)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Plane CreateFromVertices(Vector3 point1, Vector3 point2, Vector3 point3)
         {
-            if (Vector128.IsHardwareAccelerated)
-            {
-                Vector3 a = point2 - point1;
-                Vector3 b = point3 - point1;
-
-                // N = Cross(a, b)
-                Vector3 n = Vector3.Cross(a, b);
-                Vector3 normal = Vector3.Normalize(n);
-
-                // D = - Dot(N, point1)
-                float d = -Vector3.Dot(normal, point1);
-
-                return Create(normal, d);
-            }
-            else
-            {
-                float ax = point2.X - point1.X;
-                float ay = point2.Y - point1.Y;
-                float az = point2.Z - point1.Z;
+            // This implementation is based on the DirectX Math Library XMPlaneFromPoints method
+            // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl
 
-                float bx = point3.X - point1.X;
-                float by = point3.Y - point1.Y;
-                float bz = point3.Z - point1.Z;
+            Vector3 normal = Vector3.Normalize(Vector3.Cross(point2 - point1, point3 - point1));
 
-                // N=Cross(a,b)
-                float nx = ay * bz - az * by;
-                float ny = az * bx - ax * bz;
-                float nz = ax * by - ay * bx;
-
-                // Normalize(N)
-                float ls = nx * nx + ny * ny + nz * nz;
-                float invNorm = 1.0f / float.Sqrt(ls);
-
-                Vector3 normal = Vector3.Create(
-                    nx * invNorm,
-                    ny * invNorm,
-                    nz * invNorm);
-
-                return Create(
-                    normal,
-                    -(normal.X * point1.X + normal.Y * point1.Y + normal.Z * point1.Z)
-                );
-            }
+            return Create(
+                normal,
+                -Vector3.Dot(normal, point1)
+            );
         }
 
         /// <summary>Calculates the dot product of a plane and a 4-dimensional vector.</summary>
@@ -131,29 +95,40 @@ public static Plane CreateFromVertices(Vector3 point1, Vector3 point2, Vector3 p
         /// <param name="plane">The plane.</param>
         /// <param name="value">The 3-dimensional vector.</param>
         /// <returns>The dot product.</returns>
-        public static float DotCoordinate(Plane plane, Vector3 value) => Vector3.Dot(plane.Normal, value) + plane.D;
+        public static float DotCoordinate(Plane plane, Vector3 value)
+        {
+            // This implementation is based on the DirectX Math Library XMPlaneDotCoord method
+            // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl
+
+            return Dot(plane, Vector4.Create(value, 1.0f));
+        }
 
         /// <summary>Returns the dot product of a specified three-dimensional vector and the <see cref="Normal" /> vector of this plane.</summary>
         /// <param name="plane">The plane.</param>
         /// <param name="value">The three-dimensional vector.</param>
         /// <returns>The dot product.</returns>
-        public static float DotNormal(Plane plane, Vector3 value) => Vector3.Dot(plane.Normal, value);
+        public static float DotNormal(Plane plane, Vector3 value)
+        {
+            // This implementation is based on the DirectX Math Library XMPlaneDotNormal method
+            // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl
+
+            return Vector3.Dot(plane.Normal, value);
+        }
 
         /// <summary>Creates a new <see cref="Plane" /> object whose normal vector is the source plane's normal vector normalized.</summary>
         /// <param name="value">The source plane.</param>
         /// <returns>The normalized plane.</returns>
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Plane Normalize(Plane value)
         {
-            float normalLengthSquared = value.Normal.LengthSquared();
+            // This implementation is based on the DirectX Math Library XMPlaneNormalize method
+            // https://github.com/microsoft/DirectXMath/blob/master/Inc/DirectXMathMisc.inl
 
-            if (float.Abs(normalLengthSquared - 1.0f) < NormalizeEpsilon)
-            {
-                // It already normalized, so we don't need to farther process.
-                return value;
-            }
+            Vector128<float> lengthSquared = Vector128.Create(value.Normal.LengthSquared());
 
-            return (value.AsVector128() / float.Sqrt(normalLengthSquared)).AsPlane();
+            return Vector128.AndNot(
+                (value.AsVector128() / Vector128.Sqrt(lengthSquared)),
+                Vector128.Equals(lengthSquared, Vector128.Create(float.PositiveInfinity))
+            ).AsPlane();
         }
 
         /// <summary>Transforms a normalized plane by a 4x4 matrix.</summary>
@@ -162,64 +137,15 @@ public static Plane Normalize(Plane value)
         /// <returns>The transformed plane.</returns>
         /// <remarks><paramref name="plane" /> must already be normalized so that its <see cref="Normal" /> vector is of unit length before this method is called.</remarks>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static Plane Transform(Plane plane, Matrix4x4 matrix)
-        {
-            Matrix4x4.Invert(matrix, out Matrix4x4 m);
-
-            float x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z, w = plane.D;
-
-            return Create(
-                x * m.M11 + y * m.M12 + z * m.M13 + w * m.M14,
-                x * m.M21 + y * m.M22 + z * m.M23 + w * m.M24,
-                x * m.M31 + y * m.M32 + z * m.M33 + w * m.M34,
-                x * m.M41 + y * m.M42 + z * m.M43 + w * m.M44
-            );
-        }
+        public static Plane Transform(Plane plane, Matrix4x4 matrix) => Vector4.Transform(plane.AsVector4(), matrix).AsPlane();
 
         /// <summary>Transforms a normalized plane by a Quaternion rotation.</summary>
         /// <param name="plane">The normalized plane to transform.</param>
         /// <param name="rotation">The Quaternion rotation to apply to the plane.</param>
         /// <returns>A new plane that results from applying the Quaternion rotation.</returns>
         /// <remarks><paramref name="plane" /> must already be normalized so that its <see cref="Normal" /> vector is of unit length before this method is called.</remarks>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static Plane Transform(Plane plane, Quaternion rotation)
-        {
-            // Compute rotation matrix.
-            float x2 = rotation.X + rotation.X;
-            float y2 = rotation.Y + rotation.Y;
-            float z2 = rotation.Z + rotation.Z;
-
-            float wx2 = rotation.W * x2;
-            float wy2 = rotation.W * y2;
-            float wz2 = rotation.W * z2;
-            float xx2 = rotation.X * x2;
-            float xy2 = rotation.X * y2;
-            float xz2 = rotation.X * z2;
-            float yy2 = rotation.Y * y2;
-            float yz2 = rotation.Y * z2;
-            float zz2 = rotation.Z * z2;
-
-            float m11 = 1.0f - yy2 - zz2;
-            float m21 = xy2 - wz2;
-            float m31 = xz2 + wy2;
-
-            float m12 = xy2 + wz2;
-            float m22 = 1.0f - xx2 - zz2;
-            float m32 = yz2 - wx2;
-
-            float m13 = xz2 - wy2;
-            float m23 = yz2 + wx2;
-            float m33 = 1.0f - xx2 - yy2;
-
-            float x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z;
-
-            return Create(
-                x * m11 + y * m21 + z * m31,
-                x * m12 + y * m22 + z * m32,
-                x * m13 + y * m23 + z * m33,
-                plane.D
-            );
-        }
+        public static Plane Transform(Plane plane, Quaternion rotation) => Vector4.Transform(plane.AsVector4(), rotation).AsPlane();
 
         /// <summary>Returns a value that indicates whether two planes are equal.</summary>
         /// <param name="value1">The first plane to compare.</param>