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)); } {