diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs
index 492eda1b452d0d..56bd3e9fcfb07a 100644
--- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs
+++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Matrix4x4.Impl.cs
@@ -166,31 +166,10 @@ public Vector3 Translation
public static Impl operator *(in Impl left, in Impl right)
{
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;
-
+ result.X = Vector4.Transform(left.X, in right);
+ result.Y = Vector4.Transform(left.Y, in right);
+ result.Z = Vector4.Transform(left.Z, in right);
+ result.W = Vector4.Transform(left.W, in right);
return result;
}
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 ecebdb1b3d42f6..c77a258f78585e 100644
--- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs
+++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs
@@ -118,40 +118,7 @@ public readonly bool IsIdentity
/// The method defines the division operation for objects.
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);
}
/// Returns a value that indicates whether two quaternions are equal.
@@ -192,7 +159,6 @@ public readonly bool IsIdentity
{
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 +255,7 @@ public static Quaternion Add(Quaternion value1, Quaternion value2)
/// The first quaternion rotation in the series.
/// The second quaternion rotation in the series.
/// A new quaternion representing the concatenation of the rotation followed by the 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;
- }
+ public static Quaternion Concatenate(Quaternion value1, Quaternion value2) => value2 * value1;
/// Returns the conjugate of a specified quaternion.
/// The quaternion.
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 c4739ce80502c5..af7f503960d995 100644
--- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs
+++ b/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)
+ );
+ }
}
/// Computes the Euclidean distance between the two given points.
@@ -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>(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 Cross(Vector128 v1, Vector128 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;