From bb1b5d01816550b3831c26e467af8ad1dc9453a6 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 18:24:33 +0300 Subject: [PATCH 01/28] Optimize some Matrix4x4 operations with SSE --- .../src/System/Numerics/Matrix4x4.cs | 679 +++++++++++------- 1 file changed, 433 insertions(+), 246 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 16931d6d369e..1f28f8c56ad0 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -3,6 +3,7 @@ // See the LICENSE file in the project root for more information. using System.Globalization; +using System.Runtime.Intrinsics.X86; namespace System.Numerics { @@ -1786,35 +1787,60 @@ public static Matrix4x4 Transpose(Matrix4x4 matrix) /// The second source matrix. /// The relative weight of the second source matrix. /// The interpolated matrix. - public static Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) + public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) { - Matrix4x4 result; + if (Sse.IsSupported) + { + var amountVec = Sse.SetAllVector128(amount); - // First row - result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; - result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; - result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount; - result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount; + var m1Row = Sse.LoadVector128(&matrix1.M11); + var m2Row = Sse.LoadVector128(&matrix2.M11); + Sse.Store(&matrix1.M11, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - // Second row - result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; - result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; - result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount; - result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount; + m1Row = Sse.LoadVector128(&matrix1.M21); + m2Row = Sse.LoadVector128(&matrix2.M21); + Sse.Store(&matrix1.M21, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - // Third row - result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; - result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; - result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount; - result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount; - - // Fourth row - result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount; - result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount; - result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount; - result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount; + m1Row = Sse.LoadVector128(&matrix1.M31); + m2Row = Sse.LoadVector128(&matrix2.M31); + Sse.Store(&matrix1.M31, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - return result; + m1Row = Sse.LoadVector128(&matrix1.M41); + m2Row = Sse.LoadVector128(&matrix2.M41); + Sse.Store(&matrix1.M41, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); + + return matrix1; + } + else + { + Matrix4x4 result; + + // First row + result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; + result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; + result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount; + result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount; + + // Second row + result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; + result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; + result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount; + result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount; + + // Third row + result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; + result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; + result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount; + result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount; + + // Fourth row + result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount; + result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount; + result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount; + result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount; + + return result; + } } /// @@ -1822,28 +1848,41 @@ public static Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) /// /// The source matrix. /// The negated matrix. - public static Matrix4x4 Negate(Matrix4x4 value) + public static unsafe Matrix4x4 Negate(Matrix4x4 value) { - Matrix4x4 result; - - result.M11 = -value.M11; - result.M12 = -value.M12; - result.M13 = -value.M13; - result.M14 = -value.M14; - result.M21 = -value.M21; - result.M22 = -value.M22; - result.M23 = -value.M23; - result.M24 = -value.M24; - result.M31 = -value.M31; - result.M32 = -value.M32; - result.M33 = -value.M33; - result.M34 = -value.M34; - result.M41 = -value.M41; - result.M42 = -value.M42; - result.M43 = -value.M43; - result.M44 = -value.M44; + if (Sse.IsSupported) + { + var zero = Sse.SetAllVector128(0f); + Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); + Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); + Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); + Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41))); - return result; + return value; + } + else + { + Matrix4x4 result; + + result.M11 = -value.M11; + result.M12 = -value.M12; + result.M13 = -value.M13; + result.M14 = -value.M14; + result.M21 = -value.M21; + result.M22 = -value.M22; + result.M23 = -value.M23; + result.M24 = -value.M24; + result.M31 = -value.M31; + result.M32 = -value.M32; + result.M33 = -value.M33; + result.M34 = -value.M34; + result.M41 = -value.M41; + result.M42 = -value.M42; + result.M43 = -value.M43; + result.M44 = -value.M44; + + return result; + } } /// @@ -1852,28 +1891,39 @@ public static Matrix4x4 Negate(Matrix4x4 value) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 result; - - result.M11 = value1.M11 + value2.M11; - result.M12 = value1.M12 + value2.M12; - result.M13 = value1.M13 + value2.M13; - result.M14 = value1.M14 + value2.M14; - result.M21 = value1.M21 + value2.M21; - result.M22 = value1.M22 + value2.M22; - result.M23 = value1.M23 + value2.M23; - result.M24 = value1.M24 + value2.M24; - result.M31 = value1.M31 + value2.M31; - result.M32 = value1.M32 + value2.M32; - result.M33 = value1.M33 + value2.M33; - result.M34 = value1.M34 + value2.M34; - result.M41 = value1.M41 + value2.M41; - result.M42 = value1.M42 + value2.M42; - result.M43 = value1.M43 + value2.M43; - result.M44 = value1.M44 + value2.M44; - - return result; + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 result; + + result.M11 = value1.M11 + value2.M11; + result.M12 = value1.M12 + value2.M12; + result.M13 = value1.M13 + value2.M13; + result.M14 = value1.M14 + value2.M14; + result.M21 = value1.M21 + value2.M21; + result.M22 = value1.M22 + value2.M22; + result.M23 = value1.M23 + value2.M23; + result.M24 = value1.M24 + value2.M24; + result.M31 = value1.M31 + value2.M31; + result.M32 = value1.M32 + value2.M32; + result.M33 = value1.M33 + value2.M33; + result.M34 = value1.M34 + value2.M34; + result.M41 = value1.M41 + value2.M41; + result.M42 = value1.M42 + value2.M42; + result.M43 = value1.M43 + value2.M43; + result.M44 = value1.M44 + value2.M44; + + return result; + } } /// @@ -1882,28 +1932,39 @@ public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 result; - - result.M11 = value1.M11 - value2.M11; - result.M12 = value1.M12 - value2.M12; - result.M13 = value1.M13 - value2.M13; - result.M14 = value1.M14 - value2.M14; - result.M21 = value1.M21 - value2.M21; - result.M22 = value1.M22 - value2.M22; - result.M23 = value1.M23 - value2.M23; - result.M24 = value1.M24 - value2.M24; - result.M31 = value1.M31 - value2.M31; - result.M32 = value1.M32 - value2.M32; - result.M33 = value1.M33 - value2.M33; - result.M34 = value1.M34 - value2.M34; - result.M41 = value1.M41 - value2.M41; - result.M42 = value1.M42 - value2.M42; - result.M43 = value1.M43 - value2.M43; - result.M44 = value1.M44 - value2.M44; - - return result; + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 result; + + result.M11 = value1.M11 - value2.M11; + result.M12 = value1.M12 - value2.M12; + result.M13 = value1.M13 - value2.M13; + result.M14 = value1.M14 - value2.M14; + result.M21 = value1.M21 - value2.M21; + result.M22 = value1.M22 - value2.M22; + result.M23 = value1.M23 - value2.M23; + result.M24 = value1.M24 - value2.M24; + result.M31 = value1.M31 - value2.M31; + result.M32 = value1.M32 - value2.M32; + result.M33 = value1.M33 - value2.M33; + result.M34 = value1.M34 - value2.M34; + result.M41 = value1.M41 - value2.M41; + result.M42 = value1.M42 - value2.M42; + result.M43 = value1.M43 - value2.M43; + result.M44 = value1.M44 - value2.M44; + + return result; + } } /// @@ -1912,35 +1973,68 @@ public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 result; - - // First row - result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; - result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; - result.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; - result.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; - - // Second row - result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; - result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; - result.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; - result.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; - - // Third row - result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; - result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; - result.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; - result.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; - - // Fourth row - result.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; - result.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; - result.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; - result.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; - - return result; + if (Sse.IsSupported) + { + Matrix4x4 m = default; + + Sse.Store(&m.M11, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M21, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M31, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M41, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); + + return m; + } + else + { + Matrix4x4 result; + + // First row + result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; + result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; + result.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; + result.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; + + // Second row + result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; + result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; + result.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; + result.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; + + // Third row + result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; + result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; + result.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; + result.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; + + // Fourth row + result.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; + result.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; + result.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; + result.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; + + return result; + } } /// @@ -1949,28 +2043,40 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) + public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) { - Matrix4x4 result; - - result.M11 = value1.M11 * value2; - result.M12 = value1.M12 * value2; - result.M13 = value1.M13 * value2; - result.M14 = value1.M14 * value2; - result.M21 = value1.M21 * value2; - result.M22 = value1.M22 * value2; - result.M23 = value1.M23 * value2; - result.M24 = value1.M24 * value2; - result.M31 = value1.M31 * value2; - result.M32 = value1.M32 * value2; - result.M33 = value1.M33 * value2; - result.M34 = value1.M34 * value2; - result.M41 = value1.M41 * value2; - result.M42 = value1.M42 * value2; - result.M43 = value1.M43 * value2; - result.M44 = value1.M44 * value2; - - return result; + if (Sse.IsSupported) + { + var all = Sse.SetAllVector128(value2); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); + Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); + Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); + return value1; + } + else + { + Matrix4x4 result; + + result.M11 = value1.M11 * value2; + result.M12 = value1.M12 * value2; + result.M13 = value1.M13 * value2; + result.M14 = value1.M14 * value2; + result.M21 = value1.M21 * value2; + result.M22 = value1.M22 * value2; + result.M23 = value1.M23 * value2; + result.M24 = value1.M24 * value2; + result.M31 = value1.M31 * value2; + result.M32 = value1.M32 * value2; + result.M33 = value1.M33 * value2; + result.M34 = value1.M34 * value2; + result.M41 = value1.M41 * value2; + result.M42 = value1.M42 * value2; + result.M43 = value1.M43 * value2; + result.M44 = value1.M44 * value2; + + return result; + } } /// @@ -1978,28 +2084,41 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// /// The source matrix. /// The negated matrix. - public static Matrix4x4 operator -(Matrix4x4 value) + public static unsafe Matrix4x4 operator -(Matrix4x4 value) { - Matrix4x4 m; - - m.M11 = -value.M11; - m.M12 = -value.M12; - m.M13 = -value.M13; - m.M14 = -value.M14; - m.M21 = -value.M21; - m.M22 = -value.M22; - m.M23 = -value.M23; - m.M24 = -value.M24; - m.M31 = -value.M31; - m.M32 = -value.M32; - m.M33 = -value.M33; - m.M34 = -value.M34; - m.M41 = -value.M41; - m.M42 = -value.M42; - m.M43 = -value.M43; - m.M44 = -value.M44; - - return m; + if (Sse.IsSupported) + { + var zero = Sse.SetAllVector128(0f); + Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); + Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); + Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); + Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41))); + + return value; + } + else + { + Matrix4x4 m; + + m.M11 = -value.M11; + m.M12 = -value.M12; + m.M13 = -value.M13; + m.M14 = -value.M14; + m.M21 = -value.M21; + m.M22 = -value.M22; + m.M23 = -value.M23; + m.M24 = -value.M24; + m.M31 = -value.M31; + m.M32 = -value.M32; + m.M33 = -value.M33; + m.M34 = -value.M34; + m.M41 = -value.M41; + m.M42 = -value.M42; + m.M43 = -value.M43; + m.M44 = -value.M44; + + return m; + } } /// @@ -2008,28 +2127,39 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 m; - - m.M11 = value1.M11 + value2.M11; - m.M12 = value1.M12 + value2.M12; - m.M13 = value1.M13 + value2.M13; - m.M14 = value1.M14 + value2.M14; - m.M21 = value1.M21 + value2.M21; - m.M22 = value1.M22 + value2.M22; - m.M23 = value1.M23 + value2.M23; - m.M24 = value1.M24 + value2.M24; - m.M31 = value1.M31 + value2.M31; - m.M32 = value1.M32 + value2.M32; - m.M33 = value1.M33 + value2.M33; - m.M34 = value1.M34 + value2.M34; - m.M41 = value1.M41 + value2.M41; - m.M42 = value1.M42 + value2.M42; - m.M43 = value1.M43 + value2.M43; - m.M44 = value1.M44 + value2.M44; - - return m; + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 + value2.M11; + m.M12 = value1.M12 + value2.M12; + m.M13 = value1.M13 + value2.M13; + m.M14 = value1.M14 + value2.M14; + m.M21 = value1.M21 + value2.M21; + m.M22 = value1.M22 + value2.M22; + m.M23 = value1.M23 + value2.M23; + m.M24 = value1.M24 + value2.M24; + m.M31 = value1.M31 + value2.M31; + m.M32 = value1.M32 + value2.M32; + m.M33 = value1.M33 + value2.M33; + m.M34 = value1.M34 + value2.M34; + m.M41 = value1.M41 + value2.M41; + m.M42 = value1.M42 + value2.M42; + m.M43 = value1.M43 + value2.M43; + m.M44 = value1.M44 + value2.M44; + + return m; + } } /// @@ -2038,28 +2168,39 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 m; - - m.M11 = value1.M11 - value2.M11; - m.M12 = value1.M12 - value2.M12; - m.M13 = value1.M13 - value2.M13; - m.M14 = value1.M14 - value2.M14; - m.M21 = value1.M21 - value2.M21; - m.M22 = value1.M22 - value2.M22; - m.M23 = value1.M23 - value2.M23; - m.M24 = value1.M24 - value2.M24; - m.M31 = value1.M31 - value2.M31; - m.M32 = value1.M32 - value2.M32; - m.M33 = value1.M33 - value2.M33; - m.M34 = value1.M34 - value2.M34; - m.M41 = value1.M41 - value2.M41; - m.M42 = value1.M42 - value2.M42; - m.M43 = value1.M43 - value2.M43; - m.M44 = value1.M44 - value2.M44; - - return m; + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 - value2.M11; + m.M12 = value1.M12 - value2.M12; + m.M13 = value1.M13 - value2.M13; + m.M14 = value1.M14 - value2.M14; + m.M21 = value1.M21 - value2.M21; + m.M22 = value1.M22 - value2.M22; + m.M23 = value1.M23 - value2.M23; + m.M24 = value1.M24 - value2.M24; + m.M31 = value1.M31 - value2.M31; + m.M32 = value1.M32 - value2.M32; + m.M33 = value1.M33 - value2.M33; + m.M34 = value1.M34 - value2.M34; + m.M41 = value1.M41 - value2.M41; + m.M42 = value1.M42 - value2.M42; + m.M43 = value1.M43 - value2.M43; + m.M44 = value1.M44 - value2.M44; + + return m; + } } /// @@ -2068,35 +2209,68 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - public static Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) { - Matrix4x4 m; - - // First row - m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; - m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; - m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; - m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; - - // Second row - m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; - m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; - m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; - m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; - - // Third row - m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; - m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; - m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; - m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; - - // Fourth row - m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; - m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; - m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; - m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; - - return m; + if (Sse.IsSupported) + { + Matrix4x4 m = default; + + Sse.Store(&m.M11, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M21, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M31, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M41, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); + + return m; + } + else + { + Matrix4x4 m; + + // First row + m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; + m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; + m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; + m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; + + // Second row + m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; + m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; + m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; + m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; + + // Third row + m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; + m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; + m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; + m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; + + // Fourth row + m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; + m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; + m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; + m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; + + return m; + } } /// @@ -2105,27 +2279,40 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static Matrix4x4 operator *(Matrix4x4 value1, float value2) + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) { - Matrix4x4 m; - - m.M11 = value1.M11 * value2; - m.M12 = value1.M12 * value2; - m.M13 = value1.M13 * value2; - m.M14 = value1.M14 * value2; - m.M21 = value1.M21 * value2; - m.M22 = value1.M22 * value2; - m.M23 = value1.M23 * value2; - m.M24 = value1.M24 * value2; - m.M31 = value1.M31 * value2; - m.M32 = value1.M32 * value2; - m.M33 = value1.M33 * value2; - m.M34 = value1.M34 * value2; - m.M41 = value1.M41 * value2; - m.M42 = value1.M42 * value2; - m.M43 = value1.M43 * value2; - m.M44 = value1.M44 * value2; - return m; + + if (Sse.IsSupported) + { + var all = Sse.SetAllVector128(value2); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); + Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); + Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 * value2; + m.M12 = value1.M12 * value2; + m.M13 = value1.M13 * value2; + m.M14 = value1.M14 * value2; + m.M21 = value1.M21 * value2; + m.M22 = value1.M22 * value2; + m.M23 = value1.M23 * value2; + m.M24 = value1.M24 * value2; + m.M31 = value1.M31 * value2; + m.M32 = value1.M32 * value2; + m.M33 = value1.M33 * value2; + m.M34 = value1.M34 * value2; + m.M41 = value1.M41 * value2; + m.M42 = value1.M42 * value2; + m.M43 = value1.M43 * value2; + m.M44 = value1.M44 * value2; + return m; + } } /// From f84f81eefe7da35b8976958e8c97fdf65dc41ee0 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 18:52:33 +0300 Subject: [PATCH 02/28] fix typo --- src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 1f28f8c56ad0..3573f55c647b 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2050,8 +2050,8 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) var all = Sse.SetAllVector128(value2); Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); + Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); - Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); return value1; } else @@ -2287,8 +2287,8 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) var all = Sse.SetAllVector128(value2); Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); + Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); - Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); return value1; } else From c07c315ecdc46e25911eca0774a65646ac5e39dd Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 19:38:58 +0300 Subject: [PATCH 03/28] use SetZeroVector128 instead of SetAllVector128(0.0f) --- src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 3573f55c647b..c14731762dbb 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1852,7 +1852,7 @@ public static unsafe Matrix4x4 Negate(Matrix4x4 value) { if (Sse.IsSupported) { - var zero = Sse.SetAllVector128(0f); + var zero = Sse.SetZeroVector128(); Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); @@ -2088,7 +2088,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) { if (Sse.IsSupported) { - var zero = Sse.SetAllVector128(0f); + var zero = Sse.SetZeroVector128(); Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); From 39a145519b7ba8d17fd8f5f36fa39e68931c3cbb Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 19:51:20 +0300 Subject: [PATCH 04/28] [StructLayout(LayoutKind.Sequential)] on top of Matrix4x4 --- src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index c14731762dbb..d1bc700eed31 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -3,6 +3,7 @@ // See the LICENSE file in the project root for more information. using System.Globalization; +using System.Runtime.InteropServices; using System.Runtime.Intrinsics.X86; namespace System.Numerics @@ -10,6 +11,7 @@ namespace System.Numerics /// /// A structure encapsulating a 4x4 matrix. /// + [StructLayout(LayoutKind.Sequential)] public struct Matrix4x4 : IEquatable { #region Public Fields From a7387606ea3c89660d9573a41cd0e2fe58e609a5 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 19:58:30 +0300 Subject: [PATCH 05/28] collapse operators (call corresponding methods) --- .../src/System/Numerics/Matrix4x4.cs | 225 +----------------- 1 file changed, 8 insertions(+), 217 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index d1bc700eed31..fbebb15c66bd 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -3,6 +3,7 @@ // See the LICENSE file in the project root for more information. using System.Globalization; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Runtime.Intrinsics.X86; @@ -2086,42 +2087,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// /// The source matrix. /// The negated matrix. - public static unsafe Matrix4x4 operator -(Matrix4x4 value) - { - if (Sse.IsSupported) - { - var zero = Sse.SetZeroVector128(); - Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); - Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); - Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); - Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41))); - - return value; - } - else - { - Matrix4x4 m; - - m.M11 = -value.M11; - m.M12 = -value.M12; - m.M13 = -value.M13; - m.M14 = -value.M14; - m.M21 = -value.M21; - m.M22 = -value.M22; - m.M23 = -value.M23; - m.M24 = -value.M24; - m.M31 = -value.M31; - m.M32 = -value.M32; - m.M33 = -value.M33; - m.M34 = -value.M34; - m.M41 = -value.M41; - m.M42 = -value.M42; - m.M43 = -value.M43; - m.M44 = -value.M44; - - return m; - } - } + public static Matrix4x4 operator -(Matrix4x4 value) => Negate(value); /// /// Adds two matrices together. @@ -2129,40 +2095,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); - Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); - Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); - Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); - return value1; - } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 + value2.M11; - m.M12 = value1.M12 + value2.M12; - m.M13 = value1.M13 + value2.M13; - m.M14 = value1.M14 + value2.M14; - m.M21 = value1.M21 + value2.M21; - m.M22 = value1.M22 + value2.M22; - m.M23 = value1.M23 + value2.M23; - m.M24 = value1.M24 + value2.M24; - m.M31 = value1.M31 + value2.M31; - m.M32 = value1.M32 + value2.M32; - m.M33 = value1.M33 + value2.M33; - m.M34 = value1.M34 + value2.M34; - m.M41 = value1.M41 + value2.M41; - m.M42 = value1.M42 + value2.M42; - m.M43 = value1.M43 + value2.M43; - m.M44 = value1.M44 + value2.M44; - - return m; - } - } + public static Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) => Add(value1, value2); /// /// Subtracts the second matrix from the first. @@ -2170,40 +2103,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); - Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); - Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); - Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); - return value1; - } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 - value2.M11; - m.M12 = value1.M12 - value2.M12; - m.M13 = value1.M13 - value2.M13; - m.M14 = value1.M14 - value2.M14; - m.M21 = value1.M21 - value2.M21; - m.M22 = value1.M22 - value2.M22; - m.M23 = value1.M23 - value2.M23; - m.M24 = value1.M24 - value2.M24; - m.M31 = value1.M31 - value2.M31; - m.M32 = value1.M32 - value2.M32; - m.M33 = value1.M33 - value2.M33; - m.M34 = value1.M34 - value2.M34; - m.M41 = value1.M41 - value2.M41; - m.M42 = value1.M42 - value2.M42; - m.M43 = value1.M43 - value2.M43; - m.M44 = value1.M44 - value2.M44; - - return m; - } - } + public static Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) => Subtract(value1, value2); /// /// Multiplies a matrix by another matrix. @@ -2211,69 +2111,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - Matrix4x4 m = default; - - Sse.Store(&m.M11, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M21, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M31, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M41, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); - - return m; - } - else - { - Matrix4x4 m; - - // First row - m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; - m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; - m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; - m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; - - // Second row - m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; - m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; - m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; - m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; - - // Third row - m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; - m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; - m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; - m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; - - // Fourth row - m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; - m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; - m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; - m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; - - return m; - } - } + public static Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) => Multiply(value1, value2); /// /// Multiplies a matrix by a scalar value. @@ -2281,41 +2119,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) - { - - if (Sse.IsSupported) - { - var all = Sse.SetAllVector128(value2); - Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); - Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); - Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); - Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); - return value1; - } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 * value2; - m.M12 = value1.M12 * value2; - m.M13 = value1.M13 * value2; - m.M14 = value1.M14 * value2; - m.M21 = value1.M21 * value2; - m.M22 = value1.M22 * value2; - m.M23 = value1.M23 * value2; - m.M24 = value1.M24 * value2; - m.M31 = value1.M31 * value2; - m.M32 = value1.M32 * value2; - m.M33 = value1.M33 * value2; - m.M34 = value1.M34 * value2; - m.M41 = value1.M41 * value2; - m.M42 = value1.M42 * value2; - m.M43 = value1.M43 * value2; - m.M44 = value1.M44 * value2; - return m; - } - } + public static Matrix4x4 operator *(Matrix4x4 value1, float value2) => Multiply(value1, value2); /// /// Returns a boolean indicating whether the given two matrices are equal. @@ -2323,14 +2127,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are equal; False otherwise. - public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2) - { - return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. - value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && - value1.M21 == value2.M21 && value1.M23 == value2.M23 && value1.M24 == value2.M24 && - value1.M31 == value2.M31 && value1.M32 == value2.M32 && value1.M34 == value2.M34 && - value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); - } + public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2) => value1.Equals(value2); /// /// Returns a boolean indicating whether the given two matrices are not equal. @@ -2338,13 +2135,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are not equal; False if they are equal. - public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) - { - return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || - value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || - value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || - value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); - } + public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) => !value1.Equals(value2); /// /// Returns a boolean indicating whether this matrix instance is equal to the other given matrix. From a6aa63cf19867b44cb473badce4900fc40939581 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 20:02:00 +0300 Subject: [PATCH 06/28] mark operators with [MethodImpl(MethodImplOptions.AggressiveInlining)] --- .../src/System/Numerics/Matrix4x4.cs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index fbebb15c66bd..9c494401f973 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1851,6 +1851,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// /// The source matrix. /// The negated matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Negate(Matrix4x4 value) { if (Sse.IsSupported) @@ -1894,6 +1895,7 @@ public static unsafe Matrix4x4 Negate(Matrix4x4 value) /// The first source matrix. /// The second source matrix. /// The resulting matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) @@ -1935,6 +1937,7 @@ public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) @@ -1976,6 +1979,7 @@ public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) @@ -2046,6 +2050,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) { if (Sse.IsSupported) @@ -2142,6 +2147,7 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// /// The matrix to compare this instance to. /// True if the matrices are equal; False otherwise. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Equals(Matrix4x4 other) { return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. From 6070dbd9d97441a1143d6b8cbd47978ca1f7a851 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 20:26:57 +0300 Subject: [PATCH 07/28] remove [MethodImpl(MethodImplOptions.AggressiveInlining)], call operators inside static methods if SSE is enabled (operators should be small enough to be inlined) --- .../src/System/Numerics/Matrix4x4.cs | 288 ++++++++++++++---- 1 file changed, 223 insertions(+), 65 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 9c494401f973..c6115cc6c0fc 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -3,7 +3,6 @@ // See the LICENSE file in the project root for more information. using System.Globalization; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Runtime.Intrinsics.X86; @@ -1851,18 +1850,11 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// /// The source matrix. /// The negated matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Negate(Matrix4x4 value) { if (Sse.IsSupported) { - var zero = Sse.SetZeroVector128(); - Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); - Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); - Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); - Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41))); - - return value; + return -value; } else { @@ -1895,16 +1887,11 @@ public static unsafe Matrix4x4 Negate(Matrix4x4 value) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) { - Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); - Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); - Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); - Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); - return value1; + return value1 + value2; } else { @@ -1937,16 +1924,11 @@ public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) { - Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); - Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); - Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); - Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); - return value1; + return value1 - value2; } else { @@ -1979,38 +1961,11 @@ public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) + public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) { - Matrix4x4 m = default; - - Sse.Store(&m.M11, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M21, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M31, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); - - Sse.Store(&m.M41, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); - - return m; + return value1 * value2; } else { @@ -2050,17 +2005,11 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) { if (Sse.IsSupported) { - var all = Sse.SetAllVector128(value2); - Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); - Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); - Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); - Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); - return value1; + return value1 * value2; } else { @@ -2092,7 +2041,42 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// /// The source matrix. /// The negated matrix. - public static Matrix4x4 operator -(Matrix4x4 value) => Negate(value); + public static unsafe Matrix4x4 operator -(Matrix4x4 value) + { + if (Sse.IsSupported) + { + var zero = Sse.SetZeroVector128(); + Sse.Store(&value.M11, Sse.Subtract(zero, Sse.LoadVector128(&value.M11))); + Sse.Store(&value.M21, Sse.Subtract(zero, Sse.LoadVector128(&value.M21))); + Sse.Store(&value.M31, Sse.Subtract(zero, Sse.LoadVector128(&value.M31))); + Sse.Store(&value.M41, Sse.Subtract(zero, Sse.LoadVector128(&value.M41))); + + return value; + } + else + { + Matrix4x4 m; + + m.M11 = -value.M11; + m.M12 = -value.M12; + m.M13 = -value.M13; + m.M14 = -value.M14; + m.M21 = -value.M21; + m.M22 = -value.M22; + m.M23 = -value.M23; + m.M24 = -value.M24; + m.M31 = -value.M31; + m.M32 = -value.M32; + m.M33 = -value.M33; + m.M34 = -value.M34; + m.M41 = -value.M41; + m.M42 = -value.M42; + m.M43 = -value.M43; + m.M44 = -value.M44; + + return m; + } + } /// /// Adds two matrices together. @@ -2100,7 +2084,40 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) => Add(value1, value2); + public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) + { + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Add(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Add(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 + value2.M11; + m.M12 = value1.M12 + value2.M12; + m.M13 = value1.M13 + value2.M13; + m.M14 = value1.M14 + value2.M14; + m.M21 = value1.M21 + value2.M21; + m.M22 = value1.M22 + value2.M22; + m.M23 = value1.M23 + value2.M23; + m.M24 = value1.M24 + value2.M24; + m.M31 = value1.M31 + value2.M31; + m.M32 = value1.M32 + value2.M32; + m.M33 = value1.M33 + value2.M33; + m.M34 = value1.M34 + value2.M34; + m.M41 = value1.M41 + value2.M41; + m.M42 = value1.M42 + value2.M42; + m.M43 = value1.M43 + value2.M43; + m.M44 = value1.M44 + value2.M44; + + return m; + } + } /// /// Subtracts the second matrix from the first. @@ -2108,7 +2125,40 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) => Subtract(value1, value2); + public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) + { + if (Sse.IsSupported) + { + Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); + Sse.Store(&value1.M21, Sse.Subtract(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value1.M21))); + Sse.Store(&value1.M31, Sse.Subtract(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value1.M31))); + Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 - value2.M11; + m.M12 = value1.M12 - value2.M12; + m.M13 = value1.M13 - value2.M13; + m.M14 = value1.M14 - value2.M14; + m.M21 = value1.M21 - value2.M21; + m.M22 = value1.M22 - value2.M22; + m.M23 = value1.M23 - value2.M23; + m.M24 = value1.M24 - value2.M24; + m.M31 = value1.M31 - value2.M31; + m.M32 = value1.M32 - value2.M32; + m.M33 = value1.M33 - value2.M33; + m.M34 = value1.M34 - value2.M34; + m.M41 = value1.M41 - value2.M41; + m.M42 = value1.M42 - value2.M42; + m.M43 = value1.M43 - value2.M43; + m.M44 = value1.M44 - value2.M44; + + return m; + } + } /// /// Multiplies a matrix by another matrix. @@ -2116,7 +2166,69 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - public static Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) => Multiply(value1, value2); + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) + { + if (Sse.IsSupported) + { + Matrix4x4 m = default; + + Sse.Store(&m.M11, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M21, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M31, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); + + Sse.Store(&m.M41, + Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); + + return m; + } + else + { + Matrix4x4 m; + + // First row + m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; + m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; + m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; + m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; + + // Second row + m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; + m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; + m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; + m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; + + // Third row + m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; + m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; + m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; + m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; + + // Fourth row + m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; + m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; + m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; + m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; + + return m; + } + } /// /// Multiplies a matrix by a scalar value. @@ -2124,7 +2236,41 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static Matrix4x4 operator *(Matrix4x4 value1, float value2) => Multiply(value1, value2); + public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) + { + + if (Sse.IsSupported) + { + var all = Sse.SetAllVector128(value2); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); + Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); + Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); + Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); + return value1; + } + else + { + Matrix4x4 m; + + m.M11 = value1.M11 * value2; + m.M12 = value1.M12 * value2; + m.M13 = value1.M13 * value2; + m.M14 = value1.M14 * value2; + m.M21 = value1.M21 * value2; + m.M22 = value1.M22 * value2; + m.M23 = value1.M23 * value2; + m.M24 = value1.M24 * value2; + m.M31 = value1.M31 * value2; + m.M32 = value1.M32 * value2; + m.M33 = value1.M33 * value2; + m.M34 = value1.M34 * value2; + m.M41 = value1.M41 * value2; + m.M42 = value1.M42 * value2; + m.M43 = value1.M43 * value2; + m.M44 = value1.M44 * value2; + return m; + } + } /// /// Returns a boolean indicating whether the given two matrices are equal. @@ -2132,7 +2278,14 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are equal; False otherwise. - public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2) => value1.Equals(value2); + public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2) + { + return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. + value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && + value1.M21 == value2.M21 && value1.M23 == value2.M23 && value1.M24 == value2.M24 && + value1.M31 == value2.M31 && value1.M32 == value2.M32 && value1.M34 == value2.M34 && + value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); + } /// /// Returns a boolean indicating whether the given two matrices are not equal. @@ -2140,14 +2293,19 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are not equal; False if they are equal. - public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) => !value1.Equals(value2); + public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + { + return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || + value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || + value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || + value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); + } /// /// Returns a boolean indicating whether this matrix instance is equal to the other given matrix. /// /// The matrix to compare this instance to. /// True if the matrices are equal; False otherwise. - [MethodImpl(MethodImplOptions.AggressiveInlining)] public bool Equals(Matrix4x4 other) { return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. From 87318886676aa00bba4614ac57d34aebe5e1b289 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 20:33:19 +0300 Subject: [PATCH 08/28] overwrite value1 in Multiply instead of new instance --- .../src/System/Numerics/Matrix4x4.cs | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index c6115cc6c0fc..7f031ccd5004 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2170,33 +2170,30 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) { if (Sse.IsSupported) { - Matrix4x4 m = default; - - Sse.Store(&m.M11, + Sse.Store(&value1.M11, Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); - Sse.Store(&m.M21, + Sse.Store(&value1.M21, Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); - Sse.Store(&m.M31, + Sse.Store(&value1.M31, Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); - Sse.Store(&m.M41, + Sse.Store(&value1.M41, Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); - - return m; + return value1; } else { @@ -2241,11 +2238,11 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) if (Sse.IsSupported) { - var all = Sse.SetAllVector128(value2); - Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), all)); - Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), all)); - Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), all)); - Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), all)); + var value2Vec = Sse.SetAllVector128(value2); + Sse.Store(&value1.M11, Sse.Multiply(Sse.LoadVector128(&value1.M11), value2Vec)); + Sse.Store(&value1.M21, Sse.Multiply(Sse.LoadVector128(&value1.M21), value2Vec)); + Sse.Store(&value1.M31, Sse.Multiply(Sse.LoadVector128(&value1.M31), value2Vec)); + Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), value2Vec)); return value1; } else From f59cf11db83807156f8895d1e47980bf6aab5ff0 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Wed, 15 Aug 2018 21:59:23 +0300 Subject: [PATCH 09/28] Optimize == and != --- .../src/System/Numerics/Matrix4x4.cs | 60 ++++++++++++++----- 1 file changed, 44 insertions(+), 16 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 7f031ccd5004..565a64f1e0d5 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2275,13 +2275,23 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are equal; False otherwise. - public static bool operator ==(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2) { - return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. - value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && - value1.M21 == value2.M21 && value1.M23 == value2.M23 && value1.M24 == value2.M24 && - value1.M31 == value2.M31 && value1.M32 == value2.M32 && value1.M34 == value2.M34 && - value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); + if (Sse.IsSupported) + { + return + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0xF && + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0xF && + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0xF && + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0xF; + } + else + { + return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. + value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && value1.M21 == value2.M21 && + value1.M23 == value2.M23 && value1.M24 == value2.M24 && value1.M31 == value2.M31 && value1.M32 == value2.M32 && + value1.M34 == value2.M34 && value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); + } } /// @@ -2290,12 +2300,23 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are not equal; False if they are equal. - public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) { - return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || - value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || - value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || - value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); + if (Sse.IsSupported) + { + return + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) != 0xF || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) != 0xF || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) != 0xF || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) != 0xF; + } + else + { + return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || + value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || + value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || + value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); + } } /// @@ -2305,11 +2326,18 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// True if the matrices are equal; False otherwise. public bool Equals(Matrix4x4 other) { - return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. - M12 == other.M12 && M13 == other.M13 && M14 == other.M14 && - M21 == other.M21 && M23 == other.M23 && M24 == other.M24 && - M31 == other.M31 && M32 == other.M32 && M34 == other.M34 && - M41 == other.M41 && M42 == other.M42 && M43 == other.M43); + if (Sse.IsSupported) + { + return this == other; + } + else + { + return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. + M12 == other.M12 && M13 == other.M13 && M14 == other.M14 && + M21 == other.M21 && M23 == other.M23 && M24 == other.M24 && + M31 == other.M31 && M32 == other.M32 && M34 == other.M34 && + M41 == other.M41 && M42 == other.M42 && M43 == other.M43); + } } /// From 21ca6ac66e79ee99b3933660fe83ad1f2e07e545 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 00:08:20 +0300 Subject: [PATCH 10/28] prefer CompareNotEqual than CompareEqual --- .../src/System/Numerics/Matrix4x4.cs | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 565a64f1e0d5..333a35708350 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1850,7 +1850,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// /// The source matrix. /// The negated matrix. - public static unsafe Matrix4x4 Negate(Matrix4x4 value) + public static Matrix4x4 Negate(Matrix4x4 value) { if (Sse.IsSupported) { @@ -1887,7 +1887,7 @@ public static unsafe Matrix4x4 Negate(Matrix4x4 value) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) + public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) { @@ -1924,7 +1924,7 @@ public static unsafe Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static unsafe Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) + public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) { if (Sse.IsSupported) { @@ -2005,7 +2005,7 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) + public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) { if (Sse.IsSupported) { @@ -2280,10 +2280,10 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) if (Sse.IsSupported) { return - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0xF && - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0xF && - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0xF && - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0xF; + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) != 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) != 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) != 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) != 0; } else { @@ -2305,10 +2305,10 @@ public static unsafe Matrix4x4 Multiply(Matrix4x4 value1, float value2) if (Sse.IsSupported) { return - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) != 0xF || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) != 0xF || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) != 0xF || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) != 0xF; + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 || + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 || + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 || + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; } else { @@ -2333,10 +2333,9 @@ public bool Equals(Matrix4x4 other) else { return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. - M12 == other.M12 && M13 == other.M13 && M14 == other.M14 && - M21 == other.M21 && M23 == other.M23 && M24 == other.M24 && - M31 == other.M31 && M32 == other.M32 && M34 == other.M34 && - M41 == other.M41 && M42 == other.M42 && M43 == other.M43); + M12 == other.M12 && M13 == other.M13 && M14 == other.M14 && M21 == other.M21 && + M23 == other.M23 && M24 == other.M24 && M31 == other.M31 && M32 == other.M32 && + M34 == other.M34 && M41 == other.M41 && M42 == other.M42 && M43 == other.M43); } } From bac511338aa56505f30dd6ca0c5bf8c3398a0806 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 00:59:50 +0300 Subject: [PATCH 11/28] fix typo in == and != --- .../src/System/Numerics/Matrix4x4.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 333a35708350..189ffedfd1db 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2280,10 +2280,10 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) if (Sse.IsSupported) { return - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) != 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) != 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) != 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) != 0; + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 && + Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; } else { @@ -2305,10 +2305,10 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) if (Sse.IsSupported) { return - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 || - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 || - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 || - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 || + Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; } else { From 35f0d7fd7488ca96f7eaaaec90b1fb6f06f05eef Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 01:04:30 +0300 Subject: [PATCH 12/28] clean up methods-operators --- .../src/System/Numerics/Matrix4x4.cs | 181 +----------------- 1 file changed, 8 insertions(+), 173 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 189ffedfd1db..7d719d52c7c4 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1850,36 +1850,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// /// The source matrix. /// The negated matrix. - public static Matrix4x4 Negate(Matrix4x4 value) - { - if (Sse.IsSupported) - { - return -value; - } - else - { - Matrix4x4 result; - - result.M11 = -value.M11; - result.M12 = -value.M12; - result.M13 = -value.M13; - result.M14 = -value.M14; - result.M21 = -value.M21; - result.M22 = -value.M22; - result.M23 = -value.M23; - result.M24 = -value.M24; - result.M31 = -value.M31; - result.M32 = -value.M32; - result.M33 = -value.M33; - result.M34 = -value.M34; - result.M41 = -value.M41; - result.M42 = -value.M42; - result.M43 = -value.M43; - result.M44 = -value.M44; - - return result; - } - } + public static Matrix4x4 Negate(Matrix4x4 value) => -value; /// /// Adds two matrices together. @@ -1887,36 +1858,7 @@ public static Matrix4x4 Negate(Matrix4x4 value) /// The first source matrix. /// The second source matrix. /// The resulting matrix. - public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - return value1 + value2; - } - else - { - Matrix4x4 result; - - result.M11 = value1.M11 + value2.M11; - result.M12 = value1.M12 + value2.M12; - result.M13 = value1.M13 + value2.M13; - result.M14 = value1.M14 + value2.M14; - result.M21 = value1.M21 + value2.M21; - result.M22 = value1.M22 + value2.M22; - result.M23 = value1.M23 + value2.M23; - result.M24 = value1.M24 + value2.M24; - result.M31 = value1.M31 + value2.M31; - result.M32 = value1.M32 + value2.M32; - result.M33 = value1.M33 + value2.M33; - result.M34 = value1.M34 + value2.M34; - result.M41 = value1.M41 + value2.M41; - result.M42 = value1.M42 + value2.M42; - result.M43 = value1.M43 + value2.M43; - result.M44 = value1.M44 + value2.M44; - - return result; - } - } + public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) => value1 + value2; /// /// Subtracts the second matrix from the first. @@ -1924,36 +1866,7 @@ public static Matrix4x4 Add(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the subtraction. - public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - return value1 - value2; - } - else - { - Matrix4x4 result; - - result.M11 = value1.M11 - value2.M11; - result.M12 = value1.M12 - value2.M12; - result.M13 = value1.M13 - value2.M13; - result.M14 = value1.M14 - value2.M14; - result.M21 = value1.M21 - value2.M21; - result.M22 = value1.M22 - value2.M22; - result.M23 = value1.M23 - value2.M23; - result.M24 = value1.M24 - value2.M24; - result.M31 = value1.M31 - value2.M31; - result.M32 = value1.M32 - value2.M32; - result.M33 = value1.M33 - value2.M33; - result.M34 = value1.M34 - value2.M34; - result.M41 = value1.M41 - value2.M41; - result.M42 = value1.M42 - value2.M42; - result.M43 = value1.M43 - value2.M43; - result.M44 = value1.M44 - value2.M44; - - return result; - } - } + public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) => value1 - value2; /// /// Multiplies a matrix by another matrix. @@ -1961,43 +1874,7 @@ public static Matrix4x4 Subtract(Matrix4x4 value1, Matrix4x4 value2) /// The first source matrix. /// The second source matrix. /// The result of the multiplication. - public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) - { - if (Sse.IsSupported) - { - return value1 * value2; - } - else - { - Matrix4x4 result; - - // First row - result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; - result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; - result.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; - result.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; - - // Second row - result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; - result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; - result.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; - result.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; - - // Third row - result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; - result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; - result.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; - result.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; - - // Fourth row - result.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; - result.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; - result.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; - result.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; - - return result; - } - } + public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) => value1 * value2; /// /// Multiplies a matrix by a scalar value. @@ -2005,36 +1882,7 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, Matrix4x4 value2) /// The source matrix. /// The scaling factor. /// The scaled matrix. - public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) - { - if (Sse.IsSupported) - { - return value1 * value2; - } - else - { - Matrix4x4 result; - - result.M11 = value1.M11 * value2; - result.M12 = value1.M12 * value2; - result.M13 = value1.M13 * value2; - result.M14 = value1.M14 * value2; - result.M21 = value1.M21 * value2; - result.M22 = value1.M22 * value2; - result.M23 = value1.M23 * value2; - result.M24 = value1.M24 * value2; - result.M31 = value1.M31 * value2; - result.M32 = value1.M32 * value2; - result.M33 = value1.M33 * value2; - result.M34 = value1.M34 * value2; - result.M41 = value1.M41 * value2; - result.M42 = value1.M42 * value2; - result.M43 = value1.M43 * value2; - result.M44 = value1.M44 * value2; - - return result; - } - } + public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) => value1 * value2; /// /// Returns a new matrix with the negated elements of the given matrix. @@ -2324,20 +2172,7 @@ public static Matrix4x4 Multiply(Matrix4x4 value1, float value2) /// /// The matrix to compare this instance to. /// True if the matrices are equal; False otherwise. - public bool Equals(Matrix4x4 other) - { - if (Sse.IsSupported) - { - return this == other; - } - else - { - return (M11 == other.M11 && M22 == other.M22 && M33 == other.M33 && M44 == other.M44 && // Check diagonal element first for early out. - M12 == other.M12 && M13 == other.M13 && M14 == other.M14 && M21 == other.M21 && - M23 == other.M23 && M24 == other.M24 && M31 == other.M31 && M32 == other.M32 && - M34 == other.M34 && M41 == other.M41 && M42 == other.M42 && M43 == other.M43); - } - } + public bool Equals(Matrix4x4 other) => this == other; /// /// Returns a boolean indicating whether the given Object is equal to this matrix instance. @@ -2346,9 +2181,9 @@ public bool Equals(Matrix4x4 other) /// True if the Object is equal to this matrix; False otherwise. public override bool Equals(object obj) { - if (obj is Matrix4x4) + if (obj is Matrix4x4 m) { - return Equals((Matrix4x4)obj); + return this == m; } return false; From 2ee0164d847e3838670e189c5d2c0d9f30553e01 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 01:24:27 +0300 Subject: [PATCH 13/28] optimize Transpose --- .../src/System/Numerics/Matrix4x4.cs | 55 ++++++++++++------- 1 file changed, 36 insertions(+), 19 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 7d719d52c7c4..5284ad772953 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1758,28 +1758,45 @@ public static Matrix4x4 Transform(Matrix4x4 value, Quaternion rotation) /// /// The source matrix. /// The transposed matrix. - public static Matrix4x4 Transpose(Matrix4x4 matrix) + public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) { - Matrix4x4 result; + if (Sse.IsSupported) + { + Matrix4x4 result = default; + var t0 = Sse.Shuffle(Sse.LoadVector128(&matrix.M11), Sse.LoadVector128(&matrix.M21), 0x44); + var t1 = Sse.Shuffle(Sse.LoadVector128(&matrix.M31), Sse.LoadVector128(&matrix.M41), 0x44); + var t2 = Sse.Shuffle(Sse.LoadVector128(&matrix.M11), Sse.LoadVector128(&matrix.M21), 0xEE); + var t3 = Sse.Shuffle(Sse.LoadVector128(&matrix.M31), Sse.LoadVector128(&matrix.M41), 0xEE); + + Sse.Store(&result.M11, Sse.Shuffle(t0, t1, 0x88)); + Sse.Store(&result.M21, Sse.Shuffle(t0, t1, 0xDD)); + Sse.Store(&result.M31, Sse.Shuffle(t2, t3, 0x88)); + Sse.Store(&result.M41, Sse.Shuffle(t2, t3, 0xDD)); + return result; + } + else + { + Matrix4x4 result; - result.M11 = matrix.M11; - result.M12 = matrix.M21; - result.M13 = matrix.M31; - result.M14 = matrix.M41; - result.M21 = matrix.M12; - result.M22 = matrix.M22; - result.M23 = matrix.M32; - result.M24 = matrix.M42; - result.M31 = matrix.M13; - result.M32 = matrix.M23; - result.M33 = matrix.M33; - result.M34 = matrix.M43; - result.M41 = matrix.M14; - result.M42 = matrix.M24; - result.M43 = matrix.M34; - result.M44 = matrix.M44; + result.M11 = matrix.M11; + result.M12 = matrix.M21; + result.M13 = matrix.M31; + result.M14 = matrix.M41; + result.M21 = matrix.M12; + result.M22 = matrix.M22; + result.M23 = matrix.M32; + result.M24 = matrix.M42; + result.M31 = matrix.M13; + result.M32 = matrix.M23; + result.M33 = matrix.M33; + result.M34 = matrix.M43; + result.M41 = matrix.M14; + result.M42 = matrix.M24; + result.M43 = matrix.M34; + result.M44 = matrix.M44; - return result; + return result; + } } /// From 95c8fab37fafd4a0fd6c5535db58fd44d9887832 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 01:27:58 +0300 Subject: [PATCH 14/28] simplify Equals --- .../src/System/Numerics/Matrix4x4.cs | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 5284ad772953..849bb65ec593 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2196,15 +2196,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// /// The Object to compare against. /// True if the Object is equal to this matrix; False otherwise. - public override bool Equals(object obj) - { - if (obj is Matrix4x4 m) - { - return this == m; - } - - return false; - } + public override bool Equals(object obj) => (obj is Matrix4x4 other) && (this == other); /// /// Returns a String representing this matrix instance. From 046232391ab4d0f454cf524c69b85f5134e959e2 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 02:07:57 +0300 Subject: [PATCH 15/28] improve Transpose --- .../src/System/Numerics/Matrix4x4.cs | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 849bb65ec593..768fd47ca3c6 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1762,17 +1762,22 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) { if (Sse.IsSupported) { - Matrix4x4 result = default; - var t0 = Sse.Shuffle(Sse.LoadVector128(&matrix.M11), Sse.LoadVector128(&matrix.M21), 0x44); - var t1 = Sse.Shuffle(Sse.LoadVector128(&matrix.M31), Sse.LoadVector128(&matrix.M41), 0x44); - var t2 = Sse.Shuffle(Sse.LoadVector128(&matrix.M11), Sse.LoadVector128(&matrix.M21), 0xEE); - var t3 = Sse.Shuffle(Sse.LoadVector128(&matrix.M31), Sse.LoadVector128(&matrix.M41), 0xEE); - - Sse.Store(&result.M11, Sse.Shuffle(t0, t1, 0x88)); - Sse.Store(&result.M21, Sse.Shuffle(t0, t1, 0xDD)); - Sse.Store(&result.M31, Sse.Shuffle(t2, t3, 0x88)); - Sse.Store(&result.M41, Sse.Shuffle(t2, t3, 0xDD)); - return result; + var row1 = Sse.LoadVector128(&matrix.M11); + var row2 = Sse.LoadVector128(&matrix.M21); + var row3 = Sse.LoadVector128(&matrix.M31); + var row4 = Sse.LoadVector128(&matrix.M41); + + var l12 = Sse.UnpackLow(row1, row2); + var l34 = Sse.UnpackLow(row3, row4); + var h12 = Sse.UnpackHigh(row1, row2); + var h34 = Sse.UnpackHigh(row3, row4); + + Sse.Store(&matrix.M11, Sse.MoveLowToHigh(l12, l34)); + Sse.Store(&matrix.M21, Sse.MoveHighToLow(l34, l12)); + Sse.Store(&matrix.M31, Sse.MoveLowToHigh(h12, h34)); + Sse.Store(&matrix.M41, Sse.MoveHighToLow(h34, h12)); + + return matrix; } else { From cb054b5a70acd0b12548f20f0afff9aeb6d20f0f Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 14:33:57 +0300 Subject: [PATCH 16/28] Lerp as a separate method --- .../src/System/Numerics/Matrix4x4.cs | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 768fd47ca3c6..a6235d118493 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -4,6 +4,7 @@ using System.Globalization; using System.Runtime.InteropServices; +using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; namespace System.Numerics @@ -1803,6 +1804,9 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) return result; } } + + private static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) => + Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); /// /// Linearly interpolates between the corresponding values of two matrices. @@ -1816,23 +1820,10 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float if (Sse.IsSupported) { var amountVec = Sse.SetAllVector128(amount); - - var m1Row = Sse.LoadVector128(&matrix1.M11); - var m2Row = Sse.LoadVector128(&matrix2.M11); - Sse.Store(&matrix1.M11, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - - m1Row = Sse.LoadVector128(&matrix1.M21); - m2Row = Sse.LoadVector128(&matrix2.M21); - Sse.Store(&matrix1.M21, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - - m1Row = Sse.LoadVector128(&matrix1.M31); - m2Row = Sse.LoadVector128(&matrix2.M31); - Sse.Store(&matrix1.M31, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - - m1Row = Sse.LoadVector128(&matrix1.M41); - m2Row = Sse.LoadVector128(&matrix2.M41); - Sse.Store(&matrix1.M41, Sse.Add(m1Row, Sse.Multiply(Sse.Subtract(m2Row, m1Row), amountVec))); - + Sse.Store(&matrix1.M11, Lerp(Sse.LoadVector128(&matrix1.M11), Sse.LoadVector128(&matrix2.M11), amountVec)); + Sse.Store(&matrix1.M21, Lerp(Sse.LoadVector128(&matrix1.M21), Sse.LoadVector128(&matrix2.M21), amountVec)); + Sse.Store(&matrix1.M31, Lerp(Sse.LoadVector128(&matrix1.M31), Sse.LoadVector128(&matrix2.M31), amountVec)); + Sse.Store(&matrix1.M41, Lerp(Sse.LoadVector128(&matrix1.M41), Sse.LoadVector128(&matrix2.M41), amountVec)); return matrix1; } else From 7e01329dda6c7d727e818d960927a463d5b34db3 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 14:50:35 +0300 Subject: [PATCH 17/28] remove SSE from != as it fails some NaN-related tests --- .../src/System/Numerics/Matrix4x4.cs | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index a6235d118493..3cde826fb720 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2163,21 +2163,10 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// True if the given matrices are not equal; False if they are equal. public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) { - if (Sse.IsSupported) - { - return - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 || - Sse.MoveMask(Sse.CompareEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; - } - else - { - return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || - value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || - value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || - value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); - } + return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || + value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || + value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || + value1.M41 != value2.M41 || value1.M42 != value2.M42 || value1.M43 != value2.M43 || value1.M44 != value2.M44); } /// From 3dc516308e976e845308db9b3e5dc989a7c4c845 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 16:17:06 +0300 Subject: [PATCH 18/28] remove unsafe from != --- src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 3cde826fb720..481aceab2525 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2161,7 +2161,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are not equal; False if they are equal. - public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) { return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || From c9a48a7e1c7c702a818e6fe949f826ed113c93c3 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 18:03:02 +0300 Subject: [PATCH 19/28] wrap intrinsics with #if netcoreapp --- .../src/System/Numerics/Matrix4x4.cs | 357 +++++++++--------- 1 file changed, 174 insertions(+), 183 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 481aceab2525..1e065fb6a407 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -1761,6 +1761,7 @@ public static Matrix4x4 Transform(Matrix4x4 value, Quaternion rotation) /// The transposed matrix. public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) { +#if netcoreapp if (Sse.IsSupported) { var row1 = Sse.LoadVector128(&matrix.M11); @@ -1780,31 +1781,29 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) return matrix; } - else - { - Matrix4x4 result; - - result.M11 = matrix.M11; - result.M12 = matrix.M21; - result.M13 = matrix.M31; - result.M14 = matrix.M41; - result.M21 = matrix.M12; - result.M22 = matrix.M22; - result.M23 = matrix.M32; - result.M24 = matrix.M42; - result.M31 = matrix.M13; - result.M32 = matrix.M23; - result.M33 = matrix.M33; - result.M34 = matrix.M43; - result.M41 = matrix.M14; - result.M42 = matrix.M24; - result.M43 = matrix.M34; - result.M44 = matrix.M44; - - return result; - } +#endif + Matrix4x4 result; + + result.M11 = matrix.M11; + result.M12 = matrix.M21; + result.M13 = matrix.M31; + result.M14 = matrix.M41; + result.M21 = matrix.M12; + result.M22 = matrix.M22; + result.M23 = matrix.M32; + result.M24 = matrix.M42; + result.M31 = matrix.M13; + result.M32 = matrix.M23; + result.M33 = matrix.M33; + result.M34 = matrix.M43; + result.M41 = matrix.M14; + result.M42 = matrix.M24; + result.M43 = matrix.M34; + result.M44 = matrix.M44; + + return result; } - + private static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) => Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); @@ -1817,6 +1816,7 @@ private static Vector128 Lerp(Vector128 a, Vector128 b, Vec /// The interpolated matrix. public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) { +#if netcoreapp if (Sse.IsSupported) { var amountVec = Sse.SetAllVector128(amount); @@ -1826,36 +1826,34 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.Store(&matrix1.M41, Lerp(Sse.LoadVector128(&matrix1.M41), Sse.LoadVector128(&matrix2.M41), amountVec)); return matrix1; } - else - { - Matrix4x4 result; - - // First row - result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; - result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; - result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount; - result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount; - - // Second row - result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; - result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; - result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount; - result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount; - - // Third row - result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; - result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; - result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount; - result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount; - - // Fourth row - result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount; - result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount; - result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount; - result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount; - - return result; - } +#endif + Matrix4x4 result; + + // First row + result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; + result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; + result.M13 = matrix1.M13 + (matrix2.M13 - matrix1.M13) * amount; + result.M14 = matrix1.M14 + (matrix2.M14 - matrix1.M14) * amount; + + // Second row + result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; + result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; + result.M23 = matrix1.M23 + (matrix2.M23 - matrix1.M23) * amount; + result.M24 = matrix1.M24 + (matrix2.M24 - matrix1.M24) * amount; + + // Third row + result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; + result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; + result.M33 = matrix1.M33 + (matrix2.M33 - matrix1.M33) * amount; + result.M34 = matrix1.M34 + (matrix2.M34 - matrix1.M34) * amount; + + // Fourth row + result.M41 = matrix1.M41 + (matrix2.M41 - matrix1.M41) * amount; + result.M42 = matrix1.M42 + (matrix2.M42 - matrix1.M42) * amount; + result.M43 = matrix1.M43 + (matrix2.M43 - matrix1.M43) * amount; + result.M44 = matrix1.M44 + (matrix2.M44 - matrix1.M44) * amount; + + return result; } /// @@ -1904,6 +1902,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The negated matrix. public static unsafe Matrix4x4 operator -(Matrix4x4 value) { +#if netcoreapp if (Sse.IsSupported) { var zero = Sse.SetZeroVector128(); @@ -1914,29 +1913,27 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float return value; } - else - { - Matrix4x4 m; - - m.M11 = -value.M11; - m.M12 = -value.M12; - m.M13 = -value.M13; - m.M14 = -value.M14; - m.M21 = -value.M21; - m.M22 = -value.M22; - m.M23 = -value.M23; - m.M24 = -value.M24; - m.M31 = -value.M31; - m.M32 = -value.M32; - m.M33 = -value.M33; - m.M34 = -value.M34; - m.M41 = -value.M41; - m.M42 = -value.M42; - m.M43 = -value.M43; - m.M44 = -value.M44; - - return m; - } +#endif + Matrix4x4 m; + + m.M11 = -value.M11; + m.M12 = -value.M12; + m.M13 = -value.M13; + m.M14 = -value.M14; + m.M21 = -value.M21; + m.M22 = -value.M22; + m.M23 = -value.M23; + m.M24 = -value.M24; + m.M31 = -value.M31; + m.M32 = -value.M32; + m.M33 = -value.M33; + m.M34 = -value.M34; + m.M41 = -value.M41; + m.M42 = -value.M42; + m.M43 = -value.M43; + m.M44 = -value.M44; + + return m; } /// @@ -1947,6 +1944,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The resulting matrix. public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) { +#if netcoreapp if (Sse.IsSupported) { Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); @@ -1955,29 +1953,27 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.Store(&value1.M41, Sse.Add(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); return value1; } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 + value2.M11; - m.M12 = value1.M12 + value2.M12; - m.M13 = value1.M13 + value2.M13; - m.M14 = value1.M14 + value2.M14; - m.M21 = value1.M21 + value2.M21; - m.M22 = value1.M22 + value2.M22; - m.M23 = value1.M23 + value2.M23; - m.M24 = value1.M24 + value2.M24; - m.M31 = value1.M31 + value2.M31; - m.M32 = value1.M32 + value2.M32; - m.M33 = value1.M33 + value2.M33; - m.M34 = value1.M34 + value2.M34; - m.M41 = value1.M41 + value2.M41; - m.M42 = value1.M42 + value2.M42; - m.M43 = value1.M43 + value2.M43; - m.M44 = value1.M44 + value2.M44; - - return m; - } +#endif + Matrix4x4 m; + + m.M11 = value1.M11 + value2.M11; + m.M12 = value1.M12 + value2.M12; + m.M13 = value1.M13 + value2.M13; + m.M14 = value1.M14 + value2.M14; + m.M21 = value1.M21 + value2.M21; + m.M22 = value1.M22 + value2.M22; + m.M23 = value1.M23 + value2.M23; + m.M24 = value1.M24 + value2.M24; + m.M31 = value1.M31 + value2.M31; + m.M32 = value1.M32 + value2.M32; + m.M33 = value1.M33 + value2.M33; + m.M34 = value1.M34 + value2.M34; + m.M41 = value1.M41 + value2.M41; + m.M42 = value1.M42 + value2.M42; + m.M43 = value1.M43 + value2.M43; + m.M44 = value1.M44 + value2.M44; + + return m; } /// @@ -1988,6 +1984,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The result of the subtraction. public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) { +#if netcoreapp if (Sse.IsSupported) { Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); @@ -1996,29 +1993,27 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.Store(&value1.M41, Sse.Subtract(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value1.M41))); return value1; } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 - value2.M11; - m.M12 = value1.M12 - value2.M12; - m.M13 = value1.M13 - value2.M13; - m.M14 = value1.M14 - value2.M14; - m.M21 = value1.M21 - value2.M21; - m.M22 = value1.M22 - value2.M22; - m.M23 = value1.M23 - value2.M23; - m.M24 = value1.M24 - value2.M24; - m.M31 = value1.M31 - value2.M31; - m.M32 = value1.M32 - value2.M32; - m.M33 = value1.M33 - value2.M33; - m.M34 = value1.M34 - value2.M34; - m.M41 = value1.M41 - value2.M41; - m.M42 = value1.M42 - value2.M42; - m.M43 = value1.M43 - value2.M43; - m.M44 = value1.M44 - value2.M44; - - return m; - } +#endif + Matrix4x4 m; + + m.M11 = value1.M11 - value2.M11; + m.M12 = value1.M12 - value2.M12; + m.M13 = value1.M13 - value2.M13; + m.M14 = value1.M14 - value2.M14; + m.M21 = value1.M21 - value2.M21; + m.M22 = value1.M22 - value2.M22; + m.M23 = value1.M23 - value2.M23; + m.M24 = value1.M24 - value2.M24; + m.M31 = value1.M31 - value2.M31; + m.M32 = value1.M32 - value2.M32; + m.M33 = value1.M33 - value2.M33; + m.M34 = value1.M34 - value2.M34; + m.M41 = value1.M41 - value2.M41; + m.M42 = value1.M42 - value2.M42; + m.M43 = value1.M43 - value2.M43; + m.M44 = value1.M44 - value2.M44; + + return m; } /// @@ -2029,6 +2024,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The result of the multiplication. public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) { +#if netcoreapp if (Sse.IsSupported) { Sse.Store(&value1.M11, @@ -2056,36 +2052,34 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); return value1; } - else - { - Matrix4x4 m; - - // First row - m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; - m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; - m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; - m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; - - // Second row - m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; - m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; - m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; - m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; - - // Third row - m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; - m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; - m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; - m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; - - // Fourth row - m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; - m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; - m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; - m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; - - return m; - } +#endif + Matrix4x4 m; + + // First row + m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21 + value1.M13 * value2.M31 + value1.M14 * value2.M41; + m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22 + value1.M13 * value2.M32 + value1.M14 * value2.M42; + m.M13 = value1.M11 * value2.M13 + value1.M12 * value2.M23 + value1.M13 * value2.M33 + value1.M14 * value2.M43; + m.M14 = value1.M11 * value2.M14 + value1.M12 * value2.M24 + value1.M13 * value2.M34 + value1.M14 * value2.M44; + + // Second row + m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21 + value1.M23 * value2.M31 + value1.M24 * value2.M41; + m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22 + value1.M23 * value2.M32 + value1.M24 * value2.M42; + m.M23 = value1.M21 * value2.M13 + value1.M22 * value2.M23 + value1.M23 * value2.M33 + value1.M24 * value2.M43; + m.M24 = value1.M21 * value2.M14 + value1.M22 * value2.M24 + value1.M23 * value2.M34 + value1.M24 * value2.M44; + + // Third row + m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value1.M33 * value2.M31 + value1.M34 * value2.M41; + m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value1.M33 * value2.M32 + value1.M34 * value2.M42; + m.M33 = value1.M31 * value2.M13 + value1.M32 * value2.M23 + value1.M33 * value2.M33 + value1.M34 * value2.M43; + m.M34 = value1.M31 * value2.M14 + value1.M32 * value2.M24 + value1.M33 * value2.M34 + value1.M34 * value2.M44; + + // Fourth row + m.M41 = value1.M41 * value2.M11 + value1.M42 * value2.M21 + value1.M43 * value2.M31 + value1.M44 * value2.M41; + m.M42 = value1.M41 * value2.M12 + value1.M42 * value2.M22 + value1.M43 * value2.M32 + value1.M44 * value2.M42; + m.M43 = value1.M41 * value2.M13 + value1.M42 * value2.M23 + value1.M43 * value2.M33 + value1.M44 * value2.M43; + m.M44 = value1.M41 * value2.M14 + value1.M42 * value2.M24 + value1.M43 * value2.M34 + value1.M44 * value2.M44; + + return m; } /// @@ -2096,7 +2090,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The scaled matrix. public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) { - +#if netcoreapp if (Sse.IsSupported) { var value2Vec = Sse.SetAllVector128(value2); @@ -2106,28 +2100,26 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.Store(&value1.M41, Sse.Multiply(Sse.LoadVector128(&value1.M41), value2Vec)); return value1; } - else - { - Matrix4x4 m; - - m.M11 = value1.M11 * value2; - m.M12 = value1.M12 * value2; - m.M13 = value1.M13 * value2; - m.M14 = value1.M14 * value2; - m.M21 = value1.M21 * value2; - m.M22 = value1.M22 * value2; - m.M23 = value1.M23 * value2; - m.M24 = value1.M24 * value2; - m.M31 = value1.M31 * value2; - m.M32 = value1.M32 * value2; - m.M33 = value1.M33 * value2; - m.M34 = value1.M34 * value2; - m.M41 = value1.M41 * value2; - m.M42 = value1.M42 * value2; - m.M43 = value1.M43 * value2; - m.M44 = value1.M44 * value2; - return m; - } +#endif + Matrix4x4 m; + + m.M11 = value1.M11 * value2; + m.M12 = value1.M12 * value2; + m.M13 = value1.M13 * value2; + m.M14 = value1.M14 * value2; + m.M21 = value1.M21 * value2; + m.M22 = value1.M22 * value2; + m.M23 = value1.M23 * value2; + m.M24 = value1.M24 * value2; + m.M31 = value1.M31 * value2; + m.M32 = value1.M32 * value2; + m.M33 = value1.M33 * value2; + m.M34 = value1.M34 * value2; + m.M41 = value1.M41 * value2; + m.M42 = value1.M42 * value2; + m.M43 = value1.M43 * value2; + m.M44 = value1.M44 * value2; + return m; } /// @@ -2138,6 +2130,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// True if the given matrices are equal; False otherwise. public static unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2) { +#if netcoreapp if (Sse.IsSupported) { return @@ -2146,13 +2139,11 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 && Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; } - else - { - return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. - value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && value1.M21 == value2.M21 && - value1.M23 == value2.M23 && value1.M24 == value2.M24 && value1.M31 == value2.M31 && value1.M32 == value2.M32 && - value1.M34 == value2.M34 && value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); - } +#endif + return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. + value1.M12 == value2.M12 && value1.M13 == value2.M13 && value1.M14 == value2.M14 && value1.M21 == value2.M21 && + value1.M23 == value2.M23 && value1.M24 == value2.M24 && value1.M31 == value2.M31 && value1.M32 == value2.M32 && + value1.M34 == value2.M34 && value1.M41 == value2.M41 && value1.M42 == value2.M42 && value1.M43 == value2.M43); } /// From bb755b9dcbb5cef5d4ae2753efc7bf68cea5001a Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 18:11:59 +0300 Subject: [PATCH 20/28] forgot Letp method and usings --- src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 1e065fb6a407..cc2fdc766b75 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -4,8 +4,10 @@ using System.Globalization; using System.Runtime.InteropServices; +#if netcoreapp using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; +#endif namespace System.Numerics { @@ -1804,8 +1806,10 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) return result; } +#if netcoreapp private static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) => Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); +#endif /// /// Linearly interpolates between the corresponding values of two matrices. From e30f77e7de91260e6fa01ed8ecd84683eaa67162 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 19:21:32 +0300 Subject: [PATCH 21/28] define netcoreapp in csproj --- src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj | 1 + 1 file changed, 1 insertion(+) diff --git a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj index c17b0876c5af..3d37699ff2af 100644 --- a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj +++ b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj @@ -4,6 +4,7 @@ System.Numerics $(OutputPath)$(MSBuildProjectName).xml true + $(DefineConstants);netcoreapp true true true From b417948741601df62d065dd185e419cc643b088b Mon Sep 17 00:00:00 2001 From: EgorBo Date: Thu, 16 Aug 2018 19:47:19 +0300 Subject: [PATCH 22/28] rename netcoreapp symbol to HAS_INTRINSICS --- .../src/System.Numerics.Vectors.csproj | 2 +- .../src/System/Numerics/Matrix4x4.cs | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj index 3d37699ff2af..ea8cde0420d7 100644 --- a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj +++ b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj @@ -4,10 +4,10 @@ System.Numerics $(OutputPath)$(MSBuildProjectName).xml true - $(DefineConstants);netcoreapp true true true + $(DefineConstants);HAS_INTRINSICS netstandard1.0;portable-net45+win8+wp8+wpa81 net46-Debug;net46-Release;netcoreapp-Unix-Debug;netcoreapp-Unix-Release;netcoreapp-Windows_NT-Debug;netcoreapp-Windows_NT-Release;netfx-Windows_NT-Debug;netfx-Windows_NT-Release;netstandard-Debug;netstandard-Release;netstandard1.0-Debug;netstandard1.0-Release;uap-Windows_NT-Debug;uap-Windows_NT-Release;uapaot-Windows_NT-Debug;uapaot-Windows_NT-Release diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index cc2fdc766b75..ecb53124705f 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -4,7 +4,7 @@ using System.Globalization; using System.Runtime.InteropServices; -#if netcoreapp +#if HAS_INTRINSICS using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; #endif @@ -1763,7 +1763,7 @@ public static Matrix4x4 Transform(Matrix4x4 value, Quaternion rotation) /// The transposed matrix. public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { var row1 = Sse.LoadVector128(&matrix.M11); @@ -1806,7 +1806,7 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) return result; } -#if netcoreapp +#if HAS_INTRINSICS private static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) => Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); #endif @@ -1820,7 +1820,7 @@ private static Vector128 Lerp(Vector128 a, Vector128 b, Vec /// The interpolated matrix. public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float amount) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { var amountVec = Sse.SetAllVector128(amount); @@ -1906,7 +1906,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The negated matrix. public static unsafe Matrix4x4 operator -(Matrix4x4 value) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { var zero = Sse.SetZeroVector128(); @@ -1948,7 +1948,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The resulting matrix. public static unsafe Matrix4x4 operator +(Matrix4x4 value1, Matrix4x4 value2) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { Sse.Store(&value1.M11, Sse.Add(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); @@ -1988,7 +1988,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The result of the subtraction. public static unsafe Matrix4x4 operator -(Matrix4x4 value1, Matrix4x4 value2) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { Sse.Store(&value1.M11, Sse.Subtract(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value1.M11))); @@ -2028,7 +2028,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The result of the multiplication. public static unsafe Matrix4x4 operator *(Matrix4x4 value1, Matrix4x4 value2) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { Sse.Store(&value1.M11, @@ -2094,7 +2094,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The scaled matrix. public static unsafe Matrix4x4 operator *(Matrix4x4 value1, float value2) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { var value2Vec = Sse.SetAllVector128(value2); @@ -2134,7 +2134,7 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// True if the given matrices are equal; False otherwise. public static unsafe bool operator ==(Matrix4x4 value1, Matrix4x4 value2) { -#if netcoreapp +#if HAS_INTRINSICS if (Sse.IsSupported) { return From f2710fee90100c9529bb083190929f6abb5a88b3 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 00:36:36 +0300 Subject: [PATCH 23/28] Move Equal and Lerp to VectorMath.cs internal helper --- .../src/System.Numerics.Vectors.csproj | 1 + .../src/System/Numerics/Matrix4x4.cs | 22 ++++------- .../src/System/Numerics/VectorMath.cs | 38 +++++++++++++++++++ 3 files changed, 47 insertions(+), 14 deletions(-) create mode 100644 src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs diff --git a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj index ea8cde0420d7..89737426c46c 100644 --- a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj +++ b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj @@ -62,6 +62,7 @@ + diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index ecb53124705f..6f65c1a863d4 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -5,7 +5,6 @@ using System.Globalization; using System.Runtime.InteropServices; #if HAS_INTRINSICS -using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; #endif @@ -1806,11 +1805,6 @@ public static unsafe Matrix4x4 Transpose(Matrix4x4 matrix) return result; } -#if HAS_INTRINSICS - private static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) => - Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); -#endif - /// /// Linearly interpolates between the corresponding values of two matrices. /// @@ -1824,10 +1818,10 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float if (Sse.IsSupported) { var amountVec = Sse.SetAllVector128(amount); - Sse.Store(&matrix1.M11, Lerp(Sse.LoadVector128(&matrix1.M11), Sse.LoadVector128(&matrix2.M11), amountVec)); - Sse.Store(&matrix1.M21, Lerp(Sse.LoadVector128(&matrix1.M21), Sse.LoadVector128(&matrix2.M21), amountVec)); - Sse.Store(&matrix1.M31, Lerp(Sse.LoadVector128(&matrix1.M31), Sse.LoadVector128(&matrix2.M31), amountVec)); - Sse.Store(&matrix1.M41, Lerp(Sse.LoadVector128(&matrix1.M41), Sse.LoadVector128(&matrix2.M41), amountVec)); + Sse.Store(&matrix1.M11, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M11), Sse.LoadVector128(&matrix2.M11), amountVec)); + Sse.Store(&matrix1.M21, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M21), Sse.LoadVector128(&matrix2.M21), amountVec)); + Sse.Store(&matrix1.M31, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M31), Sse.LoadVector128(&matrix2.M31), amountVec)); + Sse.Store(&matrix1.M41, VectorMath.Lerp(Sse.LoadVector128(&matrix1.M41), Sse.LoadVector128(&matrix2.M41), amountVec)); return matrix1; } #endif @@ -2138,10 +2132,10 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float if (Sse.IsSupported) { return - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11))) == 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21))) == 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31))) == 0 && - Sse.MoveMask(Sse.CompareNotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41))) == 0; + VectorMath.Equal(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)) && + VectorMath.Equal(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)) && + VectorMath.Equal(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)) && + VectorMath.Equal(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41)); } #endif return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && value1.M33 == value2.M33 && value1.M44 == value2.M44 && // Check diagonal element first for early out. diff --git a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs new file mode 100644 index 000000000000..dcc0dc61f99c --- /dev/null +++ b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs @@ -0,0 +1,38 @@ +// Licensed to the .NET Foundation under one or more agreements. +// The .NET Foundation licenses this file to you under the MIT license. +// See the LICENSE file in the project root for more information. +#if HAS_INTRINSICS +using System.Runtime.Intrinsics; +using System.Runtime.Intrinsics.X86; + +namespace System.Numerics +{ + internal static class VectorMath + { + public static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) + { + if (Sse.IsSupported) + { + return Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); + } + else + { + throw new PlatformNotSupportedException(); + } + } + + public static bool Equal(Vector128 vector1, Vector128 vector2) + { + if (Sse.IsSupported) + { + // do not use !Equal if vectors contain NaN + return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) == 0; + } + else + { + throw new PlatformNotSupportedException(); + } + } + } +} +#endif From 0436823b7b3f599627ee887bbc1b8cac32a7208b Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 02:39:09 +0300 Subject: [PATCH 24/28] Implement != operator --- .../src/System/Numerics/Matrix4x4.cs | 12 +++++++++++- .../src/System/Numerics/VectorMath.cs | 13 ++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index 6f65c1a863d4..e871cd248ba7 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2150,8 +2150,18 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float /// The first matrix to compare. /// The second matrix to compare. /// True if the given matrices are not equal; False if they are equal. - public static bool operator !=(Matrix4x4 value1, Matrix4x4 value2) + public static unsafe bool operator !=(Matrix4x4 value1, Matrix4x4 value2) { +#if HAS_INTRINSICS + if (Sse.IsSupported) + { + return + VectorMath.NotEqual(Sse.LoadVector128(&value1.M11), Sse.LoadVector128(&value2.M11)) || + VectorMath.NotEqual(Sse.LoadVector128(&value1.M21), Sse.LoadVector128(&value2.M21)) || + VectorMath.NotEqual(Sse.LoadVector128(&value1.M31), Sse.LoadVector128(&value2.M31)) || + VectorMath.NotEqual(Sse.LoadVector128(&value1.M41), Sse.LoadVector128(&value2.M41)); + } +#endif return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || value1.M13 != value2.M13 || value1.M14 != value2.M14 || value1.M21 != value2.M21 || value1.M22 != value2.M22 || value1.M23 != value2.M23 || value1.M24 != value2.M24 || value1.M31 != value2.M31 || value1.M32 != value2.M32 || value1.M33 != value2.M33 || value1.M34 != value2.M34 || diff --git a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs index dcc0dc61f99c..dd65d561ccd5 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs @@ -25,7 +25,6 @@ public static bool Equal(Vector128 vector1, Vector128 vector2) { if (Sse.IsSupported) { - // do not use !Equal if vectors contain NaN return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) == 0; } else @@ -33,6 +32,18 @@ public static bool Equal(Vector128 vector1, Vector128 vector2) throw new PlatformNotSupportedException(); } } + + public static bool NotEqual(Vector128 vector1, Vector128 vector2) + { + if (Sse.IsSupported) + { + return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) != 0; + } + else + { + throw new PlatformNotSupportedException(); + } + } } } #endif From 3822f4009930a963b97d0cff9f354b63ad2210e4 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 02:42:27 +0300 Subject: [PATCH 25/28] Debug.Assert(Sse.IsSupported) in VectorMath --- .../src/System/Numerics/VectorMath.cs | 31 +++++-------------- 1 file changed, 7 insertions(+), 24 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs index dd65d561ccd5..385376e9e5bd 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs @@ -2,6 +2,7 @@ // The .NET Foundation licenses this file to you under the MIT license. // See the LICENSE file in the project root for more information. #if HAS_INTRINSICS +using System.Diagnostics; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; @@ -11,38 +12,20 @@ internal static class VectorMath { public static Vector128 Lerp(Vector128 a, Vector128 b, Vector128 t) { - if (Sse.IsSupported) - { - return Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); - } - else - { - throw new PlatformNotSupportedException(); - } + Debug.Assert(Sse.IsSupported); + return Sse.Add(a, Sse.Multiply(Sse.Subtract(b, a), t)); } public static bool Equal(Vector128 vector1, Vector128 vector2) { - if (Sse.IsSupported) - { - return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) == 0; - } - else - { - throw new PlatformNotSupportedException(); - } + Debug.Assert(Sse.IsSupported); + return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) == 0; } public static bool NotEqual(Vector128 vector1, Vector128 vector2) { - if (Sse.IsSupported) - { - return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) != 0; - } - else - { - throw new PlatformNotSupportedException(); - } + Debug.Assert(Sse.IsSupported); + return Sse.MoveMask(Sse.CompareNotEqual(vector1, vector2)) != 0; } } } From e5f03f875bdc06d7215a1e00d8c048e82562ff2f Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 19:50:48 +0300 Subject: [PATCH 26/28] replace SetAllVector with Shuffle in Multiply --- .../src/System/Numerics/Matrix4x4.cs | 39 +++++++++++-------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs index e871cd248ba7..0420cece86b2 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/Matrix4x4.cs @@ -2025,29 +2025,36 @@ public static unsafe Matrix4x4 Lerp(Matrix4x4 matrix1, Matrix4x4 matrix2, float #if HAS_INTRINSICS if (Sse.IsSupported) { + var row = Sse.LoadVector128(&value1.M11); Sse.Store(&value1.M11, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M11), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M12), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M13), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M14), Sse.LoadVector128(&value2.M41))))); + Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41))))); + // 0x00 is _MM_SHUFFLE(0,0,0,0), 0x55 is _MM_SHUFFLE(1,1,1,1), etc. + // TODO: Replace with a method once it's added to the API. + + row = Sse.LoadVector128(&value1.M21); Sse.Store(&value1.M21, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M21), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M22), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M23), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M24), Sse.LoadVector128(&value2.M41))))); + Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41))))); + row = Sse.LoadVector128(&value1.M31); Sse.Store(&value1.M31, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M31), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M32), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M33), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M34), Sse.LoadVector128(&value2.M41))))); + Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41))))); + row = Sse.LoadVector128(&value1.M41); Sse.Store(&value1.M41, - Sse.Add(Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M41), Sse.LoadVector128(&value2.M11)), - Sse.Multiply(Sse.SetAllVector128(value1.M42), Sse.LoadVector128(&value2.M21))), - Sse.Add(Sse.Multiply(Sse.SetAllVector128(value1.M43), Sse.LoadVector128(&value2.M31)), - Sse.Multiply(Sse.SetAllVector128(value1.M44), Sse.LoadVector128(&value2.M41))))); + Sse.Add(Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0x00), Sse.LoadVector128(&value2.M11)), + Sse.Multiply(Sse.Shuffle(row, row, 0x55), Sse.LoadVector128(&value2.M21))), + Sse.Add(Sse.Multiply(Sse.Shuffle(row, row, 0xAA), Sse.LoadVector128(&value2.M31)), + Sse.Multiply(Sse.Shuffle(row, row, 0xFF), Sse.LoadVector128(&value2.M41))))); return value1; } #endif From c0ac090efa38c51463d86229a961d884c0534795 Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 19:57:21 +0300 Subject: [PATCH 27/28] remove #if HAS_INTRINSICS from VectorMath --- .../src/System.Numerics.Vectors.csproj | 5 ++++- .../src/System/Numerics/VectorMath.cs | 2 -- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj index 89737426c46c..db4cc3014733 100644 --- a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj +++ b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj @@ -7,7 +7,8 @@ true true true - $(DefineConstants);HAS_INTRINSICS + true + $(DefineConstants);HAS_INTRINSICS netstandard1.0;portable-net45+win8+wp8+wpa81 net46-Debug;net46-Release;netcoreapp-Unix-Debug;netcoreapp-Unix-Release;netcoreapp-Windows_NT-Debug;netcoreapp-Windows_NT-Release;netfx-Windows_NT-Debug;netfx-Windows_NT-Release;netstandard-Debug;netstandard-Release;netstandard1.0-Debug;netstandard1.0-Release;uap-Windows_NT-Debug;uap-Windows_NT-Release;uapaot-Windows_NT-Debug;uapaot-Windows_NT-Release @@ -62,6 +63,8 @@ + + diff --git a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs index 385376e9e5bd..cec1e3a71bed 100644 --- a/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs +++ b/src/System.Numerics.Vectors/src/System/Numerics/VectorMath.cs @@ -1,7 +1,6 @@ // Licensed to the .NET Foundation under one or more agreements. // The .NET Foundation licenses this file to you under the MIT license. // See the LICENSE file in the project root for more information. -#if HAS_INTRINSICS using System.Diagnostics; using System.Runtime.Intrinsics; using System.Runtime.Intrinsics.X86; @@ -29,4 +28,3 @@ public static bool NotEqual(Vector128 vector1, Vector128 vector2) } } } -#endif From 15717e5c6bdc084690505897f476980936ae245f Mon Sep 17 00:00:00 2001 From: EgorBo Date: Fri, 17 Aug 2018 19:59:22 +0300 Subject: [PATCH 28/28] fix indention in System.Numerics.Vectors.csproj --- src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj index db4cc3014733..9f7e3eabb372 100644 --- a/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj +++ b/src/System.Numerics.Vectors/src/System.Numerics.Vectors.csproj @@ -7,7 +7,7 @@ true true true - true + true $(DefineConstants);HAS_INTRINSICS netstandard1.0;portable-net45+win8+wp8+wpa81 net46-Debug;net46-Release;netcoreapp-Unix-Debug;netcoreapp-Unix-Release;netcoreapp-Windows_NT-Debug;netcoreapp-Windows_NT-Release;netfx-Windows_NT-Debug;netfx-Windows_NT-Release;netstandard-Debug;netstandard-Release;netstandard1.0-Debug;netstandard1.0-Release;uap-Windows_NT-Debug;uap-Windows_NT-Release;uapaot-Windows_NT-Debug;uapaot-Windows_NT-Release