Added SIMD path for Vector3, Quaternion and Matrix4x4 operations

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

@@ -165,106 +165,22 @@
             [MethodImpl(MethodImplOptions.AggressiveInlining)]
             public static Impl operator *(in Impl left, in Impl right)
             {
-                if (Vector256.IsHardwareAccelerated)
-                {
-                    var lhs = Unsafe.BitCast<Impl, Vector512<float>>(left);
-
-                    var t0 = lhs.GetLower();
-                    var t1 = lhs.GetUpper();
-
-                    var a0 = Vector256.Shuffle(t0, Vector256.Create(0, 0, 0, 0, 4, 4, 4, 4));
-                    var a1 = Vector256.Shuffle(t1, Vector256.Create(0, 0, 0, 0, 4, 4, 4, 4));
-                    var b = Vector256.Create(right.X.AsVector128(), right.X.AsVector128());
-                    var c0 = a0 * b;
-                    var c1 = a1 * b;
-
-                    a0 = Vector256.Shuffle(t0, Vector256.Create(1, 1, 1, 1, 5, 5, 5, 5));
-                    a1 = Vector256.Shuffle(t1, Vector256.Create(1, 1, 1, 1, 5, 5, 5, 5));
-                    b = Vector256.Create(right.Y.AsVector128(), right.Y.AsVector128());
-                    var c2 = a0 * b;
-                    var c3 = a1 * b;
-
-                    a0 = Vector256.Shuffle(t0, Vector256.Create(2, 2, 2, 2, 6, 6, 6, 6));
-                    a1 = Vector256.Shuffle(t1, Vector256.Create(2, 2, 2, 2, 6, 6, 6, 6));
-                    b = Vector256.Create(right.Z.AsVector128(), right.Z.AsVector128());
-                    var c4 = a0 * b;
-                    var c5 = a1 * b;
-
-                    a0 = Vector256.Shuffle(t0, Vector256.Create(3, 3, 3, 3, 7, 7, 7, 7));
-                    a1 = Vector256.Shuffle(t1, Vector256.Create(3, 3, 3, 3, 7, 7, 7, 7));
-                    b = Vector256.Create(right.W.AsVector128(), right.W.AsVector128());
-
-                    var n0 = c0 + c2 + c4 + (a0 * b);
-                    var n1 = c1 + c3 + c5 + (a1 * b);
-
-                    var result = Vector512.Create(n0, n1);
-                    return Unsafe.BitCast<Vector512<float>, Impl>(result);
-                }
-                else if (Vector128.IsHardwareAccelerated)
-                {
-                    Impl result;
-
-                    var rowX = right.X.AsVector128();
-                    var rowY = right.Y.AsVector128();
-                    var rowZ = right.Z.AsVector128();
-                    var rowW = right.W.AsVector128();
-
-                    var brodXx = Vector128.Create(left.X.X);
-                    var brodXy = Vector128.Create(left.X.Y);
-                    var brodXz = Vector128.Create(left.X.Z);
-                    var brodXw = Vector128.Create(left.X.W);
-                    result.X = (brodXx * rowX + brodXy * rowY + brodXz * rowZ + brodXw * rowW).AsVector4();
-
-                    var brodYx = Vector128.Create(left.Y.X);
-                    var brodYy = Vector128.Create(left.Y.Y);
-                    var brodYz = Vector128.Create(left.Y.Z);
-                    var brodYw = Vector128.Create(left.Y.W);
-                    result.Y = (brodYx * rowX + brodYy * rowY + brodYz * rowZ + brodYw * rowW).AsVector4();
-
-                    var brodZx = Vector128.Create(left.Z.X);
-                    var brodZy = Vector128.Create(left.Z.Y);
-                    var brodZz = Vector128.Create(left.Z.Z);
-                    var brodZw = Vector128.Create(left.Z.W);
-                    result.Z = (brodZx * rowX + brodZy * rowY + brodZz * rowZ + brodZw * rowW).AsVector4();
-
-                    var brodWx = Vector128.Create(left.W.X);
-                    var brodWy = Vector128.Create(left.W.Y);
-                    var brodWz = Vector128.Create(left.W.Z);
-                    var brodWw = Vector128.Create(left.W.W);
-                    result.W = (brodWx * rowX + brodWy * rowY + brodWz * rowZ + brodWw * rowW).AsVector4();
-
-                    return result;
-                }
-                else
-                {
-                    Impl 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;
+                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;
+            }
 
-                    return result;
-                }
+            [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;
             }
 
             [MethodImpl(MethodImplOptions.AggressiveInlining)]

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

@@ -118,7 +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)
         {
-            return Concatenate(Inverse(value2), value1);
+            return value1 * Inverse(value2);
         }
 
         /// <summary>Returns a value that indicates whether two quaternions are equal.</summary>
@@ -159,24 +159,11 @@
             {
                 var left = value1.AsVector128();
                 var right = value2.AsVector128();
-
-                var l0012 = Vector128.Shuffle(left, Vector128.Create(0, 0, 1, 2));
-                var l1120 = Vector128.Shuffle(left, Vector128.Create(1, 1, 2, 0));
-                var r0333 = Vector128.Shuffle(right, Vector128.Create(0, 3, 3, 3));
-                var r1201 = Vector128.Shuffle(right, Vector128.Create(1, 2, 0, 1));
-
-                var t0 = l0012 * r0333 + l1120 * r1201;
-                var mask = Vector128.Create(0x80000000, 0, 0, 0);
-                var t0m = Vector128.Xor(t0, mask.AsSingle());
-
-                var l2201 = Vector128.Shuffle(left, Vector128.Create(2, 2, 0, 1));
-                var l3333 = Vector128.Shuffle(left, Vector128.Create(3, 3, 3, 3));
-                var r2120 = Vector128.Shuffle(right, Vector128.Create(2, 1, 2, 0));
-                var r3012 = Vector128.Shuffle(right, Vector128.Create(3, 0, 1, 2));
-
-                var t1 = l3333 * r3012 - l2201 * r2120 + t0m;
-                var result = Vector128.Shuffle(t1, Vector128.Create(1, 2, 3, 0));
-
+
+                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);
+                result += (Vector128.Shuffle(right, Vector128.Create(1, 0, 3, 2)) * left.GetElementUnsafe(2)) * Vector128.Create(-1.0f, +1.0f, +1.0f, -1.0f);
                 return Unsafe.BitCast<Vector128<float>, Quaternion>(result);
             }
             else
@@ -269,62 +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)
-        {
-            if (Vector128.IsHardwareAccelerated)
-            {
-                var left = Unsafe.BitCast<Quaternion, Vector128<float>>(value1);
-                var right = Unsafe.BitCast<Quaternion, Vector128<float>>(value2);
-
-                var c = Cross(right, left);
-                var dot = Vector3.Dot(right.AsVector3(), left.AsVector3());
-
-                var t0 = right * Vector128.Shuffle(left, Vector128.Create(3, 3, 3, 3));
-                var t1 = left * Vector128.Shuffle(right, Vector128.Create(3, 3, 3, 3)) + c;
-                var t2 = t0 + t1;
-                var ans = Unsafe.BitCast<Vector128<float>, Quaternion>(t2);
-                ans.W = t0.GetElement(3) - dot;
-                return ans;
-
-                static Vector128<float> Cross(Vector128<float> l, Vector128<float> r)
-                {
-                    return (Vector128.Shuffle(l, Vector128.Create(1, 2, 0, 3)) *
-                            Vector128.Shuffle(r, Vector128.Create(2, 0, 1, 3))) -
-                           (Vector128.Shuffle(l, Vector128.Create(2, 0, 1, 3)) *
-                            Vector128.Shuffle(r, Vector128.Create(1, 2, 0, 3)));
-                }
-            }
-            else
-            {
-                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>