diff --git a/src/Box2D.NET.Samples/Draw.cs b/src/Box2D.NET.Samples/Draw.cs index 9aefb64e..24ee8a88 100644 --- a/src/Box2D.NET.Samples/Draw.cs +++ b/src/Box2D.NET.Samples/Draw.cs @@ -59,7 +59,6 @@ public Draw() m_debugDraw.DrawStringFcn = DrawStringFcn; m_debugDraw.drawingBounds = bounds; - m_debugDraw.useDrawingBounds = false; m_debugDraw.drawShapes = true; m_debugDraw.drawJoints = true; m_debugDraw.drawJointExtras = false; diff --git a/src/Box2D.NET.Samples/SampleApp.cs b/src/Box2D.NET.Samples/SampleApp.cs index d02f4c12..0ca289d8 100644 --- a/src/Box2D.NET.Samples/SampleApp.cs +++ b/src/Box2D.NET.Samples/SampleApp.cs @@ -294,7 +294,6 @@ private void OnWindowUpdate(double dt) // #todo restore all drawing settings that may have been overridden by a sample _context.settings.subStepCount = 4; _context.settings.drawJoints = true; - _context.settings.useCameraBounds = true; s_sample?.Dispose(); s_sample = null; diff --git a/src/Box2D.NET.Samples/Samples/Bodies/BodyType.cs b/src/Box2D.NET.Samples/Samples/Bodies/BodyType.cs index 0cd51ab3..3cfa7c2d 100644 --- a/src/Box2D.NET.Samples/Samples/Bodies/BodyType.cs +++ b/src/Box2D.NET.Samples/Samples/Bodies/BodyType.cs @@ -47,6 +47,7 @@ public BodyType(SampleContext context) : base(context) B2BodyId groundId = b2_nullBodyId; { B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.name = "ground"; groundId = b2CreateBody(m_worldId, ref bodyDef); B2Segment segment = new B2Segment(new B2Vec2(-20.0f, 0.0f), new B2Vec2(20.0f, 0.0f)); @@ -59,6 +60,7 @@ public BodyType(SampleContext context) : base(context) B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; bodyDef.position = new B2Vec2(-2.0f, 3.0f); + bodyDef.name = "attach1"; m_attachmentId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeBox(0.5f, 2.0f); @@ -73,6 +75,7 @@ public BodyType(SampleContext context) : base(context) bodyDef.type = m_type; bodyDef.isEnabled = m_isEnabled; bodyDef.position = new B2Vec2(3.0f, 3.0f); + bodyDef.name = "attach2"; m_secondAttachmentId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeBox(0.5f, 2.0f); @@ -87,6 +90,7 @@ public BodyType(SampleContext context) : base(context) bodyDef.type = m_type; bodyDef.isEnabled = m_isEnabled; bodyDef.position = new B2Vec2(-4.0f, 5.0f); + bodyDef.name = "platform"; m_platformId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeOffsetBox(0.5f, 4.0f, new B2Vec2(4.0f, 0.0f), b2MakeRot(0.5f * B2_PI)); @@ -137,6 +141,7 @@ public BodyType(SampleContext context) : base(context) B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; bodyDef.position = new B2Vec2(-3.0f, 8.0f); + bodyDef.name = "crate1"; B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeBox(0.75f, 0.75f); @@ -153,6 +158,7 @@ public BodyType(SampleContext context) : base(context) bodyDef.type = m_type; bodyDef.isEnabled = m_isEnabled; bodyDef.position = new B2Vec2(2.0f, 8.0f); + bodyDef.name = "crate2"; m_secondPayloadId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeBox(0.75f, 0.75f); @@ -169,6 +175,7 @@ public BodyType(SampleContext context) : base(context) bodyDef.type = m_type; bodyDef.isEnabled = m_isEnabled; bodyDef.position = new B2Vec2(8.0f, 0.2f); + bodyDef.name = "debris"; m_touchingBodyId = b2CreateBody(m_worldId, ref bodyDef); B2Capsule capsule = new B2Capsule(new B2Vec2(0.0f, 0.0f), new B2Vec2(1.0f, 0.0f), 0.25f); @@ -186,6 +193,7 @@ public BodyType(SampleContext context) : base(context) bodyDef.isEnabled = m_isEnabled; bodyDef.position = new B2Vec2(-8.0f, 12.0f); bodyDef.gravityScale = 0.0f; + bodyDef.name = "floater"; m_floatingBodyId = b2CreateBody(m_worldId, ref bodyDef); B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.5f), 0.25f); @@ -221,6 +229,9 @@ public override void UpdateGui() { m_type = B2BodyType.b2_kinematicBody; b2Body_SetType(m_platformId, B2BodyType.b2_kinematicBody); + b2Body_SetLinearVelocity(m_secondAttachmentId, b2Vec2_zero); + b2Body_SetAngularVelocity(m_secondAttachmentId, 0.0f); + b2Body_SetLinearVelocity(m_platformId, new B2Vec2(-m_speed, 0.0f)); b2Body_SetAngularVelocity(m_platformId, 0.0f); b2Body_SetType(m_secondAttachmentId, B2BodyType.b2_kinematicBody); @@ -243,24 +254,14 @@ public override void UpdateGui() { if (m_isEnabled) { - b2Body_Enable(m_platformId); - b2Body_Enable(m_secondAttachmentId); + b2Body_Enable(m_attachmentId); b2Body_Enable(m_secondPayloadId); - b2Body_Enable(m_touchingBodyId); b2Body_Enable(m_floatingBodyId); - - if (m_type == B2BodyType.b2_kinematicBody) - { - b2Body_SetLinearVelocity(m_platformId, new B2Vec2(-m_speed, 0.0f)); - b2Body_SetAngularVelocity(m_platformId, 0.0f); - } } else { - b2Body_Disable(m_platformId); - b2Body_Disable(m_secondAttachmentId); + b2Body_Disable(m_attachmentId); b2Body_Disable(m_secondPayloadId); - b2Body_Disable(m_touchingBodyId); b2Body_Disable(m_floatingBodyId); } } diff --git a/src/Box2D.NET.Samples/Samples/Continuous/BounceHouse.cs b/src/Box2D.NET.Samples/Samples/Continuous/BounceHouse.cs index bce03518..eeb9bfbd 100644 --- a/src/Box2D.NET.Samples/Samples/Continuous/BounceHouse.cs +++ b/src/Box2D.NET.Samples/Samples/Continuous/BounceHouse.cs @@ -68,7 +68,7 @@ public BounceHouse(SampleContext context) : base(context) b2CreateSegmentShape(groundId, ref shapeDef, ref segment); } - m_shapeType = ShapeType.e_boxShape; + m_shapeType = ShapeType.e_circleShape; m_bodyId = b2_nullBodyId; m_enableHitEvents = true; @@ -93,6 +93,7 @@ void Launch() bodyDef.linearVelocity = new B2Vec2(10.0f, 20.0f); bodyDef.position = new B2Vec2(0.0f, 0.0f); bodyDef.gravityScale = 0.0f; + bodyDef.isBullet = true; // Circle shapes centered on the body can spin fast without risk of tunnelling. bodyDef.allowFastRotation = m_shapeType == ShapeType.e_circleShape; @@ -101,8 +102,8 @@ void Launch() B2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.density = 1.0f; - shapeDef.material.restitution = 1.2f; - shapeDef.material.friction = 0.3f; + shapeDef.material.restitution = 1.0f; + shapeDef.material.friction = 0.0f; shapeDef.enableHitEvents = m_enableHitEvents; if (m_shapeType == ShapeType.e_circleShape) diff --git a/src/Box2D.NET.Samples/Samples/Issues/Crash01.cs b/src/Box2D.NET.Samples/Samples/Issues/Crash01.cs new file mode 100644 index 00000000..ebd06c4d --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Issues/Crash01.cs @@ -0,0 +1,150 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System.Numerics; +using ImGuiNET; +using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.B2Ids; + +namespace Box2D.NET.Samples.Samples.Issues; + +public class Crash01 : Sample +{ + private static readonly int SampleBodyType = SampleFactory.Shared.RegisterSample("Issues", "Crash01", Create); + + private B2BodyId m_attachmentId; + private B2BodyId m_platformId; + private B2BodyType m_type; + private bool m_isEnabled; + + private static Sample Create(SampleContext context) + { + return new Crash01(context); + } + + public Crash01(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.8f, 6.4f); + m_context.camera.m_zoom = 25.0f * 0.4f; + } + + m_type = B2BodyType.b2_dynamicBody; + m_isEnabled = true; + + B2BodyId groundId = b2_nullBodyId; + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.name = "ground"; + groundId = b2CreateBody(m_worldId, ref bodyDef); + + B2Segment segment = new B2Segment(new B2Vec2(-20.0f, 0.0f), new B2Vec2(20.0f, 0.0f)); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + } + + // Define attachment + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(-2.0f, 3.0f); + bodyDef.name = "attach1"; + m_attachmentId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeBox(0.5f, 2.0f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape(m_attachmentId, ref shapeDef, ref box); + } + + // Define platform + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = new B2Vec2(-4.0f, 5.0f); + bodyDef.name = "platform"; + m_platformId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeOffsetBox(0.5f, 4.0f, new B2Vec2(4.0f, 0.0f), b2MakeRot(0.5f * B2_PI)); + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 2.0f; + b2CreatePolygonShape(m_platformId, ref shapeDef, ref box); + + B2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + B2Vec2 pivot = new B2Vec2(-2.0f, 5.0f); + revoluteDef.@base.bodyIdA = m_attachmentId; + revoluteDef.@base.bodyIdB = m_platformId; + revoluteDef.@base.localFrameA.p = b2Body_GetLocalPoint(m_attachmentId, pivot); + revoluteDef.@base.localFrameB.p = b2Body_GetLocalPoint(m_platformId, pivot); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b2CreateRevoluteJoint(m_worldId, ref revoluteDef); + + B2PrismaticJointDef prismaticDef = b2DefaultPrismaticJointDef(); + B2Vec2 anchor = new B2Vec2(0.0f, 5.0f); + prismaticDef.@base.bodyIdA = groundId; + prismaticDef.@base.bodyIdB = m_platformId; + prismaticDef.@base.localFrameA.p = b2Body_GetLocalPoint(groundId, anchor); + prismaticDef.@base.localFrameB.p = b2Body_GetLocalPoint(m_platformId, anchor); + prismaticDef.maxMotorForce = 1000.0f; + prismaticDef.motorSpeed = 0.0f; + prismaticDef.enableMotor = true; + prismaticDef.lowerTranslation = -10.0f; + prismaticDef.upperTranslation = 10.0f; + prismaticDef.enableLimit = true; + + b2CreatePrismaticJoint(m_worldId, ref prismaticDef); + } + } + + public override void UpdateGui() + { + float fontSize = ImGui.GetFontSize(); + float height = 11.0f * fontSize; + ImGui.SetNextWindowPos(new Vector2(0.5f * fontSize, m_camera.m_height - height - 2.0f * fontSize), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(9.0f * fontSize, height)); + ImGui.Begin("Crash 01", ImGuiWindowFlags.NoMove | ImGuiWindowFlags.NoResize); + + if (ImGui.RadioButton("Static", m_type == B2BodyType.b2_staticBody)) + { + m_type = B2BodyType.b2_staticBody; + b2Body_SetType(m_platformId, B2BodyType.b2_staticBody); + } + + if (ImGui.RadioButton("Kinematic", m_type == B2BodyType.b2_kinematicBody)) + { + m_type = B2BodyType.b2_kinematicBody; + b2Body_SetType(m_platformId, B2BodyType.b2_kinematicBody); + b2Body_SetLinearVelocity(m_platformId, new B2Vec2(-0.1f, 0.0f)); + } + + if (ImGui.RadioButton("Dynamic", m_type == B2BodyType.b2_dynamicBody)) + { + m_type = B2BodyType.b2_dynamicBody; + b2Body_SetType(m_platformId, B2BodyType.b2_dynamicBody); + } + + if (ImGui.Checkbox("Enable", ref m_isEnabled)) + { + if (m_isEnabled) + { + b2Body_Enable(m_attachmentId); + } + else + { + b2Body_Disable(m_attachmentId); + } + } + + ImGui.End(); + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Issues/DisableCrash.cs b/src/Box2D.NET.Samples/Samples/Issues/DisableCrash.cs new file mode 100644 index 00000000..6c41528b --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Issues/DisableCrash.cs @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System.Numerics; +using ImGuiNET; +using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; + +namespace Box2D.NET.Samples.Samples.Issues; + +public class DisableCrash : Sample +{ + private static readonly int SampleDisableCrash = SampleFactory.Shared.RegisterSample("Issues", "Disable", Create); + + private B2BodyId m_attachmentId; + private B2BodyId m_platformId; + bool m_isEnabled; + + private static Sample Create(SampleContext context) + { + return new DisableCrash(context); + } + + public DisableCrash(SampleContext context) + : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.8f, 6.4f); + m_context.camera.m_zoom = 25.0f * 0.4f; + } + + m_isEnabled = true; + + // Define attachment + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(-2.0f, 3.0f); + bodyDef.isEnabled = m_isEnabled; + m_attachmentId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeBox(0.5f, 2.0f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(m_attachmentId, ref shapeDef, ref box); + } + + // Define platform + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = new B2Vec2(-4.0f, 5.0f); + m_platformId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeOffsetBox(0.5f, 4.0f, new B2Vec2(4.0f, 0.0f), b2MakeRot(0.5f * B2_PI)); + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(m_platformId, ref shapeDef, ref box); + + B2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + B2Vec2 pivot = new B2Vec2(-2.0f, 5.0f); + revoluteDef.@base.bodyIdA = m_attachmentId; + revoluteDef.@base.bodyIdB = m_platformId; + revoluteDef.@base.localFrameA.p = b2Body_GetLocalPoint(m_attachmentId, pivot); + revoluteDef.@base.localFrameB.p = b2Body_GetLocalPoint(m_platformId, pivot); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b2CreateRevoluteJoint(m_worldId, ref revoluteDef); + } + } + + public override void UpdateGui() + { + float fontSize = ImGui.GetFontSize(); + float height = 11.0f * fontSize; + float winX = 0.5f * fontSize; + float winY = m_camera.m_height - height - 2.0f * fontSize; + ImGui.SetNextWindowPos(new Vector2(winX, winY), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(9.0f * fontSize, height)); + ImGui.Begin("Disable Crash", ImGuiWindowFlags.NoMove | ImGuiWindowFlags.NoResize); + + if (ImGui.Checkbox("Enable", ref m_isEnabled)) + { + if (m_isEnabled) + { + b2Body_Enable(m_attachmentId); + } + else + { + b2Body_Disable(m_attachmentId); + } + } + + ImGui.End(); + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Sample.cs b/src/Box2D.NET.Samples/Samples/Sample.cs index 2cbfbe68..e6473d45 100644 --- a/src/Box2D.NET.Samples/Samples/Sample.cs +++ b/src/Box2D.NET.Samples/Samples/Sample.cs @@ -466,7 +466,6 @@ public virtual void Draw(Settings settings) } m_context.draw.m_debugDraw.drawingBounds = m_camera.GetViewBounds(); - m_context.draw.m_debugDraw.useDrawingBounds = settings.useCameraBounds; m_context.draw.m_debugDraw.drawShapes = settings.drawShapes; m_context.draw.m_debugDraw.drawJoints = settings.drawJoints; m_context.draw.m_debugDraw.drawJointExtras = settings.drawJointExtras; diff --git a/src/Box2D.NET.Samples/Samples/Worlds/LargeWorld.cs b/src/Box2D.NET.Samples/Samples/Worlds/LargeWorld.cs index 56492a00..dce2fe5d 100644 --- a/src/Box2D.NET.Samples/Samples/Worlds/LargeWorld.cs +++ b/src/Box2D.NET.Samples/Samples/Worlds/LargeWorld.cs @@ -58,7 +58,6 @@ public LargeWorld(SampleContext context) : base(context) m_camera.m_center = m_viewPosition; m_camera.m_zoom = 25.0f * 1.0f; m_context.settings.drawJoints = false; - m_context.settings.useCameraBounds = true; } { diff --git a/src/Box2D.NET.Samples/Settings.cs b/src/Box2D.NET.Samples/Settings.cs index 250d2377..0869153d 100644 --- a/src/Box2D.NET.Samples/Settings.cs +++ b/src/Box2D.NET.Samples/Settings.cs @@ -23,7 +23,6 @@ public class Settings public int subStepCount = 4; public int workerCount = 1; - public bool useCameraBounds = false; public bool drawShapes = true; public bool drawJoints = true; public bool drawJointExtras = false; @@ -73,7 +72,6 @@ public void CopyFrom(Settings other) subStepCount = other.subStepCount; workerCount = other.workerCount; - useCameraBounds = other.useCameraBounds; drawShapes = other.drawShapes; drawJoints = other.drawJoints; drawJointExtras = other.drawJointExtras; diff --git a/src/Box2D.NET.Shared/Benchmarks.cs b/src/Box2D.NET.Shared/Benchmarks.cs index e1e4312b..c3f5e913 100644 --- a/src/Box2D.NET.Shared/Benchmarks.cs +++ b/src/Box2D.NET.Shared/Benchmarks.cs @@ -44,6 +44,8 @@ public static void CreateJointGrid(B2WorldId worldId) B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.0f), 0.4f); B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.@base.drawScale = 0.4f; + B2BodyDef bodyDef = b2DefaultBodyDef(); for (int k = 0; k < N; ++k) @@ -360,7 +362,8 @@ public static SpinnerData CreateSpinner(B2WorldId worldId) b2CreatePolygonShape(spinnerId, ref shapeDef, ref box); float motorSpeed = 5.0f; - float maxMotorTorque = 40000.0f; + //float maxMotorTorque = 100.0f * 40000.0f; + float maxMotorTorque = float.MaxValue; B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); jointDef.@base.bodyIdA = groundId; jointDef.@base.bodyIdB = spinnerId; @@ -384,9 +387,9 @@ public static SpinnerData CreateSpinner(B2WorldId worldId) shapeDef.material.restitution = 0.1f; shapeDef.density = 0.25f; - int bodyCount = BENCHMARK_DEBUG ? 499 : 3038; + int bodyCount = BENCHMARK_DEBUG ? 499 : 2 * 3038; - float x = -24.0f, y = 2.0f; + float x = -23.0f, y = 2.0f; for (int i = 0; i < bodyCount; ++i) { bodyDef.position = new B2Vec2(x, y); @@ -406,12 +409,12 @@ public static SpinnerData CreateSpinner(B2WorldId worldId) b2CreatePolygonShape(bodyId, ref shapeDef, ref square); } - x += 1.0f; + x += 0.5f; - if (x > 24.0f) + if (x >= 23.0f) { - x = -24.0f; - y += 1.0f; + x = -23.0f; + y += 0.5f; } } } diff --git a/src/Box2D.NET.Shared/Determinism.cs b/src/Box2D.NET.Shared/Determinism.cs index d6a42940..df4e73ff 100644 --- a/src/Box2D.NET.Shared/Determinism.cs +++ b/src/Box2D.NET.Shared/Determinism.cs @@ -58,7 +58,7 @@ public static FallingHingeData CreateFallingHinges(B2WorldId worldId) jointDef.dampingRatio = 0.5f; jointDef.@base.localFrameA.p = new B2Vec2(h, h); jointDef.@base.localFrameB.p = new B2Vec2(offset, -h); - jointDef.@base.drawScale = 0.1f; + jointDef.@base.drawScale = 0.5f; int bodyIndex = 0; diff --git a/src/Box2D.NET/B2BitSets.cs b/src/Box2D.NET/B2BitSets.cs index d2824503..afbb85f7 100644 --- a/src/Box2D.NET/B2BitSets.cs +++ b/src/Box2D.NET/B2BitSets.cs @@ -6,6 +6,7 @@ using System.Runtime.CompilerServices; using static Box2D.NET.B2Diagnostics; using static Box2D.NET.B2Buffers; +using static Box2D.NET.B2CTZs; namespace Box2D.NET { @@ -125,6 +126,20 @@ public static void b2GrowBitSet(ref B2BitSet bitSet, int blockCount) bitSet.blockCount = blockCount; } + // This function is here because ctz.h is included by + // this file but not in bitset.c + public static int b2CountSetBits(ref B2BitSet set) + { + int popCount = 0; + int blockCount = set.blockCount; + for (uint i = 0; i < blockCount; ++i) + { + popCount += b2PopCount64(set.bits[i]); + } + + return popCount; + } + public static void b2InPlaceUnion(ref B2BitSet setA, ref B2BitSet setB) { if (setA.blockCount != setB.blockCount) diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index d3202bc0..dd96f587 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/src/Box2D.NET/B2Bodies.cs @@ -332,7 +332,6 @@ public static B2BodyId b2CreateBody(B2WorldId worldId, ref B2BodyDef def) body.type = def.type; body.flags = lockFlags; body.enableSleep = def.enableSleep; - body.isSpeedCapped = false; body.isMarked = false; // dynamic and kinematic bodies that are enabled need a island @@ -605,7 +604,7 @@ public static void b2UpdateBodyMassData(B2World world, B2Body body) B2MassData massData = b2ComputeShapeMass(s); body.mass += massData.mass; localCenter = b2MulAdd(localCenter, massData.mass, massData.center); - + masses[shapeIndex] = massData; shapeIndex += 1; } @@ -1136,13 +1135,202 @@ public static B2BodyType b2Body_GetType(B2BodyId bodyId) return body.type; } - // Changing the body type is quite complex mainly due to joints. - // Considerations: - // - body and joints must be moved to the correct set - // - islands must be updated - // - graph coloring must be correct - // - any body connected to a joint may be disabled - // - joints between static bodies must go into the static set + // This should follow similar steps as you would get destroying and recreating the body, shapes, and joints. + // Contacts are difficult to preserve because the broad-phase pairs change, so I just destroy them. + // todo with a bit more effort I could support an option to let the body sleep + // + // Revised steps: + // 1 Skip disabled bodies + // 2 Destroy all contacts on the body + // 3 Wake the body + // 4 For all joints attached to the body + // - wake attached bodies + // - remove from island + // - move to static set temporarily + // 5 Change the body type and transfer the body + // 6 If the body was static + // - create an island for the body + // Else if the body is becoming static + // - remove it from the island + // 7 For all joints + // - if either body is non-static + // - link into island + // - transfer to constraint graph + // 8 For all shapes + // - Destroy proxy in old tree + // - Create proxy in new tree + // Notes: + // - the implementation below tries to minimize the number of predicates, so some + // operations may have no effect, such as transfering a joint to the same set +#if ENABLED + public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) + { + B2World world = b2GetWorld(bodyId.world0); + B2Body body = b2GetBodyFullId(world, bodyId); + + B2BodyType originalType = body.type; + if (originalType == type) + { + return; + } + + // Stage 1: skip disabled bodies + if (body.setIndex == (int)B2SetType.b2_disabledSet) + { + // Disabled bodies don't change solver sets or islands when they change type. + body.type = type; + + // Body type affects the mass properties + b2UpdateBodyMassData(world, body); + return; + } + + // Stage 2: destroy all contacts but don't wake bodies (because we don't need to) + bool wakeBodies = false; + b2DestroyBodyContacts(world, body, wakeBodies); + + // Stage 3: wake this body (does nothing if body is static), otherwise it will also wake + // all bodies in the same sleeping solver set. + b2WakeBody(world, body); + + // Stage 4: move joints to temporary storage + B2SolverSet staticSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_staticSet); + B2SolverSet awakeSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_awakeSet); + + int jointKey = body.headJointKey; + while (jointKey != B2_NULL_INDEX) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + B2Joint joint = b2Array_Get(ref world.joints, jointId); + jointKey = joint.edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if (joint.setIndex == (int)B2SetType.b2_disabledSet) + { + continue; + } + + // Wake attached bodies. The b2WakeBody call above does not wake bodies + // attached to a static body. But it is necessary because the body may have + // no joints. + B2Body bodyA = b2Array_Get(ref world.bodies, joint.edges[0].bodyId); + B2Body bodyB = b2Array_Get(ref world.bodies, joint.edges[1].bodyId); + b2WakeBody(world, bodyA); + b2WakeBody(world, bodyB); + + // Remove joint from island + b2UnlinkJoint(world, joint); + + // It is necessary to transfer all joints to the static set + // so they can be added to the constraint graph below and acquire consistent colors. + B2SolverSet jointSourceSet = b2Array_Get(ref world.solverSets, joint.setIndex); + b2TransferJoint(world, staticSet, jointSourceSet, joint); + } + + // Stage 5: change the body type and transfer body + body.type = type; + + B2SolverSet sourceSet = b2Array_Get(ref world.solverSets, body.setIndex); + B2SolverSet targetSet = type == B2BodyType.b2_staticBody ? staticSet : awakeSet; + + // Transfer body + b2TransferBody(world, targetSet, sourceSet, body); + + // Stage 6: update island participation for the body + if (originalType == B2BodyType.b2_staticBody) + { + // Create island for body + b2CreateIslandForBody(world, (int)B2SetType.b2_awakeSet, body); + } + else if (type == B2BodyType.b2_staticBody) + { + // Remove body from island. + b2RemoveBodyFromIsland(world, body); + } + + // Stage 7: Transfer joints to the target set + jointKey = body.headJointKey; + while (jointKey != B2_NULL_INDEX) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + B2Joint joint = b2Array_Get(ref world.joints, jointId); + + jointKey = joint.edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if (joint.setIndex == (int)B2SetType.b2_disabledSet) + { + continue; + } + + // All joints were transfered to the static set in an earlier stage + B2_ASSERT(joint.setIndex == (int)B2SetType.b2_staticSet); + + B2Body bodyA = b2Array_Get(ref world.bodies, joint.edges[0].bodyId); + B2Body bodyB = b2Array_Get(ref world.bodies, joint.edges[1].bodyId); + B2_ASSERT(bodyA.setIndex == (int)B2SetType.b2_staticSet || bodyA.setIndex == (int)B2SetType.b2_awakeSet); + B2_ASSERT(bodyB.setIndex == (int)B2SetType.b2_staticSet || bodyB.setIndex == (int)B2SetType.b2_awakeSet); + + if (bodyA.setIndex == (int)B2SetType.b2_awakeSet || bodyB.setIndex == (int)B2SetType.b2_awakeSet) + { + b2TransferJoint(world, awakeSet, staticSet, joint); + } + } + + // Recreate shape proxies in broadphase + B2Transform transform = b2GetBodyTransformQuick(world, body); + int shapeId = body.headShapeId; + while (shapeId != B2_NULL_INDEX) + { + B2Shape shape = b2Array_Get(ref world.shapes, shapeId); + shapeId = shape.nextShapeId; + b2DestroyShapeProxy(shape, world.broadPhase); + bool forcePairCreation = true; + b2CreateShapeProxy(shape, world.broadPhase, type, transform, forcePairCreation); + } + + // Relink all joints + jointKey = body.headJointKey; + while (jointKey != B2_NULL_INDEX) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + B2Joint joint = b2Array_Get(ref world.joints, jointId); + jointKey = joint.edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint.edges[otherEdgeIndex].bodyId; + B2Body otherBody = b2Array_Get(ref world.bodies, otherBodyId); + + if (otherBody.setIndex == (int)B2SetType.b2_disabledSet) + { + continue; + } + + if (body.type == B2BodyType.b2_staticBody && otherBody.type == B2BodyType.b2_staticBody) + { + continue; + } + + bool mergeIslands = false; + b2LinkJoint(world, joint, mergeIslands); + } + + b2MergeAwakeIslands(world); + + // Body type affects the mass + b2UpdateBodyMassData(world, body); + + b2ValidateSolverSets(world); + b2ValidateIsland(world, body.islandId); + } +#else + // todo keeping this buggy old version for reference public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) { B2World world = b2GetWorld(bodyId.world0); @@ -1232,6 +1420,13 @@ public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) // In this case the joint must be re-inserted into the constraint graph to ensure the correct // graph color. + // BUG BUG BUG + // This has a subtle bug where the joint transfer below can clear a color occupied by another + // joint attached to this body. + // This can happen when a body was previously static. + // For this reason, all joints must first be moved to the static set before being added to + // the constraint graph + // First transfer to the static set. b2TransferJoint(world, staticSet, awakeSet, joint); @@ -1392,6 +1587,7 @@ public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) b2ValidateSolverSets(world); } +#endif /// Set the body name. Up to 31 characters excluding 0 termination. public static void b2Body_SetName(B2BodyId bodyId, string name) @@ -1668,7 +1864,7 @@ public static void b2Body_EnableSleep(B2BodyId bodyId, bool enableSleep) } // Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. - // The most challenging aspect that joints may connect to bodies that are not disabled. + // The most challenging aspect is that joints may connect to bodies that are not disabled. public static void b2Body_Disable(B2BodyId bodyId) { B2World world = b2GetWorldLocked(bodyId.world0); @@ -1688,26 +1884,13 @@ public static void b2Body_Disable(B2BodyId bodyId) bool wakeBodies = true; b2DestroyBodyContacts(world, body, wakeBodies); - // Disabled bodies are not in an island. - b2RemoveBodyFromIsland(world, body); - - // Remove shapes from broad-phase - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - shapeId = shape.nextShapeId; - b2DestroyShapeProxy(shape, world.broadPhase); - } - - // Transfer simulation data to disabled set + // The current solver set of the body B2SolverSet set = b2Array_Get(ref world.solverSets, body.setIndex); - B2SolverSet disabledSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_disabledSet); - // Transfer body sim - b2TransferBody(world, disabledSet, set, body); + // Disabled bodies and connected joints are moved to the disabled set + B2SolverSet disabledSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_disabledSet); - // Unlink joints and transfer + // Unlink joints and transfer them to the disabled set int jointKey = body.headJointKey; while (jointKey != B2_NULL_INDEX) { @@ -1726,16 +1909,28 @@ public static void b2Body_Disable(B2BodyId bodyId) B2_ASSERT(joint.setIndex == set.setIndex || set.setIndex == (int)B2SetType.b2_staticSet); // Remove joint from island - if (joint.islandId != B2_NULL_INDEX) - { - b2UnlinkJoint(world, joint); - } + b2UnlinkJoint(world, joint); // Transfer joint to disabled set B2SolverSet jointSet = b2Array_Get(ref world.solverSets, joint.setIndex); b2TransferJoint(world, disabledSet, jointSet, joint); } + // Remove shapes from broad-phase + int shapeId = body.headShapeId; + while (shapeId != B2_NULL_INDEX) + { + B2Shape shape = b2Array_Get(ref world.shapes, shapeId); + shapeId = shape.nextShapeId; + b2DestroyShapeProxy(shape, world.broadPhase); + } + + // Disabled bodies are not in an island. If the island becomes empty it will be destroyed. + b2RemoveBodyFromIsland(world, body); + + // Transfer body sim + b2TransferBody(world, disabledSet, set, body); + b2ValidateConnectivity(world); b2ValidateSolverSets(world); } diff --git a/src/Box2D.NET/B2Body.cs b/src/Box2D.NET/B2Body.cs index 7086499c..ab4bb4a9 100644 --- a/src/Box2D.NET/B2Body.cs +++ b/src/Box2D.NET/B2Body.cs @@ -64,7 +64,6 @@ public class B2Body // todo move into flags public bool enableSleep; - public bool isSpeedCapped; public bool isMarked; } } diff --git a/src/Box2D.NET/B2BodyFlags.cs b/src/Box2D.NET/B2BodyFlags.cs index 8626305e..81b0ab97 100644 --- a/src/Box2D.NET/B2BodyFlags.cs +++ b/src/Box2D.NET/B2BodyFlags.cs @@ -20,14 +20,17 @@ public enum B2BodyFlags // This dynamic body does a final CCD pass against all body types, but not other bullets b2_isBullet = 0x00000010, - // This body has hit the maximum linear or angular velocity + // This body was speed capped in the current time step b2_isSpeedCapped = 0x00000020, + + // This body had a time of impact event in the current time step + b2_hadTimeOfImpact = 0x00000040, // This body has no limit on angular velocity - b2_allowFastRotation = 0x00000040, + b2_allowFastRotation = 0x00000080, // This body need's to have its AABB increased - b2_enlargeBounds = 0x00000080, + b2_enlargeBounds = 0x00000100, // All lock flags b2_allLocks = b2_lockAngularZ | b2_lockLinearX | b2_lockLinearY, diff --git a/src/Box2D.NET/B2CTZs.cs b/src/Box2D.NET/B2CTZs.cs index 21f9ebf9..25c1c483 100644 --- a/src/Box2D.NET/B2CTZs.cs +++ b/src/Box2D.NET/B2CTZs.cs @@ -2,6 +2,7 @@ // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT +using System.Numerics; using System.Runtime.CompilerServices; namespace Box2D.NET @@ -19,6 +20,7 @@ public static uint b2CTZ32(uint block) count++; block >>= 1; } + return count; } @@ -34,6 +36,7 @@ public static uint b2CLZ32(uint value) count++; mask >>= 1; } + return count; } @@ -48,6 +51,20 @@ public static uint b2CTZ64(ulong block) count++; block >>= 1; } + + return count; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int b2PopCount64(ulong block) + { + int count = 0; + while (block != 0) + { + count += (int)(block & 1); + block >>= 1; + } + return count; } @@ -79,4 +96,4 @@ public static int b2RoundUpPowerOf2(int x) return 1 << (32 - (int)b2CLZ32((uint)x - 1)); } } -} +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Constants.cs b/src/Box2D.NET/B2Constants.cs index 1ba48b45..a735b160 100644 --- a/src/Box2D.NET/B2Constants.cs +++ b/src/Box2D.NET/B2Constants.cs @@ -17,7 +17,7 @@ public static class B2Constants // Maximum number of colors in the constraint graph. Constraints that cannot // find a color are added to the overflow set which are solved single-threaded. - public const int B2_GRAPH_COLOR_COUNT = 12; + public const int B2_GRAPH_COLOR_COUNT = 24; // A small length used as a collision and constraint tolerance. Usually it is // chosen to be numerically significant, but visually insignificant. In meters. diff --git a/src/Box2D.NET/B2ConstraintGraphs.cs b/src/Box2D.NET/B2ConstraintGraphs.cs index 0efb947e..01c91c0d 100644 --- a/src/Box2D.NET/B2ConstraintGraphs.cs +++ b/src/Box2D.NET/B2ConstraintGraphs.cs @@ -30,12 +30,18 @@ public static class B2ConstraintGraphs // This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body // is touching many other bodies. public const int B2_OVERFLOW_INDEX = B2_GRAPH_COLOR_COUNT - 1; + + // This keeps constraints involving two dynamic bodies at a lower solver priority than constraints + // involving a dynamic and static bodies. This reduces tunneling due to push through. + public const int B2_DYNAMIC_COLOR_COUNT = (B2_GRAPH_COLOR_COUNT - 4); + public static void b2CreateGraph(ref B2ConstraintGraph graph, int bodyCapacity) { - B2_ASSERT(B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12"); + //B2_ASSERT(B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12"); B2_ASSERT(B2_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors"); B2_ASSERT(B2_OVERFLOW_INDEX == B2_GRAPH_COLOR_COUNT - 1, "bad over flow index"); + B2_ASSERT(B2_DYNAMIC_COLOR_COUNT >= 2, "need more dynamic colors"); graph = new B2ConstraintGraph(); graph.colors = new B2GraphColor[B2_GRAPH_COLOR_COUNT]; @@ -96,14 +102,15 @@ public static void b2AddContactToGraph(B2World world, B2ContactSim contactSim, B int bodyIdB = contact.edges[1].bodyId; B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); - bool staticA = bodyA.setIndex == (int)B2SetType.b2_staticSet; - bool staticB = bodyB.setIndex == (int)B2SetType.b2_staticSet; + bool staticA = bodyA.type == B2BodyType.b2_staticBody; + bool staticB = bodyB.type == B2BodyType.b2_staticBody; B2_ASSERT(staticA == false || staticB == false); #if B2_FORCE_OVERFLOW if (staticA == false && staticB == false) { - for (int i = 0; i < B2_OVERFLOW_INDEX; ++i) + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for (int i = 0; i < B2_DYNAMIC_COLOR_COUNT; ++i) { ref B2GraphColor color0 = ref graph.colors[i]; if (b2GetBit(ref color0.bodySet, bodyIdA) || b2GetBit(ref color0.bodySet, bodyIdB)) @@ -119,8 +126,8 @@ public static void b2AddContactToGraph(B2World world, B2ContactSim contactSim, B } else if (staticA == false) { - // No static contacts in color 0 - for (int i = 1; i < B2_OVERFLOW_INDEX; ++i) + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for (int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i) { ref B2GraphColor color0 = ref graph.colors[i]; if (b2GetBit(ref color0.bodySet, bodyIdA)) @@ -135,8 +142,8 @@ public static void b2AddContactToGraph(B2World world, B2ContactSim contactSim, B } else if (staticB == false) { - // No static contacts in color 0 - for (int i = 1; i < B2_OVERFLOW_INDEX; ++i) + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for (int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i) { ref B2GraphColor color0 = ref graph.colors[i]; if (b2GetBit(ref color0.bodySet, bodyIdB)) @@ -237,7 +244,8 @@ public static int b2AssignJointColor(ref B2ConstraintGraph graph, int bodyIdA, i #if B2_FORCE_OVERFLOW if (staticA == false && staticB == false) { - for (int i = 0; i < B2_OVERFLOW_INDEX; ++i) + // Dynamic constraint colors cannot encroach on colors reserved for static constraints + for ( int i = 0; i < B2_DYNAMIC_COLOR_COUNT; ++i ) { ref B2GraphColor color = ref graph.colors[i]; if (b2GetBit(ref color.bodySet, bodyIdA) || b2GetBit(ref color.bodySet, bodyIdB)) @@ -252,7 +260,8 @@ public static int b2AssignJointColor(ref B2ConstraintGraph graph, int bodyIdA, i } else if (staticA == false) { - for (int i = 0; i < B2_OVERFLOW_INDEX; ++i) + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i ) { ref B2GraphColor color = ref graph.colors[i]; if (b2GetBit(ref color.bodySet, bodyIdA)) @@ -266,7 +275,8 @@ public static int b2AssignJointColor(ref B2ConstraintGraph graph, int bodyIdA, i } else if (staticB == false) { - for (int i = 0; i < B2_OVERFLOW_INDEX; ++i) + // Static constraint colors build from the end to get higher priority than dyn-dyn constraints + for ( int i = B2_OVERFLOW_INDEX - 1; i >= 1; --i ) { ref B2GraphColor color = ref graph.colors[i]; if (b2GetBit(ref color.bodySet, bodyIdB)) @@ -293,8 +303,8 @@ public static ref B2JointSim b2CreateJointInGraph(B2World world, B2Joint joint) int bodyIdB = joint.edges[1].bodyId; B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); - bool staticA = bodyA.setIndex == (int)B2SetType.b2_staticSet; - bool staticB = bodyB.setIndex == (int)B2SetType.b2_staticSet; + bool staticA = bodyA.type == B2BodyType.b2_staticBody; + bool staticB = bodyB.type == B2BodyType.b2_staticBody; int colorIndex = b2AssignJointColor(ref graph, bodyIdA, bodyIdB, staticA, staticB); diff --git a/src/Box2D.NET/B2ContactSolvers.cs b/src/Box2D.NET/B2ContactSolvers.cs index fde1dd4e..fe236b15 100644 --- a/src/Box2D.NET/B2ContactSolvers.cs +++ b/src/Box2D.NET/B2ContactSolvers.cs @@ -1695,12 +1695,15 @@ public static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2StepCo // Clamp the accumulated impulse B2FloatW newImpulse = b2MaxW(b2SubW(c.normalImpulse1, negImpulse), b2ZeroW()); - B2FloatW impulse = b2SubW(newImpulse, c.normalImpulse1); + B2FloatW deltaImpulse = b2SubW(newImpulse, c.normalImpulse1); c.normalImpulse1 = newImpulse; + // Add the incremental impulse rather than the full impulse because this is not a sub-step + c.totalNormalImpulse1 = b2AddW(c.totalNormalImpulse1, deltaImpulse); + // Apply contact impulse - B2FloatW Px = b2MulW(impulse, c.normal.X); - B2FloatW Py = b2MulW(impulse, c.normal.Y); + B2FloatW Px = b2MulW(deltaImpulse, c.normal.X); + B2FloatW Py = b2MulW(deltaImpulse, c.normal.Y); bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); @@ -1733,12 +1736,15 @@ public static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2StepCo // Clamp the accumulated impulse B2FloatW newImpulse = b2MaxW(b2SubW(c.normalImpulse2, negImpulse), b2ZeroW()); - B2FloatW impulse = b2SubW(newImpulse, c.normalImpulse2); + B2FloatW deltaImpulse = b2SubW(newImpulse, c.normalImpulse2); c.normalImpulse2 = newImpulse; + // Add the incremental impulse rather than the full impulse because this is not a sub-step + c.totalNormalImpulse2 = b2AddW(c.totalNormalImpulse2, deltaImpulse); + // Apply contact impulse - B2FloatW Px = b2MulW(impulse, c.normal.X); - B2FloatW Py = b2MulW(impulse, c.normal.Y); + B2FloatW Px = b2MulW(deltaImpulse, c.normal.X); + B2FloatW Py = b2MulW(deltaImpulse, c.normal.Y); bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); diff --git a/src/Box2D.NET/B2Contacts.cs b/src/Box2D.NET/B2Contacts.cs index 629ea7ba..087b9827 100644 --- a/src/Box2D.NET/B2Contacts.cs +++ b/src/Box2D.NET/B2Contacts.cs @@ -462,7 +462,6 @@ public static bool b2UpdateContact(B2World world, B2ContactSim contactSim, B2Sha contactSim.friction = world.frictionCallback(shapeA.friction, shapeA.userMaterialId, shapeB.friction, shapeB.userMaterialId); contactSim.restitution = world.restitutionCallback(shapeA.restitution, shapeA.userMaterialId, shapeB.restitution, shapeB.userMaterialId); - // todo branch improves perf? if (shapeA.rollingResistance > 0.0f || shapeB.rollingResistance > 0.0f) { float radiusA = b2GetShapeRadius(shapeA); diff --git a/src/Box2D.NET/B2Counters.cs b/src/Box2D.NET/B2Counters.cs index 43a3cbb9..cfae2a56 100644 --- a/src/Box2D.NET/B2Counters.cs +++ b/src/Box2D.NET/B2Counters.cs @@ -17,6 +17,6 @@ public struct B2Counters public int treeHeight; public int byteCount; public int taskCount; - public B2FixedArray12 colorCounts; + public B2FixedArray24 colorCounts; } } diff --git a/src/Box2D.NET/B2DebugDraw.cs b/src/Box2D.NET/B2DebugDraw.cs index a1ad5edc..a3adaa23 100644 --- a/src/Box2D.NET/B2DebugDraw.cs +++ b/src/Box2D.NET/B2DebugDraw.cs @@ -22,9 +22,6 @@ public class B2DebugDraw /// Bounds to use if restricting drawing to a rectangular region public B2AABB drawingBounds; - /// Option to restrict drawing to a rectangular region. May suffer from unstable depth sorting. - public bool useDrawingBounds; - /// Option to draw shapes public bool drawShapes; diff --git a/src/Box2D.NET/B2FixedArray24.cs b/src/Box2D.NET/B2FixedArray24.cs new file mode 100644 index 00000000..1108f4da --- /dev/null +++ b/src/Box2D.NET/B2FixedArray24.cs @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +#pragma warning disable CS0169 + +namespace Box2D.NET +{ + [StructLayout(LayoutKind.Sequential)] + public struct B2FixedArray24 where T : unmanaged + { + public const int Size = 24; + + private T _v0000; + private T _v0001; + private T _v0002; + private T _v0003; + private T _v0004; + private T _v0005; + private T _v0006; + private T _v0007; + private T _v0008; + private T _v0009; + private T _v0010; + private T _v0011; + private T _v0012; + private T _v0013; + private T _v0014; + private T _v0015; + private T _v0016; + private T _v0017; + private T _v0018; + private T _v0019; + private T _v0020; + private T _v0021; + private T _v0022; + private T _v0023; + + public int Length => Size; + + public ref T this[int index] + { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get => ref AsSpan()[index]; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public Span AsSpan() + { + return MemoryMarshal.CreateSpan(ref _v0000, Size); + } + + } +} \ No newline at end of file diff --git a/src/Box2D.NET/B2GraphColor.cs b/src/Box2D.NET/B2GraphColor.cs index 9ff6bea5..860ed5c2 100644 --- a/src/Box2D.NET/B2GraphColor.cs +++ b/src/Box2D.NET/B2GraphColor.cs @@ -11,7 +11,11 @@ public struct B2GraphColor // This bitset is indexed by bodyId so this is over-sized to encompass static bodies // however I never traverse these bits or use the bit count for anything // This bitset is unused on the overflow color. - // todo consider having a uint_16 per body that tracks the graph color membership + // + // Dirk suggested having a uint64_t per body that tracks the graph color membership + // but I think this would make debugging harder and be less flexible. With the bitset + // I can trivially increase the number of graph colors beyond 64. See usage of b2CountSetBits + // for validation. public B2BitSet bodySet; // cache friendly arrays diff --git a/src/Box2D.NET/B2Islands.cs b/src/Box2D.NET/B2Islands.cs index c4a61486..1ec070b1 100644 --- a/src/Box2D.NET/B2Islands.cs +++ b/src/Box2D.NET/B2Islands.cs @@ -225,7 +225,7 @@ public static void b2LinkContact(B2World world, B2Contact contact) { b2AddContactToIsland(world, islandIdB, contact); } - + // todo why not merge the islands right here? } @@ -395,10 +395,13 @@ public static void b2LinkJoint(B2World world, B2Joint joint, bool mergeIslands) } } -// Unlink a joint from the island graph when it is destroyed + // Unlink a joint from the island graph when it is destroyed public static void b2UnlinkJoint(B2World world, B2Joint joint) { - B2_ASSERT(joint.islandId != B2_NULL_INDEX); + if (joint.islandId == B2_NULL_INDEX) + { + return; + } // remove from island int islandId = joint.islandId; @@ -884,6 +887,11 @@ public static void b2SplitIslandTask(int startIndex, int endIndex, uint threadIn #if DEBUG public static void b2ValidateIsland(B2World world, int islandId) { + if (islandId == B2_NULL_INDEX) + { + return; + } + B2Island island = b2Array_Get(ref world.islands, islandId); B2_ASSERT(island.islandId == islandId); B2_ASSERT(island.setIndex != B2_NULL_INDEX); diff --git a/src/Box2D.NET/B2Joints.cs b/src/Box2D.NET/B2Joints.cs index 2d2cc354..c4e1a350 100644 --- a/src/Box2D.NET/B2Joints.cs +++ b/src/Box2D.NET/B2Joints.cs @@ -1595,18 +1595,44 @@ public static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) if (draw.drawGraphColors) { - Span colors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] - { - B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorGreen, - B2HexColor.b2_colorCyan, B2HexColor.b2_colorBlue, B2HexColor.b2_colorViolet, B2HexColor.b2_colorPink, - B2HexColor.b2_colorChocolate, B2HexColor.b2_colorGoldenRod, B2HexColor.b2_colorCoral, B2HexColor.b2_colorBlack + Span graphColors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] { + B2HexColor.b2_colorRed, + B2HexColor.b2_colorOrange, + B2HexColor.b2_colorYellow, + B2HexColor.b2_colorGreen, + + B2HexColor.b2_colorCyan, + B2HexColor.b2_colorBlue, + B2HexColor.b2_colorViolet, + B2HexColor.b2_colorPink, + + B2HexColor.b2_colorChocolate, + B2HexColor.b2_colorGoldenRod, + B2HexColor.b2_colorCoral, + B2HexColor.b2_colorRosyBrown, + + B2HexColor.b2_colorAqua, + B2HexColor.b2_colorPeru, + B2HexColor.b2_colorLime, + B2HexColor.b2_colorGold, + + B2HexColor.b2_colorPlum, + B2HexColor.b2_colorSnow, + B2HexColor.b2_colorTeal, + B2HexColor.b2_colorKhaki, + + B2HexColor.b2_colorSalmon, + B2HexColor.b2_colorPeachPuff, + B2HexColor.b2_colorHoneyDew, + B2HexColor.b2_colorBlack, }; + int colorIndex = joint.colorIndex; if (colorIndex != B2_NULL_INDEX) { B2Vec2 p = b2Lerp(pA, pB, 0.5f); - draw.DrawPointFcn(p, 5.0f, colors[colorIndex], draw.context); + draw.DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw.context ); } } diff --git a/src/Box2D.NET/B2Mutex.cs b/src/Box2D.NET/B2Mutex.cs new file mode 100644 index 00000000..16007611 --- /dev/null +++ b/src/Box2D.NET/B2Mutex.cs @@ -0,0 +1,7 @@ +namespace Box2D.NET +{ + public struct B2Mutex + { + public object lc; + } +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Mutexes.cs b/src/Box2D.NET/B2Mutexes.cs new file mode 100644 index 00000000..632cdc95 --- /dev/null +++ b/src/Box2D.NET/B2Mutexes.cs @@ -0,0 +1,29 @@ +using System.Threading; + +namespace Box2D.NET +{ + public class B2Mutexes + { + public static B2Mutex b2CreateMutex() + { + B2Mutex m = new B2Mutex(); + m.lc = new object(); + return m; + } + + public static void b2DestroyMutex(ref B2Mutex m) + { + m.lc = null; + } + + public static void b2LockMutex(ref B2Mutex m) + { + Monitor.Enter(m.lc); + } + + public static void b2UnlockMutex(ref B2Mutex m) + { + Monitor.Exit(m.lc); + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET/B2SolverSet.cs b/src/Box2D.NET/B2SolverSet.cs index 69e65444..fdbf5c54 100644 --- a/src/Box2D.NET/B2SolverSet.cs +++ b/src/Box2D.NET/B2SolverSet.cs @@ -7,7 +7,7 @@ namespace Box2D.NET { // This holds solver set data. The following sets are used: - // - static set for all static bodies (no contacts or joints) + // - static set for all static bodies and joints between static bodies // - active set for all active bodies with body states (no contacts or joints) // - disabled set for disabled bodies and their joints // - all further sets are sleeping island sets along with their contacts and joints diff --git a/src/Box2D.NET/B2SolverSets.cs b/src/Box2D.NET/B2SolverSets.cs index 23c96a24..e65cee03 100644 --- a/src/Box2D.NET/B2SolverSets.cs +++ b/src/Box2D.NET/B2SolverSets.cs @@ -547,7 +547,10 @@ public static void b2MergeSolverSets(B2World world, int setId1, int setId2) public static void b2TransferBody(B2World world, B2SolverSet targetSet, B2SolverSet sourceSet, B2Body body) { - B2_ASSERT(targetSet != sourceSet); + if (targetSet == sourceSet) + { + return; + } int sourceIndex = body.localIndex; B2BodySim sourceSim = b2Array_Get(ref sourceSet.bodySims, sourceIndex); @@ -557,6 +560,9 @@ public static void b2TransferBody(B2World world, B2SolverSet targetSet, B2Solver //memcpy( targetSim, sourceSim, sizeof( b2BodySim ) ); targetSim.CopyFrom(sourceSim); + // Clear transient body flags + targetSim.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped | (uint)B2BodyFlags.b2_hadTimeOfImpact); + // Remove body sim from solver set that owns it int movedIndex = b2Array_RemoveSwap(ref sourceSet.bodySims, sourceIndex); if (movedIndex != B2_NULL_INDEX) @@ -587,7 +593,10 @@ public static void b2TransferBody(B2World world, B2SolverSet targetSet, B2Solver public static void b2TransferJoint(B2World world, B2SolverSet targetSet, B2SolverSet sourceSet, B2Joint joint) { - B2_ASSERT(targetSet != sourceSet); + if (targetSet == sourceSet) + { + return; + } int localIndex = joint.localIndex; int colorIndex = joint.colorIndex; @@ -643,4 +652,4 @@ public static void b2TransferJoint(B2World world, B2SolverSet targetSet, B2Solve } } } -} +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Solvers.cs b/src/Box2D.NET/B2Solvers.cs index 60273d22..2a292348 100644 --- a/src/Box2D.NET/B2Solvers.cs +++ b/src/Box2D.NET/B2Solvers.cs @@ -40,6 +40,8 @@ public static class B2Solvers public const int ITERATIONS = 1; public const int RELAX_ITERATIONS = 1; + public const float B2_CORE_FRACTION = 0.25f; + // TODO: @ikpil. check SIMD public static readonly int B2_SIMD_SHIFT = b2SIMDShift(); @@ -380,13 +382,14 @@ public static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref B2 if (length > B2_LINEAR_SLOP) { B2Vec2 c1 = continuousContext.centroid1; - float offset1 = b2Cross(b2Sub(c1, p1), e); + float separation1 = b2Cross(b2Sub(c1, p1), e); B2Vec2 c2 = continuousContext.centroid2; - float offset2 = b2Cross(b2Sub(c2, p1), e); + float separation2 = b2Cross(b2Sub(c2, p1), e); + + float coreDistance = B2_CORE_FRACTION * fastBodySim.minExtent; - // todo this should use the min extent of the fast shape, not the body - const float allowedFraction = 0.25f; - if (offset1 < 0.0f || offset1 - offset2 < allowedFraction * fastBodySim.minExtent) + if (separation1 < 0.0f || + (separation1 - separation2 < coreDistance && separation2 > coreDistance)) { // Minimal clipping return true; @@ -461,7 +464,7 @@ public static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref B2 // fallback to TOI of a small circle around the fast shape centroid B2Vec2 centroid = b2GetShapeCentroid(fastShape); B2ShapeExtent extent = b2ComputeShapeExtent(fastShape, centroid); - float radius = 0.25f * extent.minExtent; + float radius = B2_CORE_FRACTION * extent.minExtent; input.proxyB = b2MakeProxy(centroid, 1, radius); output = b2TimeOfImpact(ref input); if (0.0f < output.fraction && output.fraction < continuousContext.fraction) @@ -481,6 +484,7 @@ public static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref B2 if (didHit) { + fastBodySim.flags |= (uint)B2BodyFlags.b2_hadTimeOfImpact; continuousContext.fraction = hitFraction; } } @@ -739,8 +743,9 @@ public static void b2FinalizeBodiesTask(int startIndex, int endIndex, uint threa sim.force = b2Vec2_zero; sim.torque = 0.0f; - body.isSpeedCapped = (sim.flags & (uint)B2BodyFlags.b2_isSpeedCapped) != 0; - sim.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped); + body.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped | (uint)B2BodyFlags.b2_hadTimeOfImpact); + body.flags |= (sim.flags & (uint)(B2BodyFlags.b2_isSpeedCapped | B2BodyFlags.b2_hadTimeOfImpact)); + sim.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped | (uint)B2BodyFlags.b2_hadTimeOfImpact); if (enableSleep == false || body.enableSleep == false || sleepVelocity > body.sleepThreshold) { @@ -1155,6 +1160,7 @@ public static void b2SolverTask(int startIndex, int endIndex, uint threadIndexIg for (int j = 0; j < ITERATIONS; ++j) { + // Overflow constraints have lower priority b2SolveOverflowJoints(context, useBias); b2SolveOverflowContacts(context, useBias); @@ -1405,16 +1411,16 @@ public static void b2Solve(B2World world, B2StepContext stepContext) // Configure blocks for tasks parallel-for each active graph color // The blocks are a mix of SIMD contact blocks and joint blocks - B2_ASSERT(B2FixedArray12.Size == B2_GRAPH_COLOR_COUNT); - B2FixedArray12 arrayActiveColorIndices = new B2FixedArray12(); + B2_ASSERT(B2FixedArray24.Size == B2_GRAPH_COLOR_COUNT); + B2FixedArray24 arrayActiveColorIndices = new B2FixedArray24(); - B2FixedArray12 arrayColorContactCounts = new B2FixedArray12(); - B2FixedArray12 arrayColorContactBlockSizes = new B2FixedArray12(); - B2FixedArray12 arrayColorContactBlockCounts = new B2FixedArray12(); + B2FixedArray24 arrayColorContactCounts = new B2FixedArray24(); + B2FixedArray24 arrayColorContactBlockSizes = new B2FixedArray24(); + B2FixedArray24 arrayColorContactBlockCounts = new B2FixedArray24(); - B2FixedArray12 arrayColorJointCounts = new B2FixedArray12(); - B2FixedArray12 arrayColorJointBlockSizes = new B2FixedArray12(); - B2FixedArray12 arrayColorJointBlockCounts = new B2FixedArray12(); + B2FixedArray24 arrayColorJointCounts = new B2FixedArray24(); + B2FixedArray24 arrayColorJointBlockSizes = new B2FixedArray24(); + B2FixedArray24 arrayColorJointBlockCounts = new B2FixedArray24(); Span activeColorIndices = arrayActiveColorIndices.AsSpan(); @@ -1926,7 +1932,6 @@ public static void b2Solve(B2World world, B2StepContext stepContext) ulong[] bits = jointStateBitSet.bits; B2Joint[] jointArray = world.joints.data; - int jointCapacity = world.joints.capacity; ushort worldIndex0 = world.worldId; for (uint k = 0; k < wordCount; ++k) @@ -1937,7 +1942,7 @@ public static void b2Solve(B2World world, B2StepContext stepContext) uint ctz = b2CTZ64(word); int jointId = (int)(64 * k + ctz); - B2_ASSERT(jointId < jointCapacity); + B2_ASSERT(jointId < world.joints.capacity); B2Joint joint = jointArray[jointId]; diff --git a/src/Box2D.NET/B2Types.cs b/src/Box2D.NET/B2Types.cs index 20f0efa6..c85c83d8 100644 --- a/src/Box2D.NET/B2Types.cs +++ b/src/Box2D.NET/B2Types.cs @@ -168,7 +168,8 @@ public static B2DebugDraw b2DefaultDebugDraw() draw.drawingBounds.lowerBound = new B2Vec2(-float.MaxValue, -float.MaxValue); draw.drawingBounds.upperBound = new B2Vec2(float.MaxValue, float.MaxValue); - draw.useDrawingBounds = true; + + draw.drawShapes = true; return draw; } diff --git a/src/Box2D.NET/B2World.cs b/src/Box2D.NET/B2World.cs index da82313c..32ddd98c 100644 --- a/src/Box2D.NET/B2World.cs +++ b/src/Box2D.NET/B2World.cs @@ -79,6 +79,15 @@ public class B2World public B2Array contactHitEvents; public B2Array jointEvents; + // todo consider deferred waking and impulses to make it possible + // to apply forces and impulses from multiple threads + // impulses must be deferred because sleeping bodies have no velocity state + // Problems: + // - multiple forces applied to the same body from multiple threads + // Deferred wake + //b2BitSet bodyWakeSet; + //b2ImpulseArray deferredImpulses; + // Used to track debug draw public B2BitSet debugBodySet; public B2BitSet debugJointSet; diff --git a/src/Box2D.NET/B2Worlds.cs b/src/Box2D.NET/B2Worlds.cs index 085d6e00..ab58092d 100644 --- a/src/Box2D.NET/B2Worlds.cs +++ b/src/Box2D.NET/B2Worlds.cs @@ -937,11 +937,15 @@ public static bool DrawQueryCallback(int proxyId, ulong userData, ref B2DrawCont { color = B2HexColor.b2_colorWheat; } + else if (0 != (body.flags & (uint)B2BodyFlags.b2_hadTimeOfImpact)) + { + color = B2HexColor.b2_colorLime; + } else if (0 != (bodySim.flags & (uint)B2BodyFlags.b2_isBullet) && body.setIndex == (int)B2SetType.b2_awakeSet) { color = B2HexColor.b2_colorTurquoise; } - else if (body.isSpeedCapped) + else if (0 != (body.flags & (uint)B2BodyFlags.b2_isSpeedCapped)) { color = B2HexColor.b2_colorYellow; } @@ -1003,9 +1007,35 @@ public static void b2DrawWithBounds(B2World world, B2DebugDraw draw) Span graphColors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] { - B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorGreen, - B2HexColor.b2_colorCyan, B2HexColor.b2_colorBlue, B2HexColor.b2_colorViolet, B2HexColor.b2_colorPink, - B2HexColor.b2_colorChocolate, B2HexColor.b2_colorGoldenRod, B2HexColor.b2_colorCoral, B2HexColor.b2_colorBlack + B2HexColor.b2_colorRed, + B2HexColor.b2_colorOrange, + B2HexColor.b2_colorYellow, + B2HexColor.b2_colorGreen, + + B2HexColor.b2_colorCyan, + B2HexColor.b2_colorBlue, + B2HexColor.b2_colorViolet, + B2HexColor.b2_colorPink, + + B2HexColor.b2_colorChocolate, + B2HexColor.b2_colorGoldenRod, + B2HexColor.b2_colorCoral, + B2HexColor.b2_colorRosyBrown, + + B2HexColor.b2_colorAqua, + B2HexColor.b2_colorPeru, + B2HexColor.b2_colorLime, + B2HexColor.b2_colorGold, + + B2HexColor.b2_colorPlum, + B2HexColor.b2_colorSnow, + B2HexColor.b2_colorTeal, + B2HexColor.b2_colorKhaki, + + B2HexColor.b2_colorSalmon, + B2HexColor.b2_colorPeachPuff, + B2HexColor.b2_colorHoneyDew, + B2HexColor.b2_colorBlack, }; int bodyCapacity = b2GetIdCapacity(world.bodyIdPool); @@ -1200,329 +1230,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) return; } - // todo it seems bounds drawing is fast enough for regular usage - if (draw.useDrawingBounds) - { - b2DrawWithBounds(world, draw); - return; - } - - if (draw.drawShapes) - { - int setCount = world.solverSets.count; - for (int setIndex = 0; setIndex < setCount; ++setIndex) - { - B2SolverSet set = b2Array_Get(ref world.solverSets, setIndex); - int bodyCount = set.bodySims.count; - for (int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex) - { - B2BodySim bodySim = set.bodySims.data[bodyIndex]; - B2Body body = b2Array_Get(ref world.bodies, bodySim.bodyId); - B2_ASSERT(body.setIndex == setIndex); - - B2Transform xf = bodySim.transform; - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = world.shapes.data[shapeId]; - B2HexColor color; - - if (shape.customColor != 0) - { - color = (B2HexColor)shape.customColor; - } - else if (body.type == B2BodyType.b2_dynamicBody && body.mass == 0.0f) - { - // Bad body - color = B2HexColor.b2_colorRed; - } - else if (body.setIndex == (int)B2SetType.b2_disabledSet) - { - color = B2HexColor.b2_colorSlateGray; - } - else if (shape.sensorIndex != B2_NULL_INDEX) - { - color = B2HexColor.b2_colorWheat; - } - else if (0 != (bodySim.flags & (uint)B2BodyFlags.b2_isBullet) && body.setIndex == (int)B2SetType.b2_awakeSet) - { - color = B2HexColor.b2_colorTurquoise; - } - else if (body.isSpeedCapped) - { - color = B2HexColor.b2_colorYellow; - } - else if (0 != (bodySim.flags & (uint)B2BodyFlags.b2_isFast)) - { - color = B2HexColor.b2_colorSalmon; - } - else if (body.type == B2BodyType.b2_staticBody) - { - color = B2HexColor.b2_colorPaleGreen; - } - else if (body.type == B2BodyType.b2_kinematicBody) - { - color = B2HexColor.b2_colorRoyalBlue; - } - else if (body.setIndex == (int)B2SetType.b2_awakeSet) - { - color = B2HexColor.b2_colorPink; - } - else - { - color = B2HexColor.b2_colorGray; - } - - b2DrawShape(draw, shape, xf, color); - shapeId = shape.nextShapeId; - } - } - } - } - - if (draw.drawJoints) - { - int count = world.joints.count; - for (int i = 0; i < count; ++i) - { - B2Joint joint = world.joints.data[i]; - if (joint.setIndex == B2_NULL_INDEX) - { - continue; - } - - b2DrawJoint(draw, world, joint); - } - } - - if (draw.drawBounds) - { - B2HexColor color = B2HexColor.b2_colorGold; - - int setCount = world.solverSets.count; - var array4 = new B2FixedArray4(); - Span vs = array4.AsSpan(); - for (int setIndex = 0; setIndex < setCount; ++setIndex) - { - B2SolverSet set = b2Array_Get(ref world.solverSets, setIndex); - int bodyCount = set.bodySims.count; - for (int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex) - { - B2BodySim bodySim = set.bodySims.data[bodyIndex]; - - string buffer = "" + bodySim.bodyId; - draw.DrawStringFcn(bodySim.center, buffer, B2HexColor.b2_colorWhite, draw.context); - - B2Body body = b2Array_Get(ref world.bodies, bodySim.bodyId); - B2_ASSERT(body.setIndex == setIndex); - - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = world.shapes.data[shapeId]; - B2AABB aabb = shape.fatAABB; - - vs[0] = new B2Vec2(aabb.lowerBound.X, aabb.lowerBound.Y); - vs[1] = new B2Vec2(aabb.upperBound.X, aabb.lowerBound.Y); - vs[2] = new B2Vec2(aabb.upperBound.X, aabb.upperBound.Y); - vs[3] = new B2Vec2(aabb.lowerBound.X, aabb.upperBound.Y); - - draw.DrawPolygonFcn(vs, 4, color, draw.context); - - shapeId = shape.nextShapeId; - } - } - } - } - - if (draw.drawBodyNames) - { - B2Vec2 offset = new B2Vec2(0.05f, 0.05f); - int count = world.bodies.count; - for (int i = 0; i < count; ++i) - { - B2Body body = world.bodies.data[i]; - if (body.setIndex == B2_NULL_INDEX) - { - continue; - } - - if (string.IsNullOrEmpty(body.name)) - { - continue; - } - - B2BodySim bodySim = b2GetBodySim(world, body); - - B2Transform transform = new B2Transform(bodySim.center, bodySim.transform.q); - B2Vec2 p = b2TransformPoint(ref transform, offset); - draw.DrawStringFcn(p, body.name, B2HexColor.b2_colorBlueViolet, draw.context); - } - } - - if (draw.drawMass) - { - B2Vec2 offset = new B2Vec2(0.1f, 0.1f); - int setCount = world.solverSets.count; - for (int setIndex = 0; setIndex < setCount; ++setIndex) - { - B2SolverSet set = b2Array_Get(ref world.solverSets, setIndex); - int bodyCount = set.bodySims.count; - for (int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex) - { - B2BodySim bodySim = set.bodySims.data[bodyIndex]; - - B2Transform transform = new B2Transform(bodySim.center, bodySim.transform.q); - draw.DrawTransformFcn(transform, draw.context); - - B2Vec2 p = b2TransformPoint(ref transform, offset); - - float mass = bodySim.invMass > 0.0f ? 1.0f / bodySim.invMass : 0.0f; - string buffer = $"{mass:F2}"; - draw.DrawStringFcn(p, buffer, B2HexColor.b2_colorWhite, draw.context); - } - } - } - - if (draw.drawContacts) - { - const float k_impulseScale = 1.0f; - const float k_axisScale = 0.3f; - float linearSlop = B2_LINEAR_SLOP; - - B2HexColor speculativeColor = B2HexColor.b2_colorLightGray; - B2HexColor addColor = B2HexColor.b2_colorGreen; - B2HexColor persistColor = B2HexColor.b2_colorBlue; - B2HexColor normalColor = B2HexColor.b2_colorDimGray; - B2HexColor impulseColor = B2HexColor.b2_colorMagenta; - B2HexColor frictionColor = B2HexColor.b2_colorYellow; - - Span colors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] - { - B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorGreen, - B2HexColor.b2_colorCyan, B2HexColor.b2_colorBlue, B2HexColor.b2_colorViolet, B2HexColor.b2_colorPink, - B2HexColor.b2_colorChocolate, B2HexColor.b2_colorGoldenRod, B2HexColor.b2_colorCoral, B2HexColor.b2_colorBlack - }; - - for (int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex) - { - ref B2GraphColor graphColor = ref world.constraintGraph.colors[colorIndex]; - - int contactCount = graphColor.contactSims.count; - for (int contactIndex = 0; contactIndex < contactCount; ++contactIndex) - { - B2ContactSim contact = graphColor.contactSims.data[contactIndex]; - int pointCount = contact.manifold.pointCount; - B2Vec2 normal = contact.manifold.normal; - - for (int j = 0; j < pointCount; ++j) - { - ref B2ManifoldPoint point = ref contact.manifold.points[j]; - - if (draw.drawGraphColors && 0 <= colorIndex && colorIndex <= B2_GRAPH_COLOR_COUNT) - { - // graph color - float pointSize = colorIndex == B2_OVERFLOW_INDEX ? 7.5f : 5.0f; - draw.DrawPointFcn(point.point, pointSize, colors[colorIndex], draw.context); - // B2.g_draw.DrawString(point.position, "%d", point.color); - } - else if (point.separation > linearSlop) - { - // Speculative - draw.DrawPointFcn(point.point, 5.0f, speculativeColor, draw.context); - } - else if (point.persisted == false) - { - // Add - draw.DrawPointFcn(point.point, 10.0f, addColor, draw.context); - } - else if (point.persisted == true) - { - // Persist - draw.DrawPointFcn(point.point, 5.0f, persistColor, draw.context); - } - - if (draw.drawContactNormals) - { - B2Vec2 p1 = point.point; - B2Vec2 p2 = b2MulAdd(p1, k_axisScale, normal); - draw.DrawSegmentFcn(p1, p2, normalColor, draw.context); - } - else if (draw.drawContactImpulses) - { - B2Vec2 p1 = point.point; - B2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point.totalNormalImpulse, normal); - draw.DrawSegmentFcn(p1, p2, impulseColor, draw.context); - var buffer = $"{1000.0f * point.totalNormalImpulse:F2}"; - draw.DrawStringFcn(p1, buffer, B2HexColor.b2_colorWhite, draw.context); - } - - if (draw.drawContactFeatures) - { - string buffer = "" + point.id; - draw.DrawStringFcn(point.point, buffer, B2HexColor.b2_colorOrange, draw.context); - } - - if (draw.drawFrictionImpulses) - { - B2Vec2 tangent = b2RightPerp(normal); - B2Vec2 p1 = point.point; - B2Vec2 p2 = b2MulAdd(p1, k_impulseScale * point.tangentImpulse, tangent); - draw.DrawSegmentFcn(p1, p2, frictionColor, draw.context); - var buffer = $"{1000.0f * point.tangentImpulse:F2}"; - draw.DrawStringFcn(p1, buffer, B2HexColor.b2_colorWhite, draw.context); - } - } - } - } - } - - if (draw.drawIslands) - { - int count = world.islands.count; - for (int i = 0; i < count; ++i) - { - B2Island island = world.islands.data[i]; - if (island.setIndex == B2_NULL_INDEX) - { - continue; - } - - int shapeCount = 0; - B2AABB aabb = new B2AABB( - new B2Vec2(float.MaxValue, float.MaxValue), - new B2Vec2(-float.MaxValue, -float.MaxValue) - ); - - int bodyId = island.headBody; - while (bodyId != B2_NULL_INDEX) - { - B2Body body = b2Array_Get(ref world.bodies, bodyId); - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - aabb = b2AABB_Union(aabb, shape.fatAABB); - shapeCount += 1; - shapeId = shape.nextShapeId; - } - - bodyId = body.islandNext; - } - - if (shapeCount > 0) - { - B2FixedArray4 vsBuffer = new B2FixedArray4(); - Span vs = vsBuffer.AsSpan(); - vs[0] = new B2Vec2(aabb.lowerBound.X, aabb.lowerBound.Y); - vs[1] = new B2Vec2(aabb.upperBound.X, aabb.lowerBound.Y); - vs[2] = new B2Vec2(aabb.upperBound.X, aabb.upperBound.Y); - vs[3] = new B2Vec2(aabb.lowerBound.X, aabb.upperBound.Y); - - draw.DrawPolygonFcn(vs, 4, B2HexColor.b2_colorOrangeRed, draw.context); - } - } - } + b2DrawWithBounds(world, draw); } /// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. @@ -2918,16 +2626,16 @@ public static void b2ValidateSolverSets(B2World world) B2_ASSERT(set.islandSims.count == 0); B2_ASSERT(set.bodyStates.count == 0); } - else if (setIndex == (int)B2SetType.b2_awakeSet) - { - B2_ASSERT(set.bodySims.count == set.bodyStates.count); - B2_ASSERT(set.jointSims.count == 0); - } else if (setIndex == (int)B2SetType.b2_disabledSet) { B2_ASSERT(set.islandSims.count == 0); B2_ASSERT(set.bodyStates.count == 0); } + else if (setIndex == (int)B2SetType.b2_awakeSet) + { + B2_ASSERT(set.bodySims.count == set.bodyStates.count); + B2_ASSERT(set.jointSims.count == 0); + } else { B2_ASSERT(set.bodyStates.count == 0); @@ -3106,7 +2814,8 @@ public static void b2ValidateSolverSets(B2World world) for (int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex) { ref B2GraphColor color = ref world.constraintGraph.colors[colorIndex]; - { + int bitCount = 0; + B2_ASSERT(color.contactSims.count >= 0); totalContactCount += color.contactSims.count; for (int i = 0; i < color.contactSims.count; ++i) @@ -3129,11 +2838,14 @@ public static void b2ValidateSolverSets(B2World world) B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); + + bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; + bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; } } - } + - { + B2_ASSERT(color.jointSims.count >= 0); totalJointCount += color.jointSims.count; for (int i = 0; i < color.jointSims.count; ++i) @@ -3153,9 +2865,14 @@ public static void b2ValidateSolverSets(B2World world) B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); + + bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; + bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; } } - } + + // Validate the bit population for this graph color + B2_ASSERT(bitCount == b2CountSetBits(ref color.bodySet)); } int contactIdCount = b2GetIdCount(world.contactIdPool); diff --git a/test/Box2D.NET.Test/B2DeterminismTest.cs b/test/Box2D.NET.Test/B2DeterminismTest.cs index 67f424a2..993d268f 100644 --- a/test/Box2D.NET.Test/B2DeterminismTest.cs +++ b/test/Box2D.NET.Test/B2DeterminismTest.cs @@ -131,8 +131,8 @@ public void FinishTask(object userTask, object userContext) public class B2DeterminismTest { - private const int EXPECTED_SLEEP_STEP = 244; - private const uint EXPECTED_HASH = 0xfcb96059; + private const int EXPECTED_SLEEP_STEP = 320; + private const uint EXPECTED_HASH = 0x948FDA81; private const int e_maxTasks = 128;