From f57cd9f3543e1f6deda6bd767a79109229a76ece Mon Sep 17 00:00:00 2001 From: ikpil Date: Sun, 29 Jun 2025 12:34:55 +0900 Subject: [PATCH] [upstream] Remove cancellation from inertia calculation to improve accuracy (box2d #955) I was computing the shape inertia about the body origin and then shifting to the center of mass. But this involves quadratic math with bad cancellation effects. I'm now buffering the shape inertias about their centroids and then shifting them to the final body center of mass. This removes the quadratic cancellation. Fixes (box2d #949) --- .../Samples/Issues/BadSteiner.cs | 59 +++++++++++++++++++ src/Box2D.NET/B2Bodies.cs | 34 +++++++++-- src/Box2D.NET/B2Geometries.cs | 14 ++--- src/Box2D.NET/B2MassData.cs | 2 +- test/Box2D.NET.Test/B2ShapeTest.cs | 2 +- 5 files changed, 97 insertions(+), 14 deletions(-) create mode 100644 src/Box2D.NET.Samples/Samples/Issues/BadSteiner.cs diff --git a/src/Box2D.NET.Samples/Samples/Issues/BadSteiner.cs b/src/Box2D.NET.Samples/Samples/Issues/BadSteiner.cs new file mode 100644 index 00000000..33df186b --- /dev/null +++ b/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); + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index 447fc847..d3202bc0 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/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,9 +584,13 @@ public static void b2UpdateBodyMassData(B2World world, B2Body body) return; } + int shapeCount = body.shapeCount; + ArraySegment masses = b2AllocateArenaItem(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); @@ -594,13 +598,16 @@ public static void b2UpdateBodyMassData(B2World world, B2Body body) 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 diff --git a/src/Box2D.NET/B2Geometries.cs b/src/Box2D.NET/B2Geometries.cs index 3e6bd4f7..bb41c889 100644 --- a/src/Box2D.NET/B2Geometries.cs +++ b/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; } diff --git a/src/Box2D.NET/B2MassData.cs b/src/Box2D.NET/B2MassData.cs index fbb0b1a4..3e4ce0ea 100644 --- a/src/Box2D.NET/B2MassData.cs +++ b/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) diff --git a/test/Box2D.NET.Test/B2ShapeTest.cs b/test/Box2D.NET.Test/B2ShapeTest.cs index 770c2e50..90bc3f0a 100644 --- a/test/Box2D.NET.Test/B2ShapeTest.cs +++ b/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)); } {