From 015bf3318bdbb54cab6d8591138c850869d51a68 Mon Sep 17 00:00:00 2001
From: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com>
Date: Mon, 10 Nov 2025 18:30:24 +1100
Subject: [PATCH] Generic GetMassData (#6291)

* Generic GetMassData

Nothing fancy. Also made the class version use the struct.

* Slim method

* Fix test
---
 Robust.Shared/Physics/Shapes/Polygon.cs       | 20 ++++++++++++++++
 .../Physics/Systems/FixtureSystem.Shapes.cs   | 23 +++++++++++--------
 2 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/Robust.Shared/Physics/Shapes/Polygon.cs b/Robust.Shared/Physics/Shapes/Polygon.cs
index df5b51b51..5961d482d 100644
--- a/Robust.Shared/Physics/Shapes/Polygon.cs
+++ b/Robust.Shared/Physics/Shapes/Polygon.cs
@@ -41,6 +41,8 @@ internal record struct Polygon : IPhysShape
 
     }
 
+    public Polygon(PhysShapeAabb aabb) : this(aabb.LocalBounds) {}
+
     public Polygon(PolygonShape polyShape)
     {
         Unsafe.SkipInit(out this);
@@ -52,6 +54,24 @@ internal record struct Polygon : IPhysShape
         polyShape.Normals.AsSpan()[..VertexCount].CopyTo(_normals.AsSpan);
     }
 
+    internal Polygon(SlimPolygon slim)
+    {
+        Unsafe.SkipInit(out this);
+        Radius = slim.Radius;
+        VertexCount = slim.VertexCount;
+
+        _vertices._00 = slim._vertices._00;
+        _vertices._01 = slim._vertices._01;
+        _vertices._02 = slim._vertices._02;
+        _vertices._03 = slim._vertices._03;
+
+        _normals._00 = slim._normals._00;
+        _normals._01 = slim._normals._01;
+        _normals._02 = slim._normals._02;
+        _normals._03 = slim._normals._03;
+        Centroid = slim.Centroid;
+    }
+
     public Polygon(Box2 box)
     {
         Unsafe.SkipInit(out this);
diff --git a/Robust.Shared/Physics/Systems/FixtureSystem.Shapes.cs b/Robust.Shared/Physics/Systems/FixtureSystem.Shapes.cs
index 277cfaf5b..1a02f76be 100644
--- a/Robust.Shared/Physics/Systems/FixtureSystem.Shapes.cs
+++ b/Robust.Shared/Physics/Systems/FixtureSystem.Shapes.cs
@@ -73,7 +73,7 @@ namespace Robust.Shared.Physics.Systems
             }
         }
 
-        public static MassData GetMassData(IPhysShape shape, float density)
+        public static MassData GetMassData<T>(T shape, float density) where T : IPhysShape
         {
             var data = new MassData();
 
@@ -82,7 +82,7 @@ namespace Robust.Shared.Physics.Systems
             return data;
         }
 
-        public static void GetMassData(IPhysShape shape, ref MassData data, float density)
+        public static void GetMassData<T>(T shape, ref MassData data, float density) where T : IPhysShape
         {
             // Box2D just calls fixture.GetMassData which just calls the shape method anyway soooo
             // we can just cut out the middle-man
@@ -106,16 +106,17 @@ namespace Robust.Shared.Physics.Systems
                     data.I = data.Mass * (0.5f * circle.Radius * circle.Radius + Vector2.Dot(circle.Position, circle.Position));
                     break;
                 case PhysShapeAabb aabb:
-                    var polygon = (PolygonShape) aabb;
+                    var polygon = new Polygon(aabb);
                     GetMassData(polygon, ref data, density);
                     break;
-                case Polygon fastPoly:
-                    GetMassData(new PolygonShape(fastPoly), ref data, density);
+                case PolygonShape fatPoly:
+                    GetMassData(new Polygon(fatPoly), ref data, density);
                     break;
                 case SlimPolygon slim:
-                    GetMassData(new PolygonShape(slim), ref data, density);
+                    var slimPoly = new Polygon(slim);
+                    GetMassData(slimPoly, ref data, density);
                     break;
-                case PolygonShape poly:
+                case Polygon poly:
                     // Polygon mass, centroid, and inertia.
                     // Let rho be the polygon density in mass per unit area.
                     // Then:
@@ -142,6 +143,7 @@ namespace Robust.Shared.Physics.Systems
 
                     var count = poly.VertexCount;
                     DebugTools.Assert(count >= 3);
+                    DebugTools.Assert(poly._normals._00 != Vector2.Zero);
 
                     Vector2 center = new(0.0f, 0.0f);
                     float area = 0.0f;
@@ -149,15 +151,16 @@ namespace Robust.Shared.Physics.Systems
 
                     // Get a reference point for forming triangles.
                     // Use the first vertex to reduce round-off errors.
-                    var s = poly.Vertices[0];
+                    var s = poly._vertices._00;
+                    var polySpan = poly._vertices.AsSpan;
 
                     const float k_inv3 = 1.0f / 3.0f;
 
                     for (var i = 0; i < count; ++i)
                     {
 	                    // Triangle vertices.
-	                    var e1 = poly.Vertices[i] - s;
-	                    var e2 = i + 1 < count ? poly.Vertices[i+1] - s : poly.Vertices[0] - s;
+	                    var e1 = polySpan[i] - s;
+	                    var e2 = i + 1 < count ? polySpan[i+1] - s : polySpan[0] - s;
 
 	                    float D = Vector2Helpers.Cross(e1, e2);