From 38bb586e051d4976e02fb7b32f901142ddd075cb Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 11 Mar 2024 17:20:56 +0100 Subject: [PATCH 1/7] Added SIMD paths for: Vector3.Cross(Vector3 vector1, Vector3 vector2) Vector3.Transform(Vector3 value, Quaternion rotation) --- .../src/System/Numerics/Vector3.cs | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) 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..1adabd7587240d 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs @@ -302,6 +302,16 @@ public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector3 Cross(Vector3 vector1, Vector3 vector2) { + 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))); + } + return new Vector3( (vector1.Y * vector2.Z) - (vector1.Z * vector2.Y), (vector1.Z * vector2.X) - (vector1.X * vector2.Z), @@ -516,6 +526,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(qVector, Vector128.Shuffle(qVector, Vector128.Create(3, 3, 3, 3)) * vVector + Cross(qVector, 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; From 0e14da8f0fb913369c6686177991cf390da3cfcd Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 11 Mar 2024 17:34:56 +0100 Subject: [PATCH 2/7] Fixed typo --- .../System.Private.CoreLib/src/System/Numerics/Vector3.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 1adabd7587240d..991280fe3425de 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs @@ -532,7 +532,7 @@ public static Vector3 Transform(Vector3 value, Quaternion rotation) var rVector = Unsafe.BitCast>(rotation); // v + 2 * (q x (q.W * v + q x v) - return (vVector + Vector128.Create(2f) * Cross(qVector, Vector128.Shuffle(qVector, Vector128.Create(3, 3, 3, 3)) * vVector + Cross(qVector, vVector))).AsVector3(); + 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) { From cfa24d9fba21fd7ec79e04556cf5f67b68eb0ac5 Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 11 Mar 2024 18:29:19 +0100 Subject: [PATCH 3/7] fixed missing conversion back to Vector3 added simd paths for quaternion multiplication, division and concatenate added simd path for matrix4x4 multiplication --- .../src/System/Numerics/Matrix4x4.Impl.cs | 123 +++++++++++++---- .../src/System/Numerics/Quaternion.cs | 124 +++++++++--------- .../src/System/Numerics/Vector3.cs | 22 ++-- 3 files changed, 175 insertions(+), 94 deletions(-) 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..35e57cba182b2e 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 @@ -165,33 +165,106 @@ public Vector3 Translation [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl operator *(in Impl left, in Impl right) { - Impl result; + if (Vector256.IsHardwareAccelerated) + { + var lhs = Unsafe.BitCast>(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, Impl>(result); + } + else if (Vector128.IsHardwareAccelerated) + { + 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; + 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; + 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; + + return result; + } } [MethodImpl(MethodImplOptions.AggressiveInlining)] 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..8fd6d4d8c55c24 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 Concatenate(Inverse(value2), value1); } /// Returns a value that indicates whether two quaternions are equal. @@ -193,10 +160,23 @@ 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); - result += (Vector128.Shuffle(right, Vector128.Create(1, 0, 3, 2)) * left.GetElementUnsafe(2)) * Vector128.Create(-1.0f, +1.0f, +1.0f, -1.0f); + 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)); + return Unsafe.BitCast, Quaternion>(result); } else @@ -291,33 +271,59 @@ public static Quaternion Add(Quaternion value1, Quaternion value2) /// A new quaternion representing the concatenation of the rotation followed by the rotation. public static Quaternion Concatenate(Quaternion value1, Quaternion value2) { - Quaternion ans; + if (Vector128.IsHardwareAccelerated) + { + var left = Unsafe.BitCast>(value1); + var right = Unsafe.BitCast>(value2); - // 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; + var c = Cross(right, left); + var dot = Vector3.Dot(right.AsVector3(), left.AsVector3()); - float q2x = value1.X; - float q2y = value1.Y; - float q2z = value1.Z; - float q2w = value1.W; + 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, Quaternion>(t2); + ans.W = t0.GetElement(3) - dot; + return ans; - // cross(av, bv) - float cx = q1y * q2z - q1z * q2y; - float cy = q1z * q2x - q1x * q2z; - float cz = q1x * q2y - q1y * q2x; + static Vector128 Cross(Vector128 l, Vector128 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; - float dot = q1x * q2x + q1y * q2y + q1z * q2z; + // 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 = q1x * q2w + q2x * q1w + cx; - ans.Y = q1y * q2w + q2y * q1w + cy; - ans.Z = q1z * q2w + q2z * q1w + cz; - ans.W = q1w * q2w - dot; + float q2x = value1.X; + float q2y = value1.Y; + float q2z = value1.Z; + float q2w = value1.W; - return ans; + // 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; + } } /// Returns the conjugate of a specified 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 991280fe3425de..af7f503960d995 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs @@ -306,17 +306,19 @@ public static Vector3 Cross(Vector3 vector1, Vector3 vector2) { 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))); + 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) + ); } - - 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. From 8d71e348c6ad28e1e047f34fefd55882d37cf50c Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 8 Apr 2024 20:48:35 +0200 Subject: [PATCH 4/7] Implemented feedback --- .../src/System/Numerics/Matrix4x4.Impl.cs | 114 +++--------------- .../src/System/Numerics/Quaternion.cs | 82 ++----------- 2 files changed, 22 insertions(+), 174 deletions(-) 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 35e57cba182b2e..21499fddb3b920 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 @@ -165,106 +165,22 @@ public Vector3 Translation [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Impl operator *(in Impl left, in Impl right) { - if (Vector256.IsHardwareAccelerated) - { - var lhs = Unsafe.BitCast>(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, 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)] 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 8fd6d4d8c55c24..5fd70b4a8b0ea0 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs @@ -118,7 +118,7 @@ public readonly bool IsIdentity /// The method defines the division operation for objects. public static Quaternion operator /(Quaternion value1, Quaternion value2) { - return Concatenate(Inverse(value2), value1); + return value1 * Inverse(value2); } /// Returns a value that indicates whether two quaternions are equal. @@ -159,24 +159,11 @@ public readonly bool IsIdentity { 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, Quaternion>(result); } else @@ -269,62 +256,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) - { - if (Vector128.IsHardwareAccelerated) - { - var left = Unsafe.BitCast>(value1); - var right = Unsafe.BitCast>(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, Quaternion>(t2); - ans.W = t0.GetElement(3) - dot; - return ans; - - static Vector128 Cross(Vector128 l, Vector128 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; /// Returns the conjugate of a specified quaternion. /// The quaternion. From 27bd42cf06b2616bed05896bb36a38dd20add15a Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 8 Apr 2024 21:03:16 +0200 Subject: [PATCH 5/7] fixed non static field --- .../src/System/Numerics/Matrix4x4.Impl.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 21499fddb3b920..bce88b81845faa 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 @@ -174,7 +174,7 @@ public Vector3 Translation } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private Vector4 Transform(Vector4 vector, in Impl matrix) + private static Vector4 Transform(Vector4 vector, in Impl matrix) { var result = matrix.X * vector.X; result += matrix.Y * vector.Y; From 1f35b22314ac518a4763f8bbccc3db20c32f4322 Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 8 Apr 2024 21:18:21 +0200 Subject: [PATCH 6/7] removed whitespace --- .../System.Private.CoreLib/src/System/Numerics/Quaternion.cs | 1 - 1 file changed, 1 deletion(-) 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 5fd70b4a8b0ea0..c77a258f78585e 100644 --- a/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Quaternion.cs @@ -159,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); From 00199d29c8ac2a05afe4594be161f65e0da6ec99 Mon Sep 17 00:00:00 2001 From: martenf Date: Mon, 8 Apr 2024 21:25:20 +0200 Subject: [PATCH 7/7] Use Vector4.Transform --- .../src/System/Numerics/Matrix4x4.Impl.cs | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) 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 bce88b81845faa..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,20 +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.Y = Transform(left.Y, in right); - result.Z = Transform(left.Z, in right); - result.W = Transform(left.W, in right); - return result; - } - - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static 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; + 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; }