Replace Matrix3 with System.Numerics.Matrix3x2 (#5078)

* Delete Matrix3. Replace with System.Numerics.Matrix3x2

* Feedback

* release notes

Robust.Shared.Maths/Box2Rotated.cs

@@ -33,7 +33,7 @@ namespace Robust.Shared.Maths
         public readonly Vector2 BottomLeft => Origin + Rotation.RotateVec(Box.BottomLeft - Origin);
         public readonly Vector2 Center => Origin + Rotation.RotateVec((Box.BottomLeft + Box.TopRight)/2 - Origin);
 
-        public Matrix3 Transform => Matrix3.CreateTransform(Origin - Rotation.RotateVec(Origin), Rotation);
+        public Matrix3x2 Transform => Matrix3Helpers.CreateTransform(Origin - Rotation.RotateVec(Origin), Rotation);
 
         public Box2Rotated(Vector2 bottomLeft, Vector2 topRight)
             : this(new Box2(bottomLeft, topRight))

Robust.Shared.Maths/Matrix3Helpers.cs

@@ -0,0 +1,193 @@
+using System;
+using System.Numerics;
+using System.Runtime.CompilerServices;
+using System.Runtime.Intrinsics;
+
+
+namespace Robust.Shared.Maths;
+
+public static class Matrix3Helpers
+{
+    public static bool EqualsApprox(this Matrix3x2 a, Matrix3x2 b, float tolerance = 1e-6f)
+    {
+        return
+            Math.Abs(a.M11 - b.M11) <= tolerance &&
+            Math.Abs(a.M12 - b.M12) <= tolerance &&
+            Math.Abs(a.M21 - b.M21) <= tolerance &&
+            Math.Abs(a.M22 - b.M22) <= tolerance &&
+            Math.Abs(a.M31 - b.M31) <= tolerance &&
+            Math.Abs(a.M32 - b.M32) <= tolerance;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static bool EqualsApprox(this Matrix3x2 a, Matrix3x2 b, double tolerance)
+    {
+        return a.EqualsApprox(b, (float) tolerance);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Box2 TransformBox(this Matrix3x2 refFromBox, Box2Rotated box)
+    {
+        return (box.Transform * refFromBox).TransformBox(box.Box);
+    }
+
+    public static Box2 TransformBox(this Matrix3x2 refFromBox, in Box2 box)
+    {
+        // Do transformation on all 4 corners of the box at once.
+        // Then min/max the results to get the new AABB.
+
+        var boxVec = Unsafe.As<Box2, Vector128<float>>(ref Unsafe.AsRef(in box));
+
+        // Convert box into list of X and Y values for each of the 4 corners
+        var allX = Vector128.Shuffle(boxVec, Vector128.Create(0, 0, 2, 2));
+        var allY = Vector128.Shuffle(boxVec, Vector128.Create(1, 3, 3, 1));
+
+        // Transform coordinates
+        var modX = allX * Vector128.Create(refFromBox.M11);
+        var modY = allX * Vector128.Create(refFromBox.M12);
+        modX += allY * Vector128.Create(refFromBox.M21);
+        modY += allY * Vector128.Create(refFromBox.M22);
+        modX += Vector128.Create(refFromBox.M31);
+        modY += Vector128.Create(refFromBox.M32);
+
+        // Get bounding box by finding the min and max X and Y values.
+        var l = SimdHelpers.MinHorizontal128(modX);
+        var b = SimdHelpers.MinHorizontal128(modY);
+        var r = SimdHelpers.MaxHorizontal128(modX);
+        var t = SimdHelpers.MaxHorizontal128(modY);
+
+        var lbrt = SimdHelpers.MergeRows128(l, b, r, t);
+        return Unsafe.As<Vector128<float>, Box2>(ref lbrt);
+    }
+
+    /// <summary>
+    /// Gets the rotation of the Matrix. Will have some precision loss.
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Angle Rotation(this Matrix3x2 t)
+    {
+        return new Vector2(t.M11, t.M12).ToAngle();
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateTransform(float posX, float posY, double angle, float scaleX = 1, float scaleY = 1)
+    {
+        // returns a matrix that is equivalent to returning CreateScale(scale) * CreateRotation(angle) * CreateTranslation(posX, posY)
+
+        var sin = (float) Math.Sin(angle);
+        var cos = (float) Math.Cos(angle);
+
+        return new Matrix3x2
+        {
+            M11 = cos * scaleX,
+            M21 = -sin * scaleY,
+            M31 = posX,
+            M12 = sin * scaleX,
+            M22 = cos * scaleY,
+            M32 = posY,
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateTransform(in Vector2 position, in Angle angle)
+    {
+        // Rounding moment
+        return angle.Theta switch
+        {
+            -Math.PI / 2 => new Matrix3x2(0f, -1f, 1, 0, position.X, position.Y),
+            Math.PI / 2 => new Matrix3x2(0f, 1f, -1f, 0f, position.X, position.Y),
+            Math.PI => new Matrix3x2(-1f, 0f, 0f, -1f, position.X, position.Y),
+            _ => CreateTransform(position.X, position.Y, (float) angle.Theta)
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateTransform(in Vector2 position, in Angle angle, in Vector2 scale)
+    {
+        return CreateTransform(position.X, position.Y, (float)angle.Theta, scale.X, scale.Y);
+    }
+
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateInverseTransform(float posX, float posY, double angle, float scaleX = 1, float scaleY = 1)
+    {
+        // returns a matrix that is equivalent to returning CreateTranslation(-posX, -posY) * CreateRotation(-angle) * CreateScale(1/scaleX, 1/scaleY)
+
+        var sin = (float) Math.Sin(angle);
+        var cos = (float) Math.Cos(angle);
+
+        return new Matrix3x2
+        {
+            M11 = cos / scaleX,
+            M21 = sin / scaleX,
+            M31 = - (posX * cos + posY * sin) / scaleX,
+            M12 = -sin / scaleY,
+            M22 = cos / scaleY,
+            M32 = (posX * sin - posY * cos) / scaleY,
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateInverseTransform(in Vector2 position, in Angle angle)
+    {
+        return CreateInverseTransform(position.X, position.Y, (float)angle.Theta);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateInverseTransform(in Vector2 position, in Angle angle, in Vector2 scale)
+    {
+        return CreateInverseTransform(position.X, position.Y, (float)angle.Theta, scale.X, scale.Y);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateTranslation(float x, float y)
+    {
+        return new Matrix3x2 {
+            M11 = 1,
+            M12 = 0,
+            M21 = 0,
+            M22 = 1,
+            M31 = x,
+            M32 = y
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateTranslation(Vector2 vector)
+    {
+        return CreateTranslation(vector.X, vector.Y);
+    }
+
+    public static Matrix3x2 CreateRotation(double angle)
+    {
+        var cos = (float) Math.Cos(angle);
+        var sin = (float) Math.Sin(angle);
+        return new Matrix3x2 {
+            M11 = cos,
+            M12 = sin,
+            M21 = -sin,
+            M22 = cos,
+            M31 = 0,
+            M32 = 0
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateScale(float x, float y)
+    {
+        return new Matrix3x2 {
+            M11 = x,
+            M12 = 0,
+            M21 = 0,
+            M22 = y,
+            M31 = 0,
+            M32 = 0
+        };
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Matrix3x2 CreateScale(in Vector2 scale)
+    {
+        return CreateScale(scale.X, scale.Y);
+    }
+}

Robust.Shared.Maths/Quaternion.cs

@@ -1,4 +1,4 @@
-#region --- License ---
+#region --- License ---
 
 /*
 Copyright (c) 2006 - 2008 The Open Toolkit library.
@@ -26,6 +26,7 @@ SOFTWARE.
 #endregion --- License ---
 
 using System;
+using System.Numerics;
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using System.Xml.Serialization;
@@ -73,9 +74,9 @@ namespace Robust.Shared.Maths
             : this(new Vector3(x, y, z), w) { }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public Quaternion(ref Matrix3 matrix)
+        public Quaternion(ref Matrix3x2 matrix)
         {
-            var scale = Math.Pow(matrix.Determinant, 1.0d / 3.0d);
+            var scale = Math.Pow(matrix.GetDeterminant(), 1.0d / 3.0d);
             float x, y, z;
 
             w = (float) (Math.Sqrt(Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2);