Added SIMD path for Vector3, Quaternion and Matrix4x4 operations

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

@@ -166,31 +166,20 @@
             public static Impl operator *(in Impl left, in Impl right)
             {
                 Impl result;
+                result.X = Transform(left.X, in right);
+                result.Y = Transform(left.Y, in right);
+                result.Z = Transform(left.Z, in right);
+                result.W = Transform(left.W, in right);
+                return result;
+            }
 
-                // result.X = Transform(left.X, in right);
-                result.X = right.X * left.X.X;
-                result.X += right.Y * left.X.Y;
-                result.X += right.Z * left.X.Z;
-                result.X += right.W * left.X.W;
-
-                // result.Y = Transform(left.Y, in right);
-                result.Y = right.X * left.Y.X;
-                result.Y += right.Y * left.Y.Y;
-                result.Y += right.Z * left.Y.Z;
-                result.Y += right.W * left.Y.W;
-
-                // result.Z = Transform(left.Z, in right);
-                result.Z = right.X * left.Z.X;
-                result.Z += right.Y * left.Z.Y;
-                result.Z += right.Z * left.Z.Z;
-                result.Z += right.W * left.Z.W;
-
-                // result.W = Transform(left.W, in right);
-                result.W = right.X * left.W.X;
-                result.W += right.Y * left.W.Y;
-                result.W += right.Z * left.W.Z;
-                result.W += right.W * left.W.W;
-
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            private Vector4 Transform(Vector4 vector, in Impl matrix)
+            {
+                var result = matrix.X * vector.X;
+                result += matrix.Y * vector.Y;
+                result += matrix.Z * vector.Z;
+                result += matrix.W * vector.W;
                 return result;
             }
 

src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs

@@ -118,40 +118,7 @@
         /// <remarks>The <see cref="op_Division" /> method defines the division operation for <see cref="Quaternion" /> objects.</remarks>
         public static Quaternion operator /(Quaternion value1, Quaternion value2)
         {
-            Quaternion ans;
-
-            float q1x = value1.X;
-            float q1y = value1.Y;
-            float q1z = value1.Z;
-            float q1w = value1.W;
-
-            //-------------------------------------
-            // Inverse part.
-            float ls = value2.X * value2.X + value2.Y * value2.Y +
-                       value2.Z * value2.Z + value2.W * value2.W;
-            float invNorm = 1.0f / ls;
-
-            float q2x = -value2.X * invNorm;
-            float q2y = -value2.Y * invNorm;
-            float q2z = -value2.Z * invNorm;
-            float q2w = value2.W * invNorm;
-
-            //-------------------------------------
-            // Multiply part.
-
-            // cross(av, bv)
-            float cx = q1y * q2z - q1z * q2y;
-            float cy = q1z * q2x - q1x * q2z;
-            float cz = q1x * q2y - q1y * q2x;
-
-            float dot = q1x * q2x + q1y * q2y + q1z * q2z;
-
-            ans.X = q1x * q2w + q2x * q1w + cx;
-            ans.Y = q1y * q2w + q2y * q1w + cy;
-            ans.Z = q1z * q2w + q2z * q1w + cz;
-            ans.W = q1w * q2w - dot;
-
-            return ans;
+            return value1 * Inverse(value2);
         }
 
         /// <summary>Returns a value that indicates whether two quaternions are equal.</summary>
@@ -192,7 +159,7 @@
             {
                 var left = value1.AsVector128();
                 var right = value2.AsVector128();
-
+                
                 var result = right * left.GetElementUnsafe(3);
                 result += (Vector128.Shuffle(right, Vector128.Create(3, 2, 1, 0)) * left.GetElementUnsafe(0)) * Vector128.Create(+1.0f, -1.0f, +1.0f, -1.0f);
                 result += (Vector128.Shuffle(right, Vector128.Create(2, 3, 0, 1)) * left.GetElementUnsafe(1)) * Vector128.Create(+1.0f, +1.0f, -1.0f, -1.0f);
@@ -289,36 +256,7 @@
         /// <param name="value1">The first quaternion rotation in the series.</param>
         /// <param name="value2">The second quaternion rotation in the series.</param>
         /// <returns>A new quaternion representing the concatenation of the <paramref name="value1" /> rotation followed by the <paramref name="value2" /> rotation.</returns>
-        public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
-        {
-            Quaternion ans;
-
-            // Concatenate rotation is actually q2 * q1 instead of q1 * q2.
-            // So that's why value2 goes q1 and value1 goes q2.
-            float q1x = value2.X;
-            float q1y = value2.Y;
-            float q1z = value2.Z;
-            float q1w = value2.W;
-
-            float q2x = value1.X;
-            float q2y = value1.Y;
-            float q2z = value1.Z;
-            float q2w = value1.W;
-
-            // cross(av, bv)
-            float cx = q1y * q2z - q1z * q2y;
-            float cy = q1z * q2x - q1x * q2z;
-            float cz = q1x * q2y - q1y * q2x;
-
-            float dot = q1x * q2x + q1y * q2y + q1z * q2z;
-
-            ans.X = q1x * q2w + q2x * q1w + cx;
-            ans.Y = q1y * q2w + q2y * q1w + cy;
-            ans.Z = q1z * q2w + q2z * q1w + cz;
-            ans.W = q1w * q2w - dot;
-
-            return ans;
-        }
+        public static Quaternion Concatenate(Quaternion value1, Quaternion value2) => value2 * value1;
 
         /// <summary>Returns the conjugate of a specified quaternion.</summary>
         /// <param name="value">The quaternion.</param>

src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs

@@ -302,11 +302,23 @@ public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
         [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)
-            );
+            if (Vector128.IsHardwareAccelerated)
+            {
+                var v1 = vector1.AsVector128();
+                var v2 = vector2.AsVector128();
+                return ((Vector128.Shuffle(v1, Vector128.Create(1, 2, 0, 3)) *
+                         Vector128.Shuffle(v2, Vector128.Create(2, 0, 1, 3))) -
+                        (Vector128.Shuffle(v1, Vector128.Create(2, 0, 1, 3)) *
+                         Vector128.Shuffle(v2, Vector128.Create(1, 2, 0, 3)))).AsVector3();
+            }
+            else
+            {
+                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)
+                );
+            }
         }
 
         /// <summary>Computes the Euclidean distance between the two given points.</summary>
@@ -516,6 +528,23 @@ public static Vector3 Transform(Vector3 position, Matrix4x4 matrix)
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static Vector3 Transform(Vector3 value, Quaternion rotation)
         {
+            if (Vector128.IsHardwareAccelerated)
+            {
+                var vVector = value.AsVector128();
+                var rVector = Unsafe.BitCast<Quaternion, Vector128<float>>(rotation);
+
+                // v + 2 * (q x (q.W  * v + q x v)
+                return (vVector + Vector128.Create(2f) * Cross(rVector, Vector128.Shuffle(rVector, Vector128.Create(3, 3, 3, 3)) * vVector + Cross(rVector, vVector))).AsVector3();
+
+                static Vector128<float> Cross(Vector128<float> v1, Vector128<float> v2)
+                {
+                    return (Vector128.Shuffle(v1, Vector128.Create(1, 2, 0, 3)) *
+                            Vector128.Shuffle(v2, Vector128.Create(2, 0, 1, 3))) -
+                           (Vector128.Shuffle(v1, Vector128.Create(2, 0, 1, 3)) *
+                            Vector128.Shuffle(v2, Vector128.Create(1, 2, 0, 3)));
+                }
+            }
+
             float x2 = rotation.X + rotation.X;
             float y2 = rotation.Y + rotation.Y;
             float z2 = rotation.Z + rotation.Z;