[upstream] Remove cancellation from inertia calculation to improve ac…

src/Box2D.NET.Samples/Samples/Issues/BadSteiner.cs

@@ -0,0 +1,59 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com)
+// SPDX-License-Identifier: MIT
+
+using static Box2D.NET.B2Geometries;
+using static Box2D.NET.B2Types;
+using static Box2D.NET.B2Bodies;
+using static Box2D.NET.B2Shapes;
+using static Box2D.NET.B2Hulls;
+
+namespace Box2D.NET.Samples.Samples.Issues;
+
+public class BadSteiner : Sample
+{
+    private static readonly int SampleBadSteiner = SampleFactory.Shared.RegisterSample("Issues", "Bad Steiner", Create);
+
+    private static Sample Create(SampleContext context)
+    {
+        return new BadSteiner(context);
+    }
+
+    public BadSteiner(SampleContext context) : base(context)
+    {
+        if (m_context.settings.restart == false)
+        {
+            m_context.camera.m_center = new B2Vec2(0.0f, 1.75f);
+            m_context.camera.m_zoom = 2.5f;
+        }
+
+        {
+            B2BodyDef bodyDef = b2DefaultBodyDef();
+            B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef);
+
+            B2ShapeDef shapeDef = b2DefaultShapeDef();
+            B2Segment segment = new B2Segment(new B2Vec2(-100.0f, 0.0f), new B2Vec2(100.0f, 0.0f));
+            b2CreateSegmentShape(groundId, ref shapeDef, ref segment);
+        }
+
+        {
+            B2BodyDef bodyDef = b2DefaultBodyDef();
+            bodyDef.type = B2BodyType.b2_dynamicBody;
+            bodyDef.position = new B2Vec2(-48.0f, 62.0f);
+            B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+
+            B2ShapeDef shapeDef = b2DefaultShapeDef();
+
+            B2Vec2[] points =
+            [
+                new B2Vec2(48.7599983f, -60.5699997f),
+                new B2Vec2(48.7400017f, -60.5400009f),
+                new B2Vec2(48.6800003f, -60.5600014f)
+            ];
+
+            B2Hull hull = b2ComputeHull(points, 3);
+            B2Polygon poly = b2MakePolygon(ref hull, 0.0f);
+            b2CreatePolygonShape(bodyId, ref shapeDef, ref poly);
+        }
+    }
+}

src/Box2D.NET/B2Bodies.cs

@@ -3,7 +3,6 @@
 // SPDX-License-Identifier: MIT
 
 using System;
-using System.Runtime.CompilerServices;
 using static Box2D.NET.B2Arrays;
 using static Box2D.NET.B2Cores;
 using static Box2D.NET.B2Diagnostics;
@@ -19,6 +18,7 @@
 using static Box2D.NET.B2Sensors;
 using static Box2D.NET.B2SolverSets;
 using static Box2D.NET.B2BoardPhases;
+using static Box2D.NET.B2ArenaAllocators;
 
 namespace Box2D.NET
 {
@@ -584,23 +584,30 @@ public static void b2UpdateBodyMassData(B2World world, B2Body body)
                 return;
             }
 
+            int shapeCount = body.shapeCount;
+            ArraySegment<B2MassData> masses = b2AllocateArenaItem<B2MassData>(world.arena, shapeCount, "mass data");
+
             // Accumulate mass over all shapes.
             B2Vec2 localCenter = b2Vec2_zero;
             int shapeId = body.headShapeId;
+            int shapeIndex = 0;
             while (shapeId != B2_NULL_INDEX)
             {
                 B2Shape s = b2Array_Get(ref world.shapes, shapeId);
                 shapeId = s.nextShapeId;
 
                 if (s.density == 0.0f)
                 {
+                    masses[shapeIndex] = new B2MassData();
                     continue;
                 }
 
                 B2MassData massData = b2ComputeShapeMass(s);
                 body.mass += massData.mass;
                 localCenter = b2MulAdd(localCenter, massData.mass, massData.center);
-                body.inertia += massData.rotationalInertia;
+
+                masses[shapeIndex] = massData;
+                shapeIndex += 1;
             }
 
             // Compute center of mass.
@@ -610,11 +617,28 @@ public static void b2UpdateBodyMassData(B2World world, B2Body body)
                 localCenter = b2MulSV(bodySim.invMass, localCenter);
             }
 
+            // Second loop to accumulate the rotational inertia about the center of mass
+            for (shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex)
+            {
+                B2MassData massData = masses[shapeIndex];
+                if (massData.mass == 0.0f)
+                {
+                    continue;
+                }
+
+                // Shift to center of mass. This is safe because it can only increase.
+                B2Vec2 offset = b2Sub(localCenter, massData.center);
+                float inertia = massData.rotationalInertia + massData.mass * b2Dot(offset, offset);
+                body.inertia += inertia;
+            }
+
+            b2FreeArenaItem(world.arena, masses);
+            masses = null;
+
+            B2_ASSERT(body.inertia >= 0.0f);
+
             if (body.inertia > 0.0f)
             {
-                // Center the inertia about the center of mass.
-                body.inertia -= body.mass * b2Dot(localCenter, localCenter);
-                B2_ASSERT(body.inertia > 0.0f);
                 bodySim.invInertia = 1.0f / body.inertia;
             }
             else

src/Box2D.NET/B2Geometries.cs

@@ -257,8 +257,8 @@ public static B2MassData b2ComputeCircleMass(ref B2Circle shape, float density)
             massData.mass = density * B2_PI * rr;
             massData.center = shape.center;
 
-            // inertia about the local origin
-            massData.rotationalInertia = massData.mass * (0.5f * rr + b2Dot(shape.center, shape.center));
+            // inertia about the center of mass
+            massData.rotationalInertia = massData.mass * 0.5f * rr;
 
             return massData;
         }
@@ -300,9 +300,6 @@ public static B2MassData b2ComputeCapsuleMass(ref B2Capsule shape, float density
             float boxInertia = boxMass * (4.0f * rr + ll) / 12.0f;
             massData.rotationalInertia = circleInertia + boxInertia;
 
-            // inertia about the local origin
-            massData.rotationalInertia += massData.mass * b2Dot(massData.center, massData.center);
-
             return massData;
         }
 
@@ -424,8 +421,11 @@ public static B2MassData b2ComputePolygonMass(ref B2Polygon shape, float density
             // Inertia tensor relative to the local origin (point s).
             massData.rotationalInertia = density * rotationalInertia;
 
-            // Shift to center of mass then to original body origin.
-            massData.rotationalInertia += massData.mass * (b2Dot(massData.center, massData.center) - b2Dot(center, center));
+            // Shift inertia to center of mass
+            massData.rotationalInertia -= massData.mass * b2Dot(center, center);
+
+            // If this goes negative we are hosed
+            B2_ASSERT(massData.rotationalInertia >= 0.0f);
 
             return massData;
         }

src/Box2D.NET/B2MassData.cs

@@ -13,7 +13,7 @@ public struct B2MassData
         /// The position of the shape's centroid relative to the shape's origin.
         public B2Vec2 center;
 
-        /// The rotational inertia of the shape about the local origin.
+        /// The rotational inertia of the shape about the shape center.
         public float rotationalInertia;
 
         public B2MassData(float mass, B2Vec2 center, float rotationalInertia)

test/Box2D.NET.Test/B2ShapeTest.cs

@@ -28,7 +28,7 @@ public void ShapeMassTest()
             Assert.That(md.mass - B2_PI, Is.LessThan(FLT_EPSILON));
             Assert.That(md.center.X, Is.EqualTo(1.0f));
             Assert.That(md.center.Y, Is.EqualTo(0.0f));
-            Assert.That(md.rotationalInertia - 1.5f * B2_PI, Is.LessThan(FLT_EPSILON));
+            Assert.That(md.rotationalInertia - 0.5f * B2_PI, Is.LessThan(FLT_EPSILON));
         }
 
         {