From 47d21f14cfe70bd7e85daca98527774297b0e76b Mon Sep 17 00:00:00 2001 From: ikpil Date: Wed, 3 Jun 2026 16:43:45 +0900 Subject: [PATCH] [upstream] Optimizations (erincatto/box2d#1050) Improved performance, up to 20% gain in stacking scenarios. Behavior change: contact solver bias stage no longer applies friction (almost free perf gain) Behavior change: b2World_Step still performs all operations, including collision when timeStep == 0. b2WorldDef now has b2Capacity that lets you pre-size some arrays to reduce allocations. Optimized hit events. These are basically free now. Cleaned up and optimized solver scheduling. Converted to new dynamic array type. Added threading logic to prevent theoretical solver deadlock if the user task system stalls worker 0. Added Compounds benchmark. --- .../Graphics/SolidPolygonRender.cs | 2 +- .../Primitives/SampleEntry.cs | 6 +- src/Box2D.NET.Samples/SampleContext.cs | 3 +- .../Samples/Benchmarks/BenchmarkCompounds.cs | 29 + ...Compound.cs => BenchmarkLargeCompounds.cs} | 10 +- .../Benchmarks/BenchmarkManyPyramids.cs | 8 +- .../Samples/Benchmarks/BenchmarkSleep.cs | 2 +- .../Samples/Benchmarks/BenchmarkWasher.cs | 9 + .../Samples/Collisions/DynamicTree.cs | 2 +- .../Samples/Joints/FilterJoint.cs | 2 +- src/Box2D.NET.Samples/Samples/Sample.cs | 453 +++++++--- .../Samples/SampleFactory.cs | 14 +- .../Samples/Stackings/CircleImpulse.cs | 4 +- src/Box2D.NET.Shared/Benchmarks.cs | 110 ++- src/Box2D.NET/B2ArenaAllocators.cs | 32 +- src/Box2D.NET/B2Arrays.cs | 14 +- src/Box2D.NET/B2BlockDim.cs | 15 + src/Box2D.NET/B2Bodies.cs | 12 +- src/Box2D.NET/B2BroadPhases.cs | 44 +- src/Box2D.NET/B2Capacity.cs | 27 + src/Box2D.NET/B2ConstraintGraphs.cs | 57 +- src/Box2D.NET/B2ContactPrepareSpan.cs | 18 + src/Box2D.NET/B2ContactSolvers.cs | 630 +++++++------ src/Box2D.NET/B2Contacts.cs | 2 +- src/Box2D.NET/B2Cores.cs | 2 +- src/Box2D.NET/B2Counters.cs | 6 + src/Box2D.NET/B2DebugDraw.cs | 2 +- src/Box2D.NET/B2DistanceJoints.cs | 8 +- src/Box2D.NET/B2DynamicTrees.cs | 7 +- ...2FilterJointDef.cs => B2FilterJointDef.cs} | 2 +- src/Box2D.NET/B2FixedArray1.cs | 8 +- src/Box2D.NET/B2FixedArray1024.cs | 8 +- src/Box2D.NET/B2FixedArray11.cs | 8 +- src/Box2D.NET/B2FixedArray12.cs | 8 +- src/Box2D.NET/B2FixedArray16.cs | 8 +- src/Box2D.NET/B2FixedArray2.cs | 8 +- src/Box2D.NET/B2FixedArray24.cs | 8 +- src/Box2D.NET/B2FixedArray3.cs | 8 +- src/Box2D.NET/B2FixedArray32.cs | 8 +- src/Box2D.NET/B2FixedArray4.cs | 8 +- src/Box2D.NET/B2FixedArray64.cs | 8 +- src/Box2D.NET/B2FixedArray7.cs | 8 +- src/Box2D.NET/B2FixedArray8.cs | 8 +- src/Box2D.NET/B2FloatW.cs | 9 +- src/Box2D.NET/B2GraphColor.cs | 4 +- src/Box2D.NET/B2Islands.cs | 46 +- src/Box2D.NET/B2JointPrepareSpan.cs | 14 + src/Box2D.NET/B2Joints.cs | 123 ++- src/Box2D.NET/B2MotorJoints.cs | 12 +- src/Box2D.NET/B2PrismaticJoints.cs | 35 +- src/Box2D.NET/B2Profile.cs | 4 +- src/Box2D.NET/B2RevoluteJoints.cs | 16 +- src/Box2D.NET/B2Sensors.cs | 2 +- src/Box2D.NET/B2Shapes.cs | 28 +- src/Box2D.NET/B2SolverBlock.cs | 94 +- src/Box2D.NET/B2SolverSets.cs | 32 +- src/Box2D.NET/B2SolverStage.cs | 9 +- src/Box2D.NET/B2SolverStageType.cs | 2 + src/Box2D.NET/B2Solvers.cs | 850 ++++++++---------- .../{B2ArenaAllocatorTyped.cs => B2Stack.cs} | 6 +- ...2ArenaAllocator.cs => B2StackAllocator.cs} | 10 +- .../{B2ArenaEntry.cs => B2StackEntry.cs} | 2 +- src/Box2D.NET/B2StepContext.cs | 41 +- src/Box2D.NET/B2SyncBlock.cs | 16 + src/Box2D.NET/B2TaskContext.cs | 9 + src/Box2D.NET/B2TracyCZone.cs | 2 +- src/Box2D.NET/B2Types.cs | 2 +- src/Box2D.NET/B2WeldJoints.cs | 14 +- src/Box2D.NET/B2WheelJoints.cs | 19 +- src/Box2D.NET/B2World.cs | 17 +- src/Box2D.NET/B2WorldDef.cs | 3 + src/Box2D.NET/B2Worlds.cs | 139 ++- test/Box2D.NET.Test/B2ArenaAllocatorTests.cs | 42 +- .../B2ArenaAllocatorTypedTests.cs | 8 +- test/Box2D.NET.Test/B2ArrayTests.cs | 6 +- test/Box2D.NET.Test/B2DeterminismTest.cs | 6 +- test/Box2D.NET.Test/B2DynamicTreeTest.cs | 18 +- test/Box2D.NET.Test/B2WorldTest.cs | 36 + 78 files changed, 2011 insertions(+), 1301 deletions(-) create mode 100644 src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompounds.cs rename src/Box2D.NET.Samples/Samples/Benchmarks/{BenchmarkCompound.cs => BenchmarkLargeCompounds.cs} (91%) create mode 100644 src/Box2D.NET/B2BlockDim.cs create mode 100644 src/Box2D.NET/B2Capacity.cs create mode 100644 src/Box2D.NET/B2ContactPrepareSpan.cs rename src/Box2D.NET/{b2FilterJointDef.cs => B2FilterJointDef.cs} (93%) create mode 100644 src/Box2D.NET/B2JointPrepareSpan.cs rename src/Box2D.NET/{B2ArenaAllocatorTyped.cs => B2Stack.cs} (87%) rename src/Box2D.NET/{B2ArenaAllocator.cs => B2StackAllocator.cs} (90%) rename src/Box2D.NET/{B2ArenaEntry.cs => B2StackEntry.cs} (90%) create mode 100644 src/Box2D.NET/B2SyncBlock.cs diff --git a/src/Box2D.NET.Samples/Graphics/SolidPolygonRender.cs b/src/Box2D.NET.Samples/Graphics/SolidPolygonRender.cs index 72b5ce2c..b3abb93d 100644 --- a/src/Box2D.NET.Samples/Graphics/SolidPolygonRender.cs +++ b/src/Box2D.NET.Samples/Graphics/SolidPolygonRender.cs @@ -19,7 +19,7 @@ public struct SolidPolygonRender public SolidPolygonRender() { - polygons = new List(); + polygons = new List(10 * SolidPolygons.e_batchSize); vaoId = new uint[1]; vboIds = new uint[2]; } diff --git a/src/Box2D.NET.Samples/Primitives/SampleEntry.cs b/src/Box2D.NET.Samples/Primitives/SampleEntry.cs index 1410d287..da553d0d 100644 --- a/src/Box2D.NET.Samples/Primitives/SampleEntry.cs +++ b/src/Box2D.NET.Samples/Primitives/SampleEntry.cs @@ -13,12 +13,14 @@ public class SampleEntry public readonly string Name; public readonly string Title; public readonly Func CreateFcn; + public readonly Func CapacityFcn; - public SampleEntry(string category, string name, Func createFcn) + public SampleEntry(string category, string name, Func createFcn, Func capacityFcn) { Category = category; Name = name; Title = $"{Category} : {Name}"; CreateFcn = createFcn; + CapacityFcn = capacityFcn; } -} \ No newline at end of file +} diff --git a/src/Box2D.NET.Samples/SampleContext.cs b/src/Box2D.NET.Samples/SampleContext.cs index e39b975b..a10ad239 100644 --- a/src/Box2D.NET.Samples/SampleContext.cs +++ b/src/Box2D.NET.Samples/SampleContext.cs @@ -41,6 +41,7 @@ public class SampleContext // These are persisted public int sampleIndex = 0; + public B2Capacity capacity; public B2DebugDraw debugDraw; @@ -75,7 +76,7 @@ private SampleContext(string signature, Glfw glfw) debugDraw.DrawCircleFcn = DrawCircleFcn; debugDraw.DrawSolidCircleFcn = DrawSolidCircleFcn; debugDraw.DrawSolidCapsuleFcn = DrawSolidCapsuleFcn; - debugDraw.drawLineFcn = DrawLineFcn; + debugDraw.DrawLineFcn = DrawLineFcn; debugDraw.DrawTransformFcn = DrawTransformFcn; debugDraw.DrawPointFcn = DrawPointFcn; debugDraw.DrawStringFcn = DrawStringFcn; diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompounds.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompounds.cs new file mode 100644 index 00000000..f8074fda --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompounds.cs @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using static Box2D.NET.Shared.Benchmarks; + +namespace Box2D.NET.Samples.Samples.Benchmarks; + +public class BenchmarkCompounds : Sample +{ + private static readonly int SampleBenchmarkCompounds = SampleFactory.Shared.RegisterSample("Benchmark", "Compounds", Create); + + private static Sample Create(SampleContext context) + { + return new BenchmarkCompounds(context); + } + + public BenchmarkCompounds(SampleContext context) : base(context) + { + if (m_context.restart == false) + { + m_context.camera.center = new B2Vec2(0.0f, 50.0f); + m_context.camera.zoom = 25.0f * 2.2f; + m_context.enableSleep = false; + } + + CreateCompounds(m_worldId); + } +} diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompound.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkLargeCompounds.cs similarity index 91% rename from src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompound.cs rename to src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkLargeCompounds.cs index 31fc6e6f..6c854c8f 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCompound.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkLargeCompounds.cs @@ -10,16 +10,16 @@ namespace Box2D.NET.Samples.Samples.Benchmarks; -public class BenchmarkCompound : Sample +public class BenchmarkLargeCompounds : Sample { - private static readonly int SampleCompound = SampleFactory.Shared.RegisterSample("Benchmark", "Compound", Create); + private static readonly int SampleLargeCompounds = SampleFactory.Shared.RegisterSample("Benchmark", "Large Compounds", Create); private static Sample Create(SampleContext context) { - return new BenchmarkCompound(context); + return new BenchmarkLargeCompounds(context); } - public BenchmarkCompound(SampleContext context) : base(context) + public BenchmarkLargeCompounds(SampleContext context) : base(context) { if (m_context.restart == false) { @@ -105,4 +105,4 @@ public BenchmarkCompound(SampleContext context) : base(context) } } } -} \ No newline at end of file +} diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkManyPyramids.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkManyPyramids.cs index 21433084..0b3c1776 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkManyPyramids.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkManyPyramids.cs @@ -8,13 +8,19 @@ namespace Box2D.NET.Samples.Samples.Benchmarks; public class BenchmarkManyPyramids : Sample { - private static readonly int SampleBenchmarkManyPyramids = SampleFactory.Shared.RegisterSample("Benchmark", "Many Pyramids", Create); + private static readonly int SampleBenchmarkManyPyramids = + SampleFactory.Shared.RegisterSampleWithCapacity("Benchmark", "Many Pyramids", Create, GetCapacity); private static Sample Create(SampleContext context) { return new BenchmarkManyPyramids(context); } + private static B2Capacity GetCapacity() + { + return GetManyPyramidsCapacity(); + } + public BenchmarkManyPyramids(SampleContext context) : base(context) { if (m_context.restart == false) diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSleep.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSleep.cs index 4f29d1a3..f09e2e8a 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSleep.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSleep.cs @@ -103,7 +103,7 @@ public override void Step() if (m_stepCount > 20) { // Creating and destroying a joint will engage the island splitter. - b2FilterJointDef jointDef = b2DefaultFilterJointDef(); + B2FilterJointDef jointDef = b2DefaultFilterJointDef(); jointDef.@base.bodyIdA = m_bodies[0]; jointDef.@base.bodyIdB = m_bodies[1]; B2JointId jointId = b2CreateFilterJoint(m_worldId, jointDef); diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkWasher.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkWasher.cs index bbc7b206..fd70ee88 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkWasher.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkWasher.cs @@ -3,6 +3,7 @@ // SPDX-License-Identifier: MIT using static Box2D.NET.Shared.Benchmarks; +using static Box2D.NET.B2Worlds; namespace Box2D.NET.Samples.Samples.Benchmarks; @@ -25,4 +26,12 @@ private BenchmarkWasher(SampleContext context) : base(context) CreateWasher(m_worldId); } + + public override void Step() + { + base.Step(); + + B2ContactEvents events = b2World_GetContactEvents(m_worldId); + DrawTextLine($"hits = {events.hitCount}"); + } } diff --git a/src/Box2D.NET.Samples/Samples/Collisions/DynamicTree.cs b/src/Box2D.NET.Samples/Samples/Collisions/DynamicTree.cs index 15fe6907..ca7598b6 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/DynamicTree.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/DynamicTree.cs @@ -139,7 +139,7 @@ void BuildTree() float y = -4.0f; - m_tree = b2DynamicTree_Create(); + m_tree = b2DynamicTree_Create(16); B2Vec2 aabbMargin = new B2Vec2(0.1f, 0.1f); diff --git a/src/Box2D.NET.Samples/Samples/Joints/FilterJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/FilterJoint.cs index e0920ba1..1b5c5656 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/FilterJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/FilterJoint.cs @@ -52,7 +52,7 @@ public FilterJoint(SampleContext context) : base(context) B2BodyId bodyId2 = b2CreateBody(m_worldId, bodyDef); b2CreatePolygonShape(bodyId2, shapeDef, box); - b2FilterJointDef jointDef = b2DefaultFilterJointDef(); + B2FilterJointDef jointDef = b2DefaultFilterJointDef(); jointDef.@base.bodyIdA = bodyId1; jointDef.@base.bodyIdB = bodyId2; diff --git a/src/Box2D.NET.Samples/Samples/Sample.cs b/src/Box2D.NET.Samples/Samples/Sample.cs index 03e3bc20..7db11327 100644 --- a/src/Box2D.NET.Samples/Samples/Sample.cs +++ b/src/Box2D.NET.Samples/Samples/Sample.cs @@ -4,7 +4,6 @@ using System; using System.Numerics; -using System.Text; using Box2D.NET.Samples.Graphics; using Box2D.NET.Samples.Helpers; using Box2D.NET.Samples.Primitives; @@ -17,6 +16,7 @@ using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Shapes; using static Box2D.NET.B2Worlds; +using static Box2D.NET.B2ConstraintGraphs; using static Box2D.NET.Shared.RandomSupports; using static Box2D.NET.B2Diagnostics; using static Box2D.NET.Samples.Graphics.Draws; @@ -59,8 +59,9 @@ public class Sample : IDisposable private ulong m_profileWriteIndex; // - private B2Profile m_maxProfile; private B2Profile m_totalProfile; + private static bool s_showProfilePlots; + private static readonly bool[] s_profileRowOpen = new bool[22]; // private bool m_didStep; @@ -93,7 +94,6 @@ public Sample(SampleContext context) m_profileReadIndex = 0; m_profileWriteIndex = 0; - m_maxProfile = new B2Profile(); m_totalProfile = new B2Profile(); g_randomSeed = RAND_SEED; @@ -119,13 +119,9 @@ public void CreateWorld() B2WorldDef worldDef = b2DefaultWorldDef(); worldDef.workerCount = m_context.workerCount; - // worldDef.enqueueTask = EnqueueTask; - // worldDef.finishTask = FinishTask; worldDef.userTaskContext = this; worldDef.enableSleep = m_context.enableSleep; - - // todo experimental - // worldDef.enableContactSoftening = true; + worldDef.capacity = m_context.capacity; m_worldId = b2CreateWorld(worldDef); } @@ -160,77 +156,351 @@ public void TestMathCpp() public virtual void UpdateGui() { - if (m_context.frameTime) - { - UpdateFrameTimeGui(); - } + float fontSize = ImGui.GetFontSize(); if (m_context.drawProfile) { - B2Profile aveProfile = new B2Profile(); + ImGui.SetNextWindowPos(new Vector2(fontSize, 8.0f * fontSize), ImGuiCond.FirstUseEver); + ImGui.Begin("Profile (ms)", ImGuiWindowFlags.NoCollapse | ImGuiWindowFlags.AlwaysAutoResize); + + int count = (int)(m_profileWriteIndex - m_profileReadIndex); + + // Unroll ring buffer into per-field histories. + const int rowCount = 22; + float[][] histories = new float[rowCount][]; + for (int row = 0; row < rowCount; ++row) + { + histories[row] = new float[m_profileCapacity]; + } + + for (int i = 0; i < count; ++i) + { + int index = (int)((m_profileReadIndex + (ulong)i) & (m_profileCapacity - 1)); + B2Profile profile = m_profiles[index]; + for (int row = 0; row < rowCount; ++row) + { + histories[row][i] = GetProfileValue(profile, row); + } + } + + float[] avg = new float[rowCount]; if (m_stepCount > 0) { float scale = 1.0f / m_stepCount; - aveProfile.step = scale * m_totalProfile.step; - aveProfile.pairs = scale * m_totalProfile.pairs; - aveProfile.collide = scale * m_totalProfile.collide; - aveProfile.solve = scale * m_totalProfile.solve; - aveProfile.prepareStages = scale * m_totalProfile.prepareStages; - aveProfile.solveConstraints = scale * m_totalProfile.solveConstraints; - aveProfile.prepareConstraints = scale * m_totalProfile.prepareConstraints; - aveProfile.integrateVelocities = scale * m_totalProfile.integrateVelocities; - aveProfile.warmStart = scale * m_totalProfile.warmStart; - aveProfile.solveImpulses = scale * m_totalProfile.solveImpulses; - aveProfile.integratePositions = scale * m_totalProfile.integratePositions; - aveProfile.relaxImpulses = scale * m_totalProfile.relaxImpulses; - aveProfile.applyRestitution = scale * m_totalProfile.applyRestitution; - aveProfile.storeImpulses = scale * m_totalProfile.storeImpulses; - aveProfile.transforms = scale * m_totalProfile.transforms; - aveProfile.splitIslands = scale * m_totalProfile.splitIslands; - aveProfile.jointEvents = scale * m_totalProfile.jointEvents; - aveProfile.hitEvents = scale * m_totalProfile.hitEvents; - aveProfile.refit = scale * m_totalProfile.refit; - aveProfile.bullets = scale * m_totalProfile.bullets; - aveProfile.sleepIslands = scale * m_totalProfile.sleepIslands; - aveProfile.sensors = scale * m_totalProfile.sensors; + for (int row = 0; row < rowCount; ++row) + { + avg[row] = scale * GetProfileValue(m_totalProfile, row); + } + } + + ref readonly B2Profile current = ref m_profiles[m_currentProfileIndex]; + string[] names = + [ + "step", "pairs", "collide", "solve", "setup", "constraints", "prepare", + "velocities", "warm start", "bias", "positions", "relax", + "restitution", "store", "split islands", "transforms", "joint events", + "hit events", "refit BVH", "sleep", "bullets", "sensors" + ]; + int[] indents = [0, 0, 0, 0, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 0]; + // Derive parent/child links from the indent levels so we can collapse subtrees. + int[] parents = new int[rowCount]; + bool[] hasChildren = new bool[rowCount]; + int[] stack = new int[8]; + int stackSize = 0; + for (int i = 0; i < rowCount; ++i) + { + while (stackSize > 0 && indents[stack[stackSize - 1]] >= indents[i]) + { + stackSize -= 1; + } + + parents[i] = stackSize > 0 ? stack[stackSize - 1] : -1; + stack[stackSize] = i; + stackSize += 1; + + if (parents[i] >= 0) + { + hasChildren[parents[i]] = true; + } + } + + // Match Frame Time chart's first three colors so rows read with the line plot. + Vector4 colorStep = new Vector4(102.0f / 255.0f, 153.0f / 255.0f, 1.0f, 1.0f); + Vector4 colorCollide = new Vector4(1.0f, 140.0f / 255.0f, 51.0f / 255.0f, 1.0f); + Vector4 colorSolve = new Vector4(102.0f / 255.0f, 204.0f / 255.0f, 102.0f / 255.0f, 1.0f); + Vector4 colorDefault = new Vector4(220.0f / 255.0f, 220.0f / 255.0f, 220.0f / 255.0f, 1.0f); + + Vector4[] colors = + [ + colorStep, colorDefault, colorCollide, colorSolve, colorDefault, colorDefault, + colorDefault, colorDefault, colorDefault, colorDefault, colorDefault, colorDefault, + colorDefault, colorDefault, colorDefault, colorDefault, colorDefault, colorDefault, + colorDefault, colorDefault, colorDefault, colorDefault + ]; + float[] now = + [ + current.step, current.pairs, current.collide, current.solve, current.solverSetup, + current.constraints, current.prepareConstraints, current.integrateVelocities, current.warmStart, + current.solveImpulses, current.integratePositions, current.relaxImpulses, current.applyRestitution, + current.storeImpulses, current.splitIslands, current.transforms, current.jointEvents, + current.hitEvents, current.refit, current.sleepIslands, current.bullets, current.sensors + ]; + + if (ImGui.Button("Reset")) + { + ResetProfile(); + } + + ImGui.SameLine(); + ImGui.Checkbox("Show plots", ref s_showProfilePlots); + + ImGuiTableFlags tableFlags = ImGuiTableFlags.BordersInnerV | ImGuiTableFlags.RowBg | ImGuiTableFlags.SizingFixedFit; + int columnCount = s_showProfilePlots ? 6 : 5; + if (ImGui.BeginTable("profile", columnCount, tableFlags)) + { + ImGui.TableSetupColumn("section", ImGuiTableColumnFlags.WidthFixed, 8.0f * fontSize); + ImGui.TableSetupColumn("now", ImGuiTableColumnFlags.WidthFixed, 3.0f * fontSize); + ImGui.TableSetupColumn("avg", ImGuiTableColumnFlags.WidthFixed, 3.0f * fontSize); + ImGui.TableSetupColumn("max", ImGuiTableColumnFlags.WidthFixed, 3.0f * fontSize); + ImGui.TableSetupColumn("% step", ImGuiTableColumnFlags.WidthFixed, 8.0f * fontSize); + if (s_showProfilePlots) + { + ImGui.TableSetupColumn("history", ImGuiTableColumnFlags.WidthFixed, 16.0f * fontSize); + } + ImGui.TableHeadersRow(); + + float rowHeight = 1.5f * fontSize; + + // Bars are drawn relative to the step row so the proportions are visually consistent. + float stepNow = b2MaxFloat(current.step, 0.001f); + + for (int row = 0; row < rowCount; ++row) + { + bool visible = true; + for (int parent = parents[row]; parent >= 0; parent = parents[parent]) + { + if (s_profileRowOpen[parent] == false) + { + visible = false; + break; + } + } + + if (visible == false) + { + continue; + } + + float[] history = histories[row]; + + // Rolling max from live history, replacing the old session-sticky max. + float rollingMax = 0.0f; + for (int i = 0; i < count; ++i) + { + rollingMax = b2MaxFloat(rollingMax, history[i]); + } + + ImGui.TableNextRow(); + ImGui.TableNextColumn(); + if (indents[row] > 0) + { + ImGui.Indent(indents[row] * fontSize); + } + if (hasChildren[row]) + { + ImGuiTreeNodeFlags flags = ImGuiTreeNodeFlags.OpenOnArrow | ImGuiTreeNodeFlags.OpenOnDoubleClick | + ImGuiTreeNodeFlags.NoTreePushOnOpen; + ImGui.PushStyleColor(ImGuiCol.Text, colors[row]); + s_profileRowOpen[row] = ImGui.TreeNodeEx(names[row], flags); + ImGui.PopStyleColor(); + } + else + { + float leafIndent = ImGui.GetTreeNodeToLabelSpacing(); + ImGui.Indent(leafIndent); + ImGui.PushStyleColor(ImGuiCol.Text, colors[row]); + ImGui.TextUnformatted(names[row]); + ImGui.PopStyleColor(); + ImGui.Unindent(leafIndent); + } + if (indents[row] > 0) + { + ImGui.Unindent(indents[row] * fontSize); + } + + ImGui.TableNextColumn(); + ImGui.Text($"{now[row],6:F2}"); + ImGui.TableNextColumn(); + ImGui.Text($"{avg[row],6:F2}"); + ImGui.TableNextColumn(); + ImGui.Text($"{rollingMax,6:F2}"); + + ImGui.TableNextColumn(); + float frac = b2ClampFloat(now[row] / stepNow, 0.0f, 1.0f); + ImGui.PushStyleColor(ImGuiCol.PlotHistogram, colors[row]); + ImGui.ProgressBar(frac, new Vector2(-float.Epsilon, 0.0f), ""); + ImGui.PopStyleColor(); + + if (s_showProfilePlots) + { + ImGui.TableNextColumn(); + if (count > 1) + { + ImGui.PushStyleColor(ImGuiCol.PlotLines, colors[row]); + ImGui.PlotLines($"##h{row}", ref history[0], count, 0, null, 0.0f, rollingMax * 1.05f + 0.001f, new Vector2(-float.Epsilon, rowHeight)); + ImGui.PopStyleColor(); + } + } + } + + ImGui.EndTable(); + } + + ImGui.End(); + } + + if (m_context.drawCounters) + { + B2Counters s = b2World_GetCounters(m_worldId); + int colorCount = s.colorCounts.Length; + int overflowIndex = colorCount - 1; + + // Bars are scaled to the largest non-overflow color so the distribution shape reads clearly; + // overflow gets its own bar against the same scale, with a red tint to flag coupling problems. + int totalCount = 0; + int maxCount = 1; + for (int i = 0; i < colorCount; ++i) + { + totalCount += s.colorCounts[i]; + if (i != overflowIndex && s.colorCounts[i] > maxCount) + { + maxCount = s.colorCounts[i]; + } } - ref readonly B2Profile p = ref m_profiles[m_currentProfileIndex]; - DrawTextLine($"step [ave] (max) = {p.step,5:F2} [{aveProfile.step,6:F2}] ({m_maxProfile.step,6:F2})"); - DrawTextLine($"pairs [ave] (max) = {p.pairs,5:F2} [{aveProfile.pairs,6:F2}] ({m_maxProfile.pairs,6:F2})"); - DrawTextLine($"collide [ave] (max) = {p.collide,5:F2} [{aveProfile.collide,6:F2}] ({m_maxProfile.collide,6:F2})"); - DrawTextLine($"solve [ave] (max) = {p.solve,5:F2} [{aveProfile.solve,6:F2}] ({m_maxProfile.solve,6:F2})"); - DrawTextLine($"> prepare tasks [ave] (max) = {p.prepareStages,5:F2} [{aveProfile.prepareStages,6:F2}] ({m_maxProfile.prepareStages,6:F2})"); - DrawTextLine($"> solve constraints [ave] (max) = {p.solveConstraints,5:F2} [{aveProfile.solveConstraints,6:F2}] ({m_maxProfile.solveConstraints,6:F2})"); - DrawTextLine($">> prepare constraints [ave] (max) = {p.prepareConstraints,5:F2} [{aveProfile.prepareConstraints,6:F2}] ({m_maxProfile.prepareConstraints,6:F2})"); - DrawTextLine($">> integrate velocities [ave] (max) = {p.integrateVelocities,5:F2} [{aveProfile.integrateVelocities,6:F2}] ({m_maxProfile.integrateVelocities,6:F2})"); - DrawTextLine($">> warm start [ave] (max) = {p.warmStart,5:F2} [{aveProfile.warmStart,6:F2}] ({m_maxProfile.warmStart,6:F2})"); - DrawTextLine($">> solve impulses [ave] (max) = {p.solveImpulses,5:F2} [{aveProfile.solveImpulses,6:F2}] ({m_maxProfile.solveImpulses,6:F2})"); - DrawTextLine($">> integrate positions [ave] (max) = {p.integratePositions,5:F2} [{aveProfile.integratePositions,6:F2}] ({m_maxProfile.integratePositions,6:F2})"); - DrawTextLine($">> relax impulses [ave] (max) = {p.relaxImpulses,5:F2} [{aveProfile.relaxImpulses,6:F2}] ({m_maxProfile.relaxImpulses,6:F2})"); - DrawTextLine($">> apply restitution [ave] (max) = {p.applyRestitution,5:F2} [{aveProfile.applyRestitution,6:F2}] ({m_maxProfile.applyRestitution,6:F2})"); - DrawTextLine($">> store impulses [ave] (max) = {p.storeImpulses,5:F2} [{aveProfile.storeImpulses,6:F2}] ({m_maxProfile.storeImpulses,6:F2})"); - DrawTextLine($">> split islands [ave] (max) = {p.splitIslands,5:F2} [{aveProfile.splitIslands,6:F2}] ({m_maxProfile.splitIslands,6:F2})"); - DrawTextLine($"> update transforms [ave] (max) = {p.transforms,5:F2} [{aveProfile.transforms,6:F2}] ({m_maxProfile.transforms,6:F2})"); - DrawTextLine($"> joint events [ave] (max) = {p.jointEvents,5:F2} [{aveProfile.jointEvents,6:F2}] ({m_maxProfile.jointEvents})"); - DrawTextLine($"> hit events [ave] (max) = {p.hitEvents,5:F2} [{aveProfile.hitEvents,6:F2}] ({m_maxProfile.hitEvents,6:F2})"); - DrawTextLine($"> refit BVH [ave] (max) = {p.refit,5:F2} [{aveProfile.refit,6:F2}] ({m_maxProfile.refit,6:F2})"); - DrawTextLine($"> sleep islands [ave] (max) = {p.sleepIslands,5:F2} [{aveProfile.sleepIslands,6:F2}] ({m_maxProfile.sleepIslands,6:F2})"); - DrawTextLine($"> bullets [ave] (max) = {p.bullets,5:F2} [{aveProfile.bullets,6:F2}] ({m_maxProfile.bullets,6:F2})"); - DrawTextLine($"sensors [ave] (max) = {p.sensors,5:F2} [{aveProfile.sensors,6:F2}] ({m_maxProfile.sensors,6:F2})"); + ImGui.SetNextWindowPos(new Vector2(fontSize, 8.0f * fontSize), ImGuiCond.FirstUseEver); + ImGui.Begin("Counters", ImGuiWindowFlags.NoCollapse | ImGuiWindowFlags.AlwaysAutoResize); + ImGui.Text($"bodies/shapes/contacts/joints = {s.bodyCount}/{s.shapeCount}/{s.contactCount}/{s.jointCount}"); + + float recycledFraction = s.awakeContactCount > 0 + ? b2ClampFloat((float)s.recycledContactCount / s.awakeContactCount, 0.0f, 1.0f) + : 0.0f; + ImGui.TextUnformatted("recycled contacts"); + ImGui.SameLine(); + ImGui.ProgressBar(recycledFraction, new Vector2(-float.Epsilon, 0.0f), $"{s.recycledContactCount} / {s.awakeContactCount}"); + + ImGui.Text($"islands/tasks = {s.islandCount}/{s.taskCount}"); + ImGui.Text($"tree height static/movable = {s.staticTreeHeight}/{s.treeHeight}"); + ImGui.Text($"stack allocator size = {s.stackUsed / 1024} K"); + ImGui.Text($"total allocation = {s.byteCount / 1024} K"); + + ImGui.Separator(); + B2Capacity capacity = b2World_GetMaxCapacity(m_worldId); + ImGui.TextUnformatted("max capacities"); + ImGui.BulletText($"static shapes/bodies = {capacity.staticShapeCount}/{capacity.staticBodyCount}"); + ImGui.BulletText($"dynamic shapes/bodies = {capacity.dynamicShapeCount}/{capacity.dynamicBodyCount}"); + ImGui.BulletText($"contacts = {capacity.contactCount}"); + + ImGui.Separator(); + ImGui.Text($"{totalCount} constraints across {colorCount} colors"); + + ImGuiTableFlags tableFlags = ImGuiTableFlags.BordersInnerV | ImGuiTableFlags.RowBg | ImGuiTableFlags.SizingFixedFit; + if (ImGui.BeginTable("graphColors", 3, tableFlags)) + { + ImGui.TableSetupColumn("color", ImGuiTableColumnFlags.WidthFixed, 3.5f * fontSize); + ImGui.TableSetupColumn("count", ImGuiTableColumnFlags.WidthFixed, 5.0f * fontSize); + ImGui.TableSetupColumn("share", ImGuiTableColumnFlags.WidthFixed, 16.0f * fontSize); + ImGui.TableHeadersRow(); + + float invMax = 1.0f / maxCount; + for (int i = 0; i < colorCount; ++i) + { + int count = s.colorCounts[i]; + bool isOverflow = i == overflowIndex; + + // Skip empty slots, but always show overflow -- a non-zero overflow row is the signal we care about. + if (count == 0 && isOverflow == false) + { + continue; + } + + Vector4 color = isOverflow + ? new Vector4(220.0f / 255.0f, 60.0f / 255.0f, 60.0f / 255.0f, 1.0f) + : HexToColor(b2GetGraphColor(i)); + + ImGui.TableNextRow(); + ImGui.TableNextColumn(); + ImGui.PushStyleColor(ImGuiCol.Text, color); + ImGui.TextUnformatted(isOverflow ? "over" : i.ToString()); + ImGui.PopStyleColor(); + + ImGui.TableNextColumn(); + ImGui.Text(count.ToString()); + + ImGui.TableNextColumn(); + ImGui.PushStyleColor(ImGuiCol.PlotHistogram, color); + ImGui.ProgressBar(b2ClampFloat(count * invMax, 0.0f, 1.0f), new Vector2(-float.Epsilon, 0.0f), ""); + ImGui.PopStyleColor(); + } + + ImGui.EndTable(); + } + + ImGui.End(); + } + + if (m_context.frameTime) + { + UpdateFrameTimeGui(fontSize); } } - private void UpdateFrameTimeGui() + private static float GetProfileValue(in B2Profile profile, int row) + { + return row switch + { + 0 => profile.step, + 1 => profile.pairs, + 2 => profile.collide, + 3 => profile.solve, + 4 => profile.solverSetup, + 5 => profile.constraints, + 6 => profile.prepareConstraints, + 7 => profile.integrateVelocities, + 8 => profile.warmStart, + 9 => profile.solveImpulses, + 10 => profile.integratePositions, + 11 => profile.relaxImpulses, + 12 => profile.applyRestitution, + 13 => profile.storeImpulses, + 14 => profile.splitIslands, + 15 => profile.transforms, + 16 => profile.jointEvents, + 17 => profile.hitEvents, + 18 => profile.refit, + 19 => profile.sleepIslands, + 20 => profile.bullets, + 21 => profile.sensors, + _ => 0.0f, + }; + } + + private static Vector4 HexToColor(B2HexColor color) { - const float frameTimeHeight = 400.0f; - const float frameTimeWidth = 800.0f; + uint hex = (uint)color; + return new Vector4(((hex >> 16) & 0xFF) / 255.0f, ((hex >> 8) & 0xFF) / 255.0f, (hex & 0xFF) / 255.0f, 1.0f); + } - ImGui.SetNextWindowPos(new Vector2(30.0f, 30.0f), ImGuiCond.FirstUseEver); + private void UpdateFrameTimeGui(float fontSize) + { + float frameTimeHeight = 30.0f * fontSize; + float frameTimeWidth = 50.0f * fontSize; + + ImGui.SetNextWindowPos(new Vector2(3.0f * fontSize, 3.0f * fontSize), ImGuiCond.FirstUseEver); ImGui.SetNextWindowSize(new Vector2(frameTimeWidth, frameTimeHeight), ImGuiCond.FirstUseEver); ImGui.Begin("Frame Time", ref m_context.frameTime, ImGuiWindowFlags.NoCollapse); - ImGui.PushItemWidth(ImGui.GetWindowWidth() - 20.0f); + ImGui.PushItemWidth(ImGui.GetWindowWidth() - 2.0f * fontSize); int count = (int)(m_profileWriteIndex - m_profileReadIndex); float maxValue = 0.0f; @@ -242,7 +512,7 @@ private void UpdateFrameTimeGui() } // This is the pixel size, not the range. - Vector2 plotSize = new Vector2(-1.0f, 22.0f * ImGui.GetTextLineHeight()); + Vector2 plotSize = new Vector2(-1.0f, 22.0f * fontSize); DrawProfilePlot("Profile", count, maxValue, plotSize); ImGui.PopItemWidth(); @@ -453,7 +723,6 @@ public void DrawTextLine(string text) public void ResetProfile() { m_totalProfile = new B2Profile(); - m_maxProfile = new B2Profile(); m_stepCount = 0; m_currentProfileIndex = 0; m_profileReadIndex = 0; @@ -530,39 +799,16 @@ public virtual void Step() m_profileWriteIndex += 1; } - // Track maximum profile times + // Accumulate profile averages if (m_didStep) { B2Profile p = m_profiles[m_currentProfileIndex]; - m_maxProfile.step = b2MaxFloat(m_maxProfile.step, p.step); - m_maxProfile.pairs = b2MaxFloat(m_maxProfile.pairs, p.pairs); - m_maxProfile.collide = b2MaxFloat(m_maxProfile.collide, p.collide); - m_maxProfile.solve = b2MaxFloat(m_maxProfile.solve, p.solve); - m_maxProfile.prepareStages = b2MaxFloat(m_maxProfile.prepareStages, p.prepareStages); - m_maxProfile.solveConstraints = b2MaxFloat(m_maxProfile.solveConstraints, p.solveConstraints); - m_maxProfile.prepareConstraints = b2MaxFloat(m_maxProfile.prepareConstraints, p.prepareConstraints); - m_maxProfile.integrateVelocities = b2MaxFloat(m_maxProfile.integrateVelocities, p.integrateVelocities); - m_maxProfile.warmStart = b2MaxFloat(m_maxProfile.warmStart, p.warmStart); - m_maxProfile.solveImpulses = b2MaxFloat(m_maxProfile.solveImpulses, p.solveImpulses); - m_maxProfile.integratePositions = b2MaxFloat(m_maxProfile.integratePositions, p.integratePositions); - m_maxProfile.relaxImpulses = b2MaxFloat(m_maxProfile.relaxImpulses, p.relaxImpulses); - m_maxProfile.applyRestitution = b2MaxFloat(m_maxProfile.applyRestitution, p.applyRestitution); - m_maxProfile.storeImpulses = b2MaxFloat(m_maxProfile.storeImpulses, p.storeImpulses); - m_maxProfile.transforms = b2MaxFloat(m_maxProfile.transforms, p.transforms); - m_maxProfile.splitIslands = b2MaxFloat(m_maxProfile.splitIslands, p.splitIslands); - m_maxProfile.jointEvents = b2MaxFloat(m_maxProfile.jointEvents, p.jointEvents); - m_maxProfile.hitEvents = b2MaxFloat(m_maxProfile.hitEvents, p.hitEvents); - m_maxProfile.refit = b2MaxFloat(m_maxProfile.refit, p.refit); - m_maxProfile.bullets = b2MaxFloat(m_maxProfile.bullets, p.bullets); - m_maxProfile.sleepIslands = b2MaxFloat(m_maxProfile.sleepIslands, p.sleepIslands); - m_maxProfile.sensors = b2MaxFloat(m_maxProfile.sensors, p.sensors); - m_totalProfile.step += p.step; m_totalProfile.pairs += p.pairs; m_totalProfile.collide += p.collide; m_totalProfile.solve += p.solve; - m_totalProfile.prepareStages += p.prepareStages; - m_totalProfile.solveConstraints += p.solveConstraints; + m_totalProfile.solverSetup += p.solverSetup; + m_totalProfile.constraints += p.constraints; m_totalProfile.prepareConstraints += p.prepareConstraints; m_totalProfile.integrateVelocities += p.integrateVelocities; m_totalProfile.warmStart += p.warmStart; @@ -596,31 +842,6 @@ public virtual void Draw() b2World_Draw(m_worldId, m_context.debugDraw); - if (m_context.drawCounters) - { - B2Counters s = b2World_GetCounters(m_worldId); - - DrawTextLine($"bodies/shapes/contacts/joints = {s.bodyCount}/{s.shapeCount}/{s.contactCount}/{s.jointCount}"); - DrawTextLine($"islands/tasks = {s.islandCount}/{s.taskCount}"); - DrawTextLine($"tree height static/movable = {s.staticTreeHeight}/{s.treeHeight}"); - - int totalCount = 0; - var buffer = new StringBuilder(); - B2_ASSERT(s.colorCounts.Length == 24); - - // todo fix this - buffer.Append("colors: "); - for (int i = 0; i < s.colorCounts.Length; ++i) - { - buffer.Append($"{s.colorCounts[i]}/"); - totalCount += s.colorCounts[i]; - } - - buffer.Append($"[{totalCount}]"); - DrawTextLine(buffer.ToString()); - DrawTextLine($"stack allocator size = {s.stackUsed / 1024} K"); - DrawTextLine($"total allocation = {s.byteCount / 1024} K"); - } } public void ShiftOrigin(B2Vec2 newOrigin) diff --git a/src/Box2D.NET.Samples/Samples/SampleFactory.cs b/src/Box2D.NET.Samples/Samples/SampleFactory.cs index 59a522c9..d381008a 100644 --- a/src/Box2D.NET.Samples/Samples/SampleFactory.cs +++ b/src/Box2D.NET.Samples/Samples/SampleFactory.cs @@ -8,6 +8,7 @@ using System.Reflection; using Box2D.NET.Samples.Primitives; using Serilog; +using static Box2D.NET.B2Types; namespace Box2D.NET.Samples.Samples; @@ -28,7 +29,15 @@ private SampleFactory() public int RegisterSample(string category, string name, Func fcn) { int index = _sampleEntries.Count; - var entry = new SampleEntry(category, name, fcn); + var entry = new SampleEntry(category, name, fcn, null); + _sampleEntries.Add(entry); + return index; + } + + public int RegisterSampleWithCapacity(string category, string name, Func fcn, Func capacityFcn) + { + int index = _sampleEntries.Count; + var entry = new SampleEntry(category, name, fcn, capacityFcn); _sampleEntries.Add(entry); return index; } @@ -36,6 +45,7 @@ public int RegisterSample(string category, string name, Func b2CreateArenaAllocator(int capacity) where T : new() + public static B2Stack b2CreateStack(int capacity) where T : new() { B2_ASSERT(capacity >= 0); - B2ArenaAllocatorTyped allocatorImpl = new B2ArenaAllocatorTyped(); + B2Stack allocatorImpl = new B2Stack(); allocatorImpl.capacity = capacity; allocatorImpl.data = b2Alloc(capacity); allocatorImpl.allocation = 0; allocatorImpl.maxAllocation = 0; allocatorImpl.index = 0; - allocatorImpl.entries = b2Array_Create>(capacity); + allocatorImpl.entries = b2Array_Create>(capacity); return allocatorImpl; } - public static ArraySegment b2AllocateArenaItem(B2ArenaAllocator allocator, int size, string name) where T : new() + public static ArraySegment b2StackAlloc(B2StackAllocator allocator, int size, string name) where T : new() { var alloc = allocator.GetOrCreateFor(); // ensure allocation is 32 byte aligned to support 256-bit SIMD int size32 = ((size - 1) | 0x1F) + 1; - B2ArenaEntry entry = new B2ArenaEntry(); + B2StackEntry entry = new B2StackEntry(); entry.size = size32; entry.name = name; if (alloc.index + size32 > alloc.capacity) @@ -75,12 +75,12 @@ public static void b2DestroyArenaAllocator(B2ArenaAllocator allocator) return entry.data; } - public static void b2FreeArenaItem(B2ArenaAllocator allocator, ArraySegment mem) where T : new() + public static void b2StackFree(B2StackAllocator allocator, ArraySegment mem) where T : new() { var alloc = allocator.GetOrCreateFor(); int entryCount = alloc.entries.count; B2_ASSERT(entryCount > 0); - ref B2ArenaEntry entry = ref alloc.entries.data[entryCount - 1]; + ref B2StackEntry entry = ref alloc.entries.data[entryCount - 1]; B2_ASSERT(mem == entry.data); if (entry.usedMalloc) { @@ -94,8 +94,9 @@ public static void b2DestroyArenaAllocator(B2ArenaAllocator allocator) alloc.allocation -= entry.size; b2Array_Pop(ref alloc.entries); } - // Grow the arena based on usage - public static void b2GrowArena(B2ArenaAllocator allocator) + + // Grow the stack based on usage + public static void b2GrowStack(B2StackAllocator allocator) { var allocSpan = allocator.AsSpan(); @@ -106,8 +107,7 @@ public static void b2GrowArena(B2ArenaAllocator allocator) } } - // Grow the arena based on usage - public static int b2GetArenaCapacity(B2ArenaAllocator allocator) + public static int b2GetStackCapacity(B2StackAllocator allocator) { int capacity = 0; var allocSpan = allocator.AsSpan(); @@ -119,7 +119,7 @@ public static int b2GetArenaCapacity(B2ArenaAllocator allocator) return capacity; } - public static int b2GetArenaAllocation(B2ArenaAllocator allocator) + public static int b2GetStackAllocation(B2StackAllocator allocator) { int allocation = 0; var allocSpan = allocator.AsSpan(); @@ -131,7 +131,7 @@ public static int b2GetArenaAllocation(B2ArenaAllocator allocator) return allocation; } - public static int b2GetMaxArenaAllocation(B2ArenaAllocator allocator) + public static int b2GetMaxStackAllocation(B2StackAllocator allocator) { int maxAllocation = 0; var allocSpan = allocator.AsSpan(); diff --git a/src/Box2D.NET/B2Arrays.cs b/src/Box2D.NET/B2Arrays.cs index 6a48e4bb..6190712b 100644 --- a/src/Box2D.NET/B2Arrays.cs +++ b/src/Box2D.NET/B2Arrays.cs @@ -56,13 +56,13 @@ public static ref T b2Array_Get(ref B2Array a, int index) return ref a.data[index]; } - /* Add */ + /* Emplace */ [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static ref T b2Array_Add(ref B2Array a) where T : new() + public static ref T b2Array_Emplace(ref B2Array a) where T : new() { - if (a.count == a.capacity) + if (a.count >= a.capacity) { - int newCapacity = a.capacity < 2 ? 2 : a.capacity + (a.capacity >> 1); + int newCapacity = a.capacity == 0 ? 8 : 2 * a.capacity; b2Array_Reserve(ref a, newCapacity); } @@ -74,9 +74,9 @@ public static ref T b2Array_Get(ref B2Array a, int index) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void b2Array_Push(ref B2Array a, T value) where T : new() { - if (a.count == a.capacity) + if (a.count >= a.capacity) { - int newCapacity = a.capacity < 2 ? 2 : a.capacity + (a.capacity >> 1); + int newCapacity = a.capacity == 0 ? 8 : 2 * a.capacity; b2Array_Reserve(ref a, newCapacity); } @@ -211,4 +211,4 @@ public static void b2Array_Destroy(ref B2Array a) a.count = n; } } -} \ No newline at end of file +} diff --git a/src/Box2D.NET/B2BlockDim.cs b/src/Box2D.NET/B2BlockDim.cs new file mode 100644 index 00000000..23f3c7fc --- /dev/null +++ b/src/Box2D.NET/B2BlockDim.cs @@ -0,0 +1,15 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +namespace Box2D.NET +{ + public struct B2BlockDim + { + // number of items per block (except last block) + public int size; + + // total number of blocks + public int count; + } +} diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index ad26b4c6..fdbb66d0 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/src/Box2D.NET/B2Bodies.cs @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT @@ -258,7 +258,7 @@ public static B2BodyId b2CreateBody(B2WorldId worldId, in B2BodyDef def) B2SolverSet set = b2Array_Get(ref world.solverSets, setId); - ref B2BodySim bodySim = ref b2Array_Add(ref set.bodySims); + ref B2BodySim bodySim = ref b2Array_Emplace(ref set.bodySims); //*bodySim = ( b2BodySim ){ 0 }; bodySim.Clear(); bodySim.transform.p = def.position; @@ -280,7 +280,7 @@ public static B2BodyId b2CreateBody(B2WorldId worldId, in B2BodyDef def) if (setId == (int)B2SolverSetType.b2_awakeSet) { - ref B2BodyState bodyState = ref b2Array_Add(ref set.bodyStates); + ref B2BodyState bodyState = ref b2Array_Emplace(ref set.bodyStates); //B2_ASSERT( ( (uintptr_t)bodyState & 0x1F ) == 0 ); //*bodyState = ( b2BodyState ){ 0 }; bodyState.Clear(); @@ -581,7 +581,7 @@ internal static void b2UpdateBodyMassData(B2World world, B2Body body) } int shapeCount = body.shapeCount; - ArraySegment masses = b2AllocateArenaItem(world.arena, shapeCount, "mass data"); + ArraySegment masses = b2StackAlloc(world.stack, shapeCount, "mass data"); // Accumulate mass over all shapes. B2Vec2 localCenter = b2Vec2_zero; @@ -629,7 +629,7 @@ internal static void b2UpdateBodyMassData(B2World world, B2Body body) body.inertia += inertia; } - b2FreeArenaItem(world.arena, masses); + b2StackFree(world.stack, masses); masses = null; B2_ASSERT(body.inertia >= 0.0f); @@ -1052,7 +1052,7 @@ public static void b2Body_ApplyTorque(B2BodyId bodyId, float torque, bool wake) /// step. So this only needs to be called if the application wants to remove the effect of previous /// calls to apply forces and torques before the world step is called. /// @param bodyId The body id - internal static void b2Body_ClearForces(B2BodyId bodyId) + public static void b2Body_ClearForces(B2BodyId bodyId) { B2World world = b2GetWorld(bodyId.world0); B2Body body = b2GetBodyFullId(world, bodyId); diff --git a/src/Box2D.NET/B2BroadPhases.cs b/src/Box2D.NET/B2BroadPhases.cs index 77d59651..7159a000 100644 --- a/src/Box2D.NET/B2BroadPhases.cs +++ b/src/Box2D.NET/B2BroadPhases.cs @@ -66,7 +66,7 @@ public static void b2BufferMove(B2BroadPhase bp, int queryProxy) // #include - public static void b2CreateBroadPhase(ref B2BroadPhase bp) + public static void b2CreateBroadPhase(ref B2BroadPhase bp, in B2Capacity capacity) { B2_ASSERT((int)B2BodyType.b2_bodyTypeCount == 3, "must be three body types"); @@ -77,18 +77,27 @@ public static void b2CreateBroadPhase(ref B2BroadPhase bp) // } bp = new B2BroadPhase(); bp.trees = new B2DynamicTree[(int)B2BodyType.b2_bodyTypeCount]; - bp.moveSet = b2CreateSet(16); - bp.moveArray = b2Array_Create(16); + bp.moveSet = b2CreateSet(b2MaxInt(16, 2 * capacity.dynamicShapeCount)); + bp.moveArray = b2Array_Create(b2MaxInt(16, capacity.dynamicShapeCount)); bp.moveResults = null; bp.movePairs = null; bp.movePairCapacity = 0; b2AtomicStoreInt(ref bp.movePairIndex, 0); - bp.pairSet = b2CreateSet(32); + bp.pairSet = b2CreateSet(b2MaxInt(32, 2 * capacity.contactCount)); - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) - { - bp.trees[i] = b2DynamicTree_Create(); - } + int staticCapacity = b2MaxInt(16, capacity.staticShapeCount); + bp.trees[(int)B2BodyType.b2_staticBody] = b2DynamicTree_Create(staticCapacity); + + int kinematicCapacity = 16; + bp.trees[(int)B2BodyType.b2_kinematicBody] = b2DynamicTree_Create(kinematicCapacity); + + int dynamicCapacity = b2MaxInt(16, capacity.dynamicShapeCount); + bp.trees[(int)B2BodyType.b2_dynamicBody] = b2DynamicTree_Create(dynamicCapacity); + } + + public static void b2CreateBroadPhase(ref B2BroadPhase bp) + { + b2CreateBroadPhase(ref bp, new B2Capacity()); } public static void b2DestroyBroadPhase(B2BroadPhase bp) @@ -410,7 +419,8 @@ public static void b2UpdateTreesTask(object context) b2TracyCZoneNC(B2TracyCZone.tree_task, "Rebuild BVH", B2HexColor.b2_colorFireBrick, true); B2World world = (B2World)context; - b2BroadPhase_RebuildTrees(world.broadPhase); + b2DynamicTree_Rebuild(world.broadPhase.trees[(int)B2BodyType.b2_dynamicBody], false); + b2DynamicTree_Rebuild(world.broadPhase.trees[(int)B2BodyType.b2_kinematicBody], false); b2TracyCZoneEnd(B2TracyCZone.tree_task); } @@ -429,15 +439,15 @@ public static void b2UpdateBroadPhasePairs(B2World world) b2TracyCZoneNC(B2TracyCZone.update_pairs, "Find Pairs", B2HexColor.b2_colorMediumSlateBlue, true); - B2ArenaAllocator alloc = world.arena; + B2StackAllocator alloc = world.stack; // todo these could be in the step context - bp.moveResults = b2AllocateArenaItem(alloc, moveCount, "move results"); + bp.moveResults = b2StackAlloc(alloc, moveCount, "move results"); // This capacity can be exceeded if there are many overlapping pairs (e.g. all shapes at the origin) bp.movePairCapacity = 32 * moveCount; - bp.movePairs = b2AllocateArenaItem(alloc, bp.movePairCapacity, "move pairs"); + bp.movePairs = b2StackAlloc(alloc, bp.movePairCapacity, "move pairs"); b2AtomicStoreInt(ref bp.movePairIndex, 0); #if B2_SNOOP_TABLE_COUNTERS @@ -518,9 +528,9 @@ public static void b2UpdateBroadPhasePairs(B2World world) b2Array_Clear(ref bp.moveArray); b2ClearSet(ref bp.moveSet); - b2FreeArenaItem(alloc, bp.movePairs); + b2StackFree(alloc, bp.movePairs); bp.movePairs = null; - b2FreeArenaItem(alloc, bp.moveResults); + b2StackFree(alloc, bp.moveResults); bp.moveResults = null; b2ValidateSolverSets(world); @@ -542,12 +552,6 @@ public static bool b2BroadPhase_TestOverlap(B2BroadPhase bp, int proxyKeyA, int return b2AABB_Overlaps(aabbA, aabbB); } - internal static void b2BroadPhase_RebuildTrees(B2BroadPhase bp) - { - b2DynamicTree_Rebuild(bp.trees[(int)B2BodyType.b2_dynamicBody], false); - b2DynamicTree_Rebuild(bp.trees[(int)B2BodyType.b2_kinematicBody], false); - } - public static int b2BroadPhase_GetShapeIndex(B2BroadPhase bp, int proxyKey) { int typeIndex = (int)B2_PROXY_TYPE(proxyKey); diff --git a/src/Box2D.NET/B2Capacity.cs b/src/Box2D.NET/B2Capacity.cs new file mode 100644 index 00000000..391c7506 --- /dev/null +++ b/src/Box2D.NET/B2Capacity.cs @@ -0,0 +1,27 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +namespace Box2D.NET +{ + /// Optional world capacities that can be used to avoid run-time allocations. + /// @see b2World_GetMaxCapacity + /// @ingroup world + public struct B2Capacity + { + /// Number of expected static shapes. + public int staticShapeCount; + + /// Number of expected dynamic and kinematic shapes. + public int dynamicShapeCount; + + /// Number of expected static bodies. + public int staticBodyCount; + + /// Number of expected dynamic and kinematic bodies. + public int dynamicBodyCount; + + /// Number of expected contacts. + public int contactCount; + } +} diff --git a/src/Box2D.NET/B2ConstraintGraphs.cs b/src/Box2D.NET/B2ConstraintGraphs.cs index e9f795f1..457a8393 100644 --- a/src/Box2D.NET/B2ConstraintGraphs.cs +++ b/src/Box2D.NET/B2ConstraintGraphs.cs @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT @@ -12,10 +12,10 @@ // cause horrible cache stalls. To make this feasible I would need a way to block these writes. // todo should be possible to branch on the scatters to avoid writing to kinematic bodies -// TODO: @ikpil, check // This is used for debugging by making all constraints be assigned to overflow. - -#define B2_FORCE_OVERFLOW +// C uses "#define B2_FORCE_OVERFLOW 0" and tests "== 0". The C# preprocessor only has +// boolean symbols, so leaving this undefined matches the C default of 0. +//#define B2_FORCE_OVERFLOW using static Box2D.NET.B2Arrays; using static Box2D.NET.B2Constants; @@ -36,7 +36,7 @@ public static class B2ConstraintGraphs public const int B2_DYNAMIC_COLOR_COUNT = (B2_GRAPH_COLOR_COUNT - 4); - public static void b2CreateGraph(ref B2ConstraintGraph graph, int bodyCapacity) + public static void b2CreateGraph(ref B2ConstraintGraph graph, in B2Capacity capacity) { 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"); @@ -45,7 +45,7 @@ public static void b2CreateGraph(ref B2ConstraintGraph graph, int bodyCapacity) graph = new B2ConstraintGraph(); graph.colors = new B2GraphColor[B2_GRAPH_COLOR_COUNT]; - bodyCapacity = b2MaxInt(bodyCapacity, 8); + int bodyCapacity = b2MaxInt(capacity.staticBodyCount + capacity.dynamicBodyCount, 16); // Initialize graph color bit set. // No bitset for overflow color. @@ -53,19 +53,9 @@ public static void b2CreateGraph(ref B2ConstraintGraph graph, int bodyCapacity) { ref B2GraphColor color = ref graph.colors[i]; color.bodySet = b2CreateBitSet(bodyCapacity); - color.contactSims = b2Array_Create(); - color.jointSims = b2Array_Create(); - b2SetBitCountAndClear(ref color.bodySet, bodyCapacity); - } - - // @ikpil, for dummy - for (int i = B2_OVERFLOW_INDEX; i < B2_GRAPH_COLOR_COUNT; ++i) - { - var color = graph.colors[i]; - color.bodySet = new B2BitSet(); - color.contactSims = b2Array_Create(); - color.jointSims = b2Array_Create(); + + b2Array_Reserve(ref color.contactSims, 16); } } @@ -106,7 +96,7 @@ internal static void b2AddContactToGraph(B2World world, B2ContactSim contactSim, B2BodyType typeB = bodyB.type; B2_ASSERT(typeA == B2BodyType.b2_dynamicBody || typeB == B2BodyType.b2_dynamicBody); -#if B2_FORCE_OVERFLOW +#if !B2_FORCE_OVERFLOW if (typeA == B2BodyType.b2_dynamicBody && typeB == B2BodyType.b2_dynamicBody) { // Dynamic constraint colors cannot encroach on colors reserved for static constraints @@ -162,7 +152,7 @@ internal static void b2AddContactToGraph(B2World world, B2ContactSim contactSim, contact.colorIndex = colorIndex; contact.localIndex = color0.contactSims.count; - ref B2ContactSim newContact = ref b2Array_Add(ref color0.contactSims); + ref B2ContactSim newContact = ref b2Array_Emplace(ref color0.contactSims); //memcpy( newContact, contactSim, sizeof( b2ContactSim ) ); newContact.CopyFrom(contactSim); @@ -237,13 +227,13 @@ internal static void b2RemoveContactFromGraph(B2World world, int bodyIdA, int bo } } - // Contacts are always created as non-touching. They get moved into the constraint - // graph once they are found to be touching. + // Notice that a joint cannot share the same color as a contact between the same two bodies. This means I can solve contacts and + // joints in parallel with each other within each color. static int b2AssignJointColor(ref B2ConstraintGraph graph, int bodyIdA, int bodyIdB, B2BodyType typeA, B2BodyType typeB) { B2_ASSERT(typeA == B2BodyType.b2_dynamicBody || typeB == B2BodyType.b2_dynamicBody); -#if B2_FORCE_OVERFLOW +#if !B2_FORCE_OVERFLOW if (typeA == B2BodyType.b2_dynamicBody && typeB == B2BodyType.b2_dynamicBody) { // Dynamic constraint colors cannot encroach on colors reserved for static constraints @@ -308,7 +298,7 @@ internal static ref B2JointSim b2CreateJointInGraph(B2World world, B2Joint joint int colorIndex = b2AssignJointColor(ref graph, bodyIdA, bodyIdB, bodyA.type, bodyB.type); - ref B2JointSim jointSim = ref b2Array_Add(ref graph.colors[colorIndex].jointSims); + ref B2JointSim jointSim = ref b2Array_Emplace(ref graph.colors[colorIndex].jointSims); //memset( jointSim, 0, sizeof( b2JointSim ) ); jointSim.Clear(); @@ -354,10 +344,19 @@ internal static void b2RemoveJointFromGraph(B2World world, int bodyIdA, int body internal static readonly B2HexColor[] b2_graphColors = new B2HexColor[] { - 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, + B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorLimeGreen, B2HexColor.b2_colorSpringGreen, + B2HexColor.b2_colorAqua, B2HexColor.b2_colorDodgerBlue, B2HexColor.b2_colorBlueViolet, B2HexColor.b2_colorMagenta, B2HexColor.b2_colorDeepPink, + B2HexColor.b2_colorCrimson, B2HexColor.b2_colorCoral, B2HexColor.b2_colorGold, B2HexColor.b2_colorGreenYellow, B2HexColor.b2_colorMediumSeaGreen, + B2HexColor.b2_colorTurquoise, B2HexColor.b2_colorDeepSkyBlue, B2HexColor.b2_colorCornflowerBlue, B2HexColor.b2_colorMediumSlateBlue, B2HexColor.b2_colorMediumOrchid, + B2HexColor.b2_colorHotPink, B2HexColor.b2_colorTomato, B2HexColor.b2_colorKhaki, B2HexColor.b2_colorSilver, }; + + /// Get the visualization color assigned to a constraint graph color slot. The last index + /// (B2_GRAPH_COLOR_COUNT - 1) is the overflow color. + public static B2HexColor b2GetGraphColor(int index) + { + B2_ASSERT(0 <= index && index < B2_GRAPH_COLOR_COUNT); + return b2_graphColors[index]; + } } -} \ No newline at end of file +} diff --git a/src/Box2D.NET/B2ContactPrepareSpan.cs b/src/Box2D.NET/B2ContactPrepareSpan.cs new file mode 100644 index 00000000..56f9cb44 --- /dev/null +++ b/src/Box2D.NET/B2ContactPrepareSpan.cs @@ -0,0 +1,18 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +namespace Box2D.NET +{ + // Prepare/store run as a flat parallel-for over the whole wide-constraint + // range. Each span maps a slice of that range back to the owning color's + // contacts so workers can decode flat wide-slot indices without touching + // graph state. The spans array has one entry per active color plus a sentinel + // whose start == wideContactCount. + public struct B2ContactPrepareSpan + { + public int start; + public int count; + public B2ContactSim[] contacts; + } +} diff --git a/src/Box2D.NET/B2ContactSolvers.cs b/src/Box2D.NET/B2ContactSolvers.cs index d04ff445..803cf695 100644 --- a/src/Box2D.NET/B2ContactSolvers.cs +++ b/src/Box2D.NET/B2ContactSolvers.cs @@ -14,6 +14,7 @@ using static Box2D.NET.B2ConstraintGraphs; using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Solvers; +using static Box2D.NET.B2BitSets; namespace Box2D.NET { @@ -27,7 +28,7 @@ public static class B2ContactSolvers // s(t) = s0 + dot(cB0 + dpB + rot(dqB, rB0) - cA0 - dpA - rot(dqA, rA0), normal) // s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal) // s_base = s0 + dot(cB0 - cA0, normal) - internal static void b2PrepareOverflowContacts(B2StepContext context) + internal static void b2PrepareContacts_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.prepare_overflow_contact, "Prepare Overflow Contact", B2HexColor.b2_colorYellow, true); @@ -166,7 +167,7 @@ internal static void b2PrepareOverflowContacts(B2StepContext context) b2TracyCZoneEnd(B2TracyCZone.prepare_overflow_contact); } - internal static void b2WarmStartOverflowContacts(B2StepContext context) + internal static void b2WarmStartContacts_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.warmstart_overflow_contact, "WarmStart Overflow Contact", B2HexColor.b2_colorDarkOrange, true); @@ -243,7 +244,7 @@ internal static void b2WarmStartOverflowContacts(B2StepContext context) b2TracyCZoneEnd(B2TracyCZone.warmstart_overflow_contact); } - internal static void b2SolveOverflowContacts(B2StepContext context, bool useBias) + internal static void b2SolveContacts_Overflow(B2StepContext context, bool useBias) { b2TracyCZoneNC(B2TracyCZone.solve_contact, "Solve Overflow Contact", B2HexColor.b2_colorAliceBlue, true); @@ -348,51 +349,54 @@ internal static void b2SolveOverflowContacts(B2StepContext context, bool useBias wB += iB * b2Cross(rB, P); } - // Friction - for (int j = 0; j < pointCount; ++j) + if (useBias == false) { - ref B2ContactConstraintPoint cp = ref constraint.points[j]; + // Friction + for (int j = 0; j < pointCount; ++j) + { + ref B2ContactConstraintPoint cp = ref constraint.points[j]; - // fixed anchor points - B2Vec2 rA = cp.anchorA; - B2Vec2 rB = cp.anchorB; + // fixed anchor points + B2Vec2 rA = cp.anchorA; + B2Vec2 rB = cp.anchorB; - // relative tangent velocity at contact - B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB)); - B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA)); + // relative tangent velocity at contact + B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB)); + B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA)); - // vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent) - // = dot(vrB - vrA, tangent) - (sA + sB) + // vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent) + // = dot(vrB - vrA, tangent) - (sA + sB) - float vt = b2Dot(b2Sub(vrB, vrA), tangent) - constraint.tangentSpeed; + float vt = b2Dot(b2Sub(vrB, vrA), tangent) - constraint.tangentSpeed; - // incremental tangent impulse - float impulse = cp.tangentMass * (-vt); + // incremental tangent impulse + float impulse = cp.tangentMass * (-vt); - // clamp the accumulated force - float maxFriction = friction * cp.normalImpulse; - float newImpulse = b2ClampFloat(cp.tangentImpulse + impulse, -maxFriction, maxFriction); - impulse = newImpulse - cp.tangentImpulse; - cp.tangentImpulse = newImpulse; + // clamp the accumulated force + float maxFriction = friction * cp.normalImpulse; + float newImpulse = b2ClampFloat(cp.tangentImpulse + impulse, -maxFriction, maxFriction); + impulse = newImpulse - cp.tangentImpulse; + cp.tangentImpulse = newImpulse; - // apply tangent impulse - B2Vec2 P = b2MulSV(impulse, tangent); - vA = b2MulSub(vA, mA, P); - wA -= iA * b2Cross(rA, P); - vB = b2MulAdd(vB, mB, P); - wB += iB * b2Cross(rB, P); - } + // apply tangent impulse + B2Vec2 P = b2MulSV(impulse, tangent); + vA = b2MulSub(vA, mA, P); + wA -= iA * b2Cross(rA, P); + vB = b2MulAdd(vB, mB, P); + wB += iB * b2Cross(rB, P); + } - // Rolling resistance - { - float deltaLambda = -constraint.rollingMass * (wB - wA); - float lambda = constraint.rollingImpulse; - float maxLambda = constraint.rollingResistance * totalNormalImpulse; - constraint.rollingImpulse = b2ClampFloat(lambda + deltaLambda, -maxLambda, maxLambda); - deltaLambda = constraint.rollingImpulse - lambda; - - wA -= iA * deltaLambda; - wB += iB * deltaLambda; + // Rolling resistance + { + float deltaLambda = -constraint.rollingMass * (wB - wA); + float lambda = constraint.rollingImpulse; + float maxLambda = constraint.rollingResistance * totalNormalImpulse; + constraint.rollingImpulse = b2ClampFloat(lambda + deltaLambda, -maxLambda, maxLambda); + deltaLambda = constraint.rollingImpulse - lambda; + + wA -= iA * deltaLambda; + wB += iB * deltaLambda; + } } if (0 != (stateA.flags & (uint)B2BodyFlags.b2_dynamicFlag)) @@ -411,7 +415,7 @@ internal static void b2SolveOverflowContacts(B2StepContext context, bool useBias b2TracyCZoneEnd(B2TracyCZone.solve_contact); } - internal static void b2ApplyOverflowRestitution(B2StepContext context) + internal static void b2ApplyRestitution_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.overflow_resitution, "Overflow Restitution", B2HexColor.b2_colorViolet, true); @@ -517,7 +521,7 @@ internal static void b2ApplyOverflowRestitution(B2StepContext context) b2TracyCZoneEnd(B2TracyCZone.overflow_resitution); } - internal static void b2StoreOverflowImpulses(B2StepContext context) + internal static void b2StoreImpulses_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.store_impulses, "Store", B2HexColor.b2_colorFireBrick, true); @@ -589,6 +593,7 @@ public static Vector b2MaxW(Vector a, Vector b) { return Vector.Max(a, b); } + // a = clamp(a, -b, b) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector b2SymClampW(Vector a, Vector b) @@ -604,10 +609,9 @@ public static Vector b2OrW(Vector a, Vector b) Vector aZeroMask = Vector.Equals(a, Vector.Zero); Vector bZeroMask = Vector.Equals(b, Vector.Zero); - // a 또는 b의 해당 요소가 0이 아니면 참인 마스크 생성 - Vector zeroMask = aZeroMask | bZeroMask; + // Zero only where both lanes are zero, matching a != 0 || b != 0 + Vector zeroMask = aZeroMask & bZeroMask; - // 마스크가 참이면 1.0f, 거짓이면 0.0f 선택 return Vector.ConditionalSelect(zeroMask, Vector.Zero, Vector.One); } @@ -624,6 +628,7 @@ public static Vector b2EqualsW(Vector a, Vector b) var mask = Vector.Equals(a, b); return Vector.ConditionalSelect(mask, Vector.One, Vector.Zero); } + // component-wise returns mask ? b : a [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Vector b2BlendW(Vector a, Vector b, Vector mask) @@ -703,6 +708,7 @@ public static B2FloatW b2MaxW(in B2FloatW a, in B2FloatW b) a.W >= b.W ? a.W : b.W ); } + // a = clamp(a, -b, b) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2FloatW b2SymClampW(in B2FloatW a, in B2FloatW b) @@ -755,6 +761,7 @@ public static bool b2AllZeroW(in B2FloatW a) { return a.X == 0.0f && a.Y == 0.0f && a.Z == 0.0f && a.W == 0.0f; } + // component-wise returns mask ? b : a [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2FloatW b2BlendW(in B2FloatW a, in B2FloatW b, in B2FloatW mask) @@ -1163,11 +1170,12 @@ static void b2ScatterBodies( b2BodyState* states, int* indices, const b2BodyStat #else - // This is a load and 8x8 transpose + // This is a load and transpose internal static B2BodyStateW b2GatherBodies(ReadOnlySpan states, ReadOnlySpan indices) { B2_VALIDATE(indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0); + // Read-only here, so alias the shared instance instead of allocating a copy per call. B2BodyState identity = b2_identityBodyState; // zero means null @@ -1195,7 +1203,6 @@ internal static B2BodyStateW b2GatherBodies(ReadOnlySpan states, Re } // This writes only the velocities back to the solver bodies - // https://developer.arm.com/documentation/102107a/0100/Floating-point-4x4-matrix-transposition internal static void b2ScatterBodies(ReadOnlySpan states, ReadOnlySpan indices, ref B2BodyStateW simdBody) { B2_VALIDATE(indices[0] >= 0 && indices[1] >= 0 && indices[2] >= 0 && indices[3] >= 0); @@ -1243,17 +1250,23 @@ internal static void b2ScatterBodies(ReadOnlySpan states, ReadOnlyS // Note: Dirk suggested preparing contacts in the narrow phase. I tried this but it made Box2D slower. // The contact preparation is extremely fast in Box2D due to the data layout (b2ContactSim). - internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepContext context) + // + // Runs as a flat parallel-for over the whole wide constraint range. Per-color contact sims + // are looked up through the prepareSpans cursor rather than the block's colorIndex, so + // blocks can be uniformly sized without honoring color boundaries. Dead lanes in each + // color's tail wide slot were all zeroed in solver setup. + internal static void b2PrepareContactsTask(B2SolverBlock block, B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.prepare_contact, "Prepare Contact", B2HexColor.b2_colorYellow, true); B2World world = context.world; - Span contacts = context.contacts; - Span constraints = context.wideContactConstraints; - B2BodyState[] awakeStates = context.states; + B2BodyState[] states = context.states; #if DEBUG B2Body[] bodies = world.bodies.data; #endif + B2ContactPrepareSpan[] spans = context.contactPrepareSpans; + Span wideBase = context.wideContactConstraints; + // Stiffer for static contacts to avoid bodies getting pushed through the ground B2Softness contactSoftness = context.contactSoftness; B2Softness staticSoftness = context.staticSoftness; @@ -1261,17 +1274,46 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC float warmStartScale = world.enableWarmStarting ? 1.0f : 0.0f; - for (int i = startIndex; i < endIndex; ++i) + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while (spans[colorIndex + 1].start <= wideIndex) + { + colorIndex += 1; + } + + // Loop over block + while (wideIndex < endWideIndex) { - ref B2ContactConstraintWide constraint = ref constraints[i]; + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b2MinInt(spans[colorIndex + 1].start, endWideIndex); + int colorContactCount = spans[colorIndex].count; + B2ContactSim[] contacts = spans[colorIndex].contacts; - for (int j = 0; j < B2_SIMD_WIDTH; ++j) +#if DEBUG + int expectedWide = colorContactCount > 0 ? ((colorContactCount - 1) >> B2_SIMD_SHIFT) + 1 : 0; + B2_ASSERT(spans[colorIndex + 1].start - spans[colorIndex].start == expectedWide); +#endif + + // Loop over color + for (; wideIndex < colorWideEndIndex; ++wideIndex) { - B2ContactSim contactSim = contacts[B2_SIMD_WIDTH * i + j]; + ref B2ContactConstraintWide constraint = ref wideBase[wideIndex]; + int localWideIndex = wideIndex - colorWideStart; - if (contactSim != null) + for (int lane = 0; lane < B2_SIMD_WIDTH; ++lane) { - ref B2Manifold manifold = ref contactSim.manifold; + int contactIndex = B2_SIMD_WIDTH * localWideIndex + lane; + if (contactIndex >= colorContactCount) + { + // Remainder lanes were zeroed in solver setup. + break; + } + + B2ContactSim contactSim = contacts[contactIndex]; + ref readonly B2Manifold manifold = ref contactSim.manifold; int indexA = contactSim.bodySimIndexA; int indexB = contactSim.bodySimIndexB; @@ -1286,8 +1328,8 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC B2_ASSERT(indexB == validIndexB); #endif // 0 for null - constraint.indexA[j] = indexA + 1; - constraint.indexB[j] = indexB + 1; + constraint.indexA[lane] = indexA + 1; + constraint.indexB[lane] = indexB + 1; B2Vec2 vA = b2Vec2_zero; float wA = 0.0f; @@ -1295,7 +1337,7 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC float iA = contactSim.invIA; if (indexA != B2_NULL_INDEX) { - B2BodyState stateA = awakeStates[indexA]; + B2BodyState stateA = states[indexA]; vA = stateA.linearVelocity; wA = stateA.angularVelocity; } @@ -1306,20 +1348,19 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC float iB = contactSim.invIB; if (indexB != B2_NULL_INDEX) { - B2BodyState stateB = awakeStates[indexB]; + B2BodyState stateB = states[indexB]; vB = stateB.linearVelocity; wB = stateB.angularVelocity; } - // TODO: @ikpil, check - constraint.invMassA[j] = mA; - constraint.invMassB[j] = mB; - constraint.invIA[j] = iA; - constraint.invIB[j] = iB; + constraint.invMassA[lane] = mA; + constraint.invMassB[lane] = mB; + constraint.invIA[lane] = iA; + constraint.invIB[lane] = iB; { float k = iA + iB; - constraint.rollingMass[j] = k > 0.0f ? 1.0f / k : 0.0f; + constraint.rollingMass[lane] = k > 0.0f ? 1.0f / k : 0.0f; } B2Softness soft = contactSoftness; @@ -1345,18 +1386,18 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC } B2Vec2 normal = manifold.normal; - constraint.normal.X[j] = normal.X; - constraint.normal.Y[j] = normal.Y; + constraint.normal.X[lane] = normal.X; + constraint.normal.Y[lane] = normal.Y; - constraint.friction[j] = contactSim.friction; - constraint.tangentSpeed[j] = contactSim.tangentSpeed; - constraint.restitution[j] = contactSim.restitution; - constraint.rollingResistance[j] = contactSim.rollingResistance; - constraint.rollingImpulse[j] = warmStartScale * manifold.rollingImpulse; + constraint.friction[lane] = contactSim.friction; + constraint.tangentSpeed[lane] = contactSim.tangentSpeed; + constraint.restitution[lane] = contactSim.restitution; + constraint.rollingResistance[lane] = contactSim.rollingResistance; + constraint.rollingImpulse[lane] = warmStartScale * manifold.rollingImpulse; - constraint.biasRate[j] = soft.biasRate; - constraint.massScale[j] = soft.massScale; - constraint.impulseScale[j] = soft.impulseScale; + constraint.biasRate[lane] = soft.biasRate; + constraint.massScale[lane] = soft.massScale; + constraint.impulseScale[lane] = soft.impulseScale; B2Vec2 tangent = b2RightPerp(normal); @@ -1366,31 +1407,31 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC B2Vec2 rA = mp.anchorA; B2Vec2 rB = mp.anchorB; - constraint.anchorA1.X[j] = rA.X; - constraint.anchorA1.Y[j] = rA.Y; - constraint.anchorB1.X[j] = rB.X; - constraint.anchorB1.Y[j] = rB.Y; + constraint.anchorA1.X[lane] = rA.X; + constraint.anchorA1.Y[lane] = rA.Y; + constraint.anchorB1.X[lane] = rB.X; + constraint.anchorB1.Y[lane] = rB.Y; - constraint.baseSeparation1[j] = mp.separation - b2Dot(b2Sub(rB, rA), normal); + constraint.baseSeparation1[lane] = mp.separation - b2Dot(b2Sub(rB, rA), normal); - constraint.normalImpulse1[j] = warmStartScale * mp.normalImpulse; - constraint.tangentImpulse1[j] = warmStartScale * mp.tangentImpulse; - constraint.totalNormalImpulse1[j] = 0.0f; + constraint.normalImpulse1[lane] = warmStartScale * mp.normalImpulse; + constraint.tangentImpulse1[lane] = warmStartScale * mp.tangentImpulse; + constraint.totalNormalImpulse1[lane] = 0.0f; float rnA = b2Cross(rA, normal); float rnB = b2Cross(rB, normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - constraint.normalMass1[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + constraint.normalMass1[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; float rtA = b2Cross(rA, tangent); float rtB = b2Cross(rB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; - constraint.tangentMass1[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + constraint.tangentMass1[lane] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // relative velocity for restitution B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA)); B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB)); - constraint.relativeVelocity1[j] = b2Dot(normal, b2Sub(vrB, vrA)); + constraint.relativeVelocity1[lane] = b2Dot(normal, b2Sub(vrB, vrA)); } int pointCount = manifold.pointCount; @@ -1403,113 +1444,66 @@ internal static void b2PrepareContactsTask(int startIndex, int endIndex, B2StepC B2Vec2 rA = mp.anchorA; B2Vec2 rB = mp.anchorB; - constraint.anchorA2.X[j] = rA.X; - constraint.anchorA2.Y[j] = rA.Y; - constraint.anchorB2.X[j] = rB.X; - constraint.anchorB2.Y[j] = rB.Y; + constraint.anchorA2.X[lane] = rA.X; + constraint.anchorA2.Y[lane] = rA.Y; + constraint.anchorB2.X[lane] = rB.X; + constraint.anchorB2.Y[lane] = rB.Y; - constraint.baseSeparation2[j] = mp.separation - b2Dot(b2Sub(rB, rA), normal); + constraint.baseSeparation2[lane] = mp.separation - b2Dot(b2Sub(rB, rA), normal); - constraint.normalImpulse2[j] = warmStartScale * mp.normalImpulse; - constraint.tangentImpulse2[j] = warmStartScale * mp.tangentImpulse; - constraint.totalNormalImpulse2[j] = 0.0f; + constraint.normalImpulse2[lane] = warmStartScale * mp.normalImpulse; + constraint.tangentImpulse2[lane] = warmStartScale * mp.tangentImpulse; + constraint.totalNormalImpulse2[lane] = 0.0f; float rnA = b2Cross(rA, normal); float rnB = b2Cross(rB, normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - constraint.normalMass2[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + constraint.normalMass2[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; float rtA = b2Cross(rA, tangent); float rtB = b2Cross(rB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; - constraint.tangentMass2[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + constraint.tangentMass2[lane] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // relative velocity for restitution B2Vec2 vrA = b2Add(vA, b2CrossSV(wA, rA)); B2Vec2 vrB = b2Add(vB, b2CrossSV(wB, rB)); - constraint.relativeVelocity2[j] = b2Dot(normal, b2Sub(vrB, vrA)); + constraint.relativeVelocity2[lane] = b2Dot(normal, b2Sub(vrB, vrA)); } else { // dummy data that has no effect - constraint.baseSeparation2[j] = 0.0f; - constraint.normalImpulse2[j] = 0.0f; - constraint.tangentImpulse2[j] = 0.0f; - constraint.totalNormalImpulse2[j] = 0.0f; - constraint.anchorA2.X[j] = 0.0f; - constraint.anchorA2.Y[j] = 0.0f; - constraint.anchorB2.X[j] = 0.0f; - constraint.anchorB2.Y[j] = 0.0f; - constraint.normalMass2[j] = 0.0f; - constraint.tangentMass2[j] = 0.0f; - constraint.relativeVelocity2[j] = 0.0f; + constraint.baseSeparation2[lane] = 0.0f; + constraint.normalImpulse2[lane] = 0.0f; + constraint.tangentImpulse2[lane] = 0.0f; + constraint.totalNormalImpulse2[lane] = 0.0f; + constraint.anchorA2.X[lane] = 0.0f; + constraint.anchorA2.Y[lane] = 0.0f; + constraint.anchorB2.X[lane] = 0.0f; + constraint.anchorB2.Y[lane] = 0.0f; + constraint.normalMass2[lane] = 0.0f; + constraint.tangentMass2[lane] = 0.0f; + constraint.relativeVelocity2[lane] = 0.0f; } } - else - { - // Wide remainder - // todo set to zero with memset - - // Zero for null - constraint.indexA[j] = 0; - constraint.indexB[j] = 0; - - constraint.invMassA[j] = 0.0f; - constraint.invMassB[j] = 0.0f; - constraint.invIA[j] = 0.0f; - constraint.invIB[j] = 0.0f; - - constraint.normal.X[j] = 0.0f; - constraint.normal.Y[j] = 0.0f; - constraint.friction[j] = 0.0f; - constraint.tangentSpeed[j] = 0.0f; - constraint.rollingResistance[j] = 0.0f; - constraint.rollingMass[j] = 0.0f; - constraint.rollingImpulse[j] = 0.0f; - constraint.biasRate[j] = 0.0f; - constraint.massScale[j] = 0.0f; - constraint.impulseScale[j] = 0.0f; - - constraint.anchorA1.X[j] = 0.0f; - constraint.anchorA1.Y[j] = 0.0f; - constraint.anchorB1.X[j] = 0.0f; - constraint.anchorB1.Y[j] = 0.0f; - constraint.baseSeparation1[j] = 0.0f; - constraint.normalImpulse1[j] = 0.0f; - constraint.tangentImpulse1[j] = 0.0f; - constraint.totalNormalImpulse1[j] = 0.0f; - constraint.normalMass1[j] = 0.0f; - constraint.tangentMass1[j] = 0.0f; - - constraint.anchorA2.X[j] = 0.0f; - constraint.anchorA2.Y[j] = 0.0f; - constraint.anchorB2.X[j] = 0.0f; - constraint.anchorB2.Y[j] = 0.0f; - constraint.baseSeparation2[j] = 0.0f; - constraint.normalImpulse2[j] = 0.0f; - constraint.tangentImpulse2[j] = 0.0f; - constraint.totalNormalImpulse2[j] = 0.0f; - constraint.normalMass2[j] = 0.0f; - constraint.tangentMass2[j] = 0.0f; - - constraint.restitution[j] = 0.0f; - constraint.relativeVelocity1[j] = 0.0f; - constraint.relativeVelocity2[j] = 0.0f; - } } + + // Advance to next color + colorIndex += 1; } b2TracyCZoneEnd(B2TracyCZone.prepare_contact); } - internal static void b2WarmStartContactsTask(int startIndex, int endIndex, B2StepContext context, int colorIndex) + internal static void b2WarmStartContactsTask(B2SolverBlock block, B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.warm_start_contact, "Warm Start", B2HexColor.b2_colorGreen, true); B2BodyState[] states = context.states; + int colorIndex = block.colorIndex; Span constraints = context.graph.colors[colorIndex].wideConstraints; - for (int i = startIndex; i < endIndex; ++i) + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) { ref B2ContactConstraintWide c = ref constraints[i]; B2BodyStateW bA = b2GatherBodies(states, c.indexA.AsSpan()); @@ -1564,18 +1558,19 @@ internal static void b2WarmStartContactsTask(int startIndex, int endIndex, B2Ste b2TracyCZoneEnd(B2TracyCZone.warm_start_contact); } - internal static void b2SolveContactsTask(int startIndex, int endIndex, B2StepContext context, int colorIndex, bool useBias) + internal static void b2SolveContactsTask(B2SolverBlock block, B2StepContext context, bool useBias) { b2TracyCZoneNC(B2TracyCZone.solve_contact, "Solve Contact", B2HexColor.b2_colorAliceBlue, true); B2BodyState[] states = context.states; + int colorIndex = block.colorIndex; ref readonly B2GraphColor color = ref context.graph.colors[colorIndex]; Span constraints = color.wideConstraints; B2FloatW inv_h = b2SplatW(context.inv_h); B2FloatW contactSpeed = b2SplatW(-context.world.contactSpeed); B2FloatW oneW = b2SplatW(1.0f); - for (int wideIndex = startIndex; wideIndex < endIndex; ++wideIndex) + for (int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex) { ref B2ContactConstraintWide c = ref constraints[wideIndex]; @@ -1602,7 +1597,7 @@ internal static void b2SolveContactsTask(int startIndex, int endIndex, B2StepCon // point1 non-penetration constraint { - // fixed anchors for Jacobians + // Fixed anchors for impulses B2Vec2W rA = c.anchorA1; B2Vec2W rB = c.anchorB1; @@ -1707,93 +1702,97 @@ internal static void b2SolveContactsTask(int startIndex, int endIndex, B2StepCon bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px))); } - B2FloatW tangentX = c.normal.Y; - B2FloatW tangentY = b2SubW(b2ZeroW(), c.normal.X); - - // point 1 friction constraint + if (useBias == false) { - // fixed anchors for Jacobians - B2Vec2W rA = c.anchorA1; - B2Vec2W rB = c.anchorB1; - - // Relative velocity at contact - B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y))); - B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X))); - B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY)); - - // Tangent speed (conveyor belt) - vt = b2SubW(vt, c.tangentSpeed); - - // Compute tangent force - B2FloatW negImpulse = b2MulW(c.tangentMass1, vt); - - // Clamp the accumulated force - B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse1); - B2FloatW newImpulse = b2SubW(c.tangentImpulse1, negImpulse); - newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction)); - B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse1); - c.tangentImpulse1 = newImpulse; - - // Apply contact impulse - B2FloatW Px = b2MulW(impulse, tangentX); - B2FloatW Py = b2MulW(impulse, tangentY); - - bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); - bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); - bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px))); - - bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px); - bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py); - bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px))); - } - - // second point friction constraint - { - // fixed anchors for Jacobians - B2Vec2W rA = c.anchorA2; - B2Vec2W rB = c.anchorB2; - - // Relative velocity at contact - B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y))); - B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X))); - B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY)); - - // Tangent speed (conveyor belt) - vt = b2SubW(vt, c.tangentSpeed); - - // Compute tangent force - B2FloatW negImpulse = b2MulW(c.tangentMass2, vt); - - // Clamp the accumulated force - B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse2); - B2FloatW newImpulse = b2SubW(c.tangentImpulse2, negImpulse); - newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction)); - B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse2); - c.tangentImpulse2 = newImpulse; - - // Apply contact impulse - B2FloatW Px = b2MulW(impulse, tangentX); - B2FloatW Py = b2MulW(impulse, tangentY); + // Rolling resistance + if (b2AllZeroW(c.rollingResistance) == false) + { + B2FloatW deltaLambda = b2MulW(c.rollingMass, b2SubW(bA.w, bB.w)); + B2FloatW lambda = c.rollingImpulse; + B2FloatW maxLambda = b2MulW(c.rollingResistance, totalNormalImpulse); + c.rollingImpulse = b2SymClampW(b2AddW(lambda, deltaLambda), maxLambda); + deltaLambda = b2SubW(c.rollingImpulse, lambda); + + bA.w = b2MulSubW(bA.w, c.invIA, deltaLambda); + bB.w = b2MulAddW(bB.w, c.invIB, deltaLambda); + } - bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); - bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); - bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px))); + B2FloatW tangentX = c.normal.Y; + B2FloatW tangentY = b2SubW(b2ZeroW(), c.normal.X); - bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px); - bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py); - bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px))); - } + // point 1 friction constraint + { + // Fixed anchor points for applying impulses + B2Vec2W rA = c.anchorA1; + B2Vec2W rB = c.anchorB1; + + // Relative velocity at contact + B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y))); + B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X))); + B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY)); + + // Tangent speed (conveyor belt) + vt = b2SubW(vt, c.tangentSpeed); + + // Compute tangent force + B2FloatW negImpulse = b2MulW(c.tangentMass1, vt); + + // Clamp the accumulated force + B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse1); + B2FloatW newImpulse = b2SubW(c.tangentImpulse1, negImpulse); + newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction)); + B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse1); + c.tangentImpulse1 = newImpulse; + + // Apply contact impulse + B2FloatW Px = b2MulW(impulse, tangentX); + B2FloatW Py = b2MulW(impulse, tangentY); + + bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); + bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); + bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px))); + + bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px); + bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py); + bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px))); + } - // Rolling resistance - { - B2FloatW deltaLambda = b2MulW(c.rollingMass, b2SubW(bA.w, bB.w)); - B2FloatW lambda = c.rollingImpulse; - B2FloatW maxLambda = b2MulW(c.rollingResistance, totalNormalImpulse); - c.rollingImpulse = b2SymClampW(b2AddW(lambda, deltaLambda), maxLambda); - deltaLambda = b2SubW(c.rollingImpulse, lambda); - - bA.w = b2MulSubW(bA.w, c.invIA, deltaLambda); - bB.w = b2MulAddW(bB.w, c.invIB, deltaLambda); + // second point friction constraint + { + // fixed anchors for Jacobians + B2Vec2W rA = c.anchorA2; + B2Vec2W rB = c.anchorB2; + + // Relative velocity at contact + B2FloatW dvx = b2SubW(b2SubW(bB.v.X, b2MulW(bB.w, rB.Y)), b2SubW(bA.v.X, b2MulW(bA.w, rA.Y))); + B2FloatW dvy = b2SubW(b2AddW(bB.v.Y, b2MulW(bB.w, rB.X)), b2AddW(bA.v.Y, b2MulW(bA.w, rA.X))); + B2FloatW vt = b2AddW(b2MulW(dvx, tangentX), b2MulW(dvy, tangentY)); + + // Tangent speed (conveyor belt) + vt = b2SubW(vt, c.tangentSpeed); + + // Compute tangent force + B2FloatW negImpulse = b2MulW(c.tangentMass2, vt); + + // Clamp the accumulated force + B2FloatW maxFriction = b2MulW(c.friction, c.normalImpulse2); + B2FloatW newImpulse = b2SubW(c.tangentImpulse2, negImpulse); + newImpulse = b2MaxW(b2SubW(b2ZeroW(), maxFriction), b2MinW(newImpulse, maxFriction)); + B2FloatW impulse = b2SubW(newImpulse, c.tangentImpulse2); + c.tangentImpulse2 = newImpulse; + + // Apply contact impulse + B2FloatW Px = b2MulW(impulse, tangentX); + B2FloatW Py = b2MulW(impulse, tangentY); + + bA.v.X = b2MulSubW(bA.v.X, c.invMassA, Px); + bA.v.Y = b2MulSubW(bA.v.Y, c.invMassA, Py); + bA.w = b2MulSubW(bA.w, c.invIA, b2SubW(b2MulW(rA.X, Py), b2MulW(rA.Y, Px))); + + bB.v.X = b2MulAddW(bB.v.X, c.invMassB, Px); + bB.v.Y = b2MulAddW(bB.v.Y, c.invMassB, Py); + bB.w = b2MulAddW(bB.w, c.invIB, b2SubW(b2MulW(rB.X, Py), b2MulW(rB.Y, Px))); + } } b2ScatterBodies(states, c.indexA.AsSpan(), ref bA); @@ -1803,16 +1802,17 @@ internal static void b2SolveContactsTask(int startIndex, int endIndex, B2StepCon b2TracyCZoneEnd(B2TracyCZone.solve_contact); } - internal static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2StepContext context, int colorIndex) + internal static void b2ApplyRestitutionTask(B2SolverBlock block, B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.restitution, "Restitution", B2HexColor.b2_colorDodgerBlue, true); B2BodyState[] states = context.states; + int colorIndex = block.colorIndex; Span constraints = context.graph.colors[colorIndex].wideConstraints; B2FloatW threshold = b2SplatW(context.world.restitutionThreshold); B2FloatW zero = b2ZeroW(); - for (int i = startIndex; i < endIndex; ++i) + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) { ref B2ContactConstraintWide c = ref constraints[i]; @@ -1837,7 +1837,7 @@ internal static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2Step B2FloatW mask = b2OrW(b2OrW(mask1, mask2), restitutionMask); B2FloatW mass = b2BlendW(c.normalMass1, zero, mask); - // fixed anchors for Jacobians + // Fixed anchors for impulses B2Vec2W rA = c.anchorA1; B2Vec2W rB = c.anchorB1; @@ -1916,48 +1916,110 @@ internal static void b2ApplyRestitutionTask(int startIndex, int endIndex, B2Step b2TracyCZoneEnd(B2TracyCZone.restitution); } - internal static void b2StoreImpulsesTask(int startIndex, int endIndex, B2StepContext context) + // I tried adding this to the last relax iterations but it was slower. + // + // Runs as a flat parallel-for over the whole wide constraint range. Per-color + // contact sims are looked up through the prepareSpans cursor rather than the + // block's colorIndex, matching the layout of b2PrepareContactsTask. + // + // Note: I could store the manifold pointer in the b2ContactConstraintWide to simplify + // this. + internal static void b2StoreImpulsesTask(B2SolverBlock block, B2StepContext context, int workerIndex) { b2TracyCZoneNC(B2TracyCZone.store_impulses, "Store", B2HexColor.b2_colorFireBrick, true); - Span contacts = context.contacts; - Span constraints = context.wideContactConstraints; - - B2Manifold dummy = new B2Manifold(); + B2World world = context.world; + ReadOnlySpan spans = context.contactPrepareSpans; + ReadOnlySpan wideBase = context.wideContactConstraints; + B2TaskContext taskContext = world.taskContexts.data[workerIndex]; + ref B2BitSet hitEventBitSet = ref taskContext.hitEventBitSet; + bool hasHitEvents = taskContext.hasHitEvents; + float negHitThreshold = -world.hitEventThreshold; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index + int colorIndex = 0; + while (spans[colorIndex + 1].start <= wideIndex) + { + colorIndex += 1; + } - for (int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex) + // Loop over block + while (wideIndex < endWideIndex) { - ref B2ContactConstraintWide c = ref constraints[constraintIndex]; - ref B2FloatW rollingImpulse = ref c.rollingImpulse; - ref B2FloatW normalImpulse1 = ref c.normalImpulse1; - ref B2FloatW normalImpulse2 = ref c.normalImpulse2; - ref B2FloatW tangentImpulse1 = ref c.tangentImpulse1; - ref B2FloatW tangentImpulse2 = ref c.tangentImpulse2; - ref B2FloatW totalNormalImpulse1 = ref c.totalNormalImpulse1; - ref B2FloatW totalNormalImpulse2 = ref c.totalNormalImpulse2; - ref B2FloatW normalVelocity1 = ref c.relativeVelocity1; - ref B2FloatW normalVelocity2 = ref c.relativeVelocity2; - - int baseIndex = B2_SIMD_WIDTH * constraintIndex; - - for (int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex) + int colorWideEndIndex = b2MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorWideStart = spans[colorIndex].start; + int colorContactCount = spans[colorIndex].count; + Span contactSims = spans[colorIndex].contacts; + + + // Loop over color + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) { - ref B2Manifold m = ref contacts[baseIndex + laneIndex] == null ? ref dummy : ref contacts[baseIndex + laneIndex].manifold; - m.rollingImpulse = rollingImpulse[laneIndex]; - - m.points[0].normalImpulse = normalImpulse1[laneIndex]; - m.points[0].tangentImpulse = tangentImpulse1[laneIndex]; - m.points[0].totalNormalImpulse = totalNormalImpulse1[laneIndex]; - m.points[0].normalVelocity = normalVelocity1[laneIndex]; - - m.points[1].normalImpulse = normalImpulse2[laneIndex]; - m.points[1].tangentImpulse = tangentImpulse2[laneIndex]; - m.points[1].totalNormalImpulse = totalNormalImpulse2[laneIndex]; - m.points[1].normalVelocity = normalVelocity2[laneIndex]; + ref readonly B2ContactConstraintWide c = ref wideBase[wideIndex]; + ref readonly B2FloatW rollingImpulse = ref c.rollingImpulse; + ref readonly B2FloatW normalImpulse1 = ref c.normalImpulse1; + ref readonly B2FloatW normalImpulse2 = ref c.normalImpulse2; + ref readonly B2FloatW tangentImpulse1 = ref c.tangentImpulse1; + ref readonly B2FloatW tangentImpulse2 = ref c.tangentImpulse2; + ref readonly B2FloatW totalNormalImpulse1 = ref c.totalNormalImpulse1; + ref readonly B2FloatW totalNormalImpulse2 = ref c.totalNormalImpulse2; + ref readonly B2FloatW normalVelocity1 = ref c.relativeVelocity1; + ref readonly B2FloatW normalVelocity2 = ref c.relativeVelocity2; + + int localWideIndex = wideIndex - colorWideStart; + int baseIndex = B2_SIMD_WIDTH * localWideIndex; + + for ( int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex ) + { + int contactIndex = baseIndex + laneIndex; + if ( contactIndex >= colorContactCount ) + { + break; + } + + B2ContactSim contactSim = contactSims[contactIndex]; + ref B2Manifold m = ref contactSim.manifold; + m.rollingImpulse = rollingImpulse[laneIndex]; + + m.points[0].normalImpulse = normalImpulse1[laneIndex]; + m.points[0].tangentImpulse = tangentImpulse1[laneIndex]; + m.points[0].totalNormalImpulse = totalNormalImpulse1[laneIndex]; + m.points[0].normalVelocity = normalVelocity1[laneIndex]; + + m.points[1].normalImpulse = normalImpulse2[laneIndex]; + m.points[1].tangentImpulse = tangentImpulse2[laneIndex]; + m.points[1].totalNormalImpulse = totalNormalImpulse2[laneIndex]; + m.points[1].normalVelocity = normalVelocity2[laneIndex]; + + // Check for hit events to speed up serial processing later in the step + if ((contactSim.simFlags & (uint)B2ContactSimFlags.b2_simEnableHitEvent) != 0) + { + for (int k = 0; k < contactSim.manifold.pointCount; ++k) + { + ref B2ManifoldPoint mp = ref m.points[k]; + + // Need to check total impulse because the point may be speculative and not colliding + if (mp.normalVelocity < negHitThreshold && mp.totalNormalImpulse > 0.0f) + { + b2SetBit(ref hitEventBitSet, contactSim.contactId); + hasHitEvents = true; + break; + } + } + } + } } + + // Advance to next color + colorIndex += 1; } + taskContext.hasHitEvents = hasHitEvents; + b2TracyCZoneEnd(B2TracyCZone.store_impulses); } } -} +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Contacts.cs b/src/Box2D.NET/B2Contacts.cs index 5f4c65b0..fea99d81 100644 --- a/src/Box2D.NET/B2Contacts.cs +++ b/src/Box2D.NET/B2Contacts.cs @@ -265,7 +265,7 @@ public static void b2CreateContact(B2World world, B2Shape shapeA, B2Shape shapeB // Contacts are created as non-touching. Later if they are found to be touching // they will link islands and be moved into the constraint graph. - ref B2ContactSim contactSim = ref b2Array_Add(ref set.contactSims); + ref B2ContactSim contactSim = ref b2Array_Emplace(ref set.contactSims); contactSim.contactId = contactId; #if DEBUG diff --git a/src/Box2D.NET/B2Cores.cs b/src/Box2D.NET/B2Cores.cs index ff6975ee..c2499555 100644 --- a/src/Box2D.NET/B2Cores.cs +++ b/src/Box2D.NET/B2Cores.cs @@ -80,7 +80,7 @@ internal static void B2_CHECK_DEF(in B2RevoluteJointDef def) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal static void B2_CHECK_DEF(in b2FilterJointDef def) + internal static void B2_CHECK_DEF(in B2FilterJointDef def) { B2_ASSERT(def.internalValue == B2_SECRET_COOKIE); } diff --git a/src/Box2D.NET/B2Counters.cs b/src/Box2D.NET/B2Counters.cs index cfae2a56..9432ec75 100644 --- a/src/Box2D.NET/B2Counters.cs +++ b/src/Box2D.NET/B2Counters.cs @@ -18,5 +18,11 @@ public struct B2Counters public int byteCount; public int taskCount; public B2FixedArray24 colorCounts; + + // Number of contacts touched by the collide pass (graph contacts + awake-set non-touching). + public int awakeContactCount; + + // Number of contacts recycled in the most recent step. + public int recycledContactCount; } } diff --git a/src/Box2D.NET/B2DebugDraw.cs b/src/Box2D.NET/B2DebugDraw.cs index 09fad0f0..d234c152 100644 --- a/src/Box2D.NET/B2DebugDraw.cs +++ b/src/Box2D.NET/B2DebugDraw.cs @@ -14,7 +14,7 @@ public class B2DebugDraw public DrawCircleFcn DrawCircleFcn; public DrawSolidCircleFcn DrawSolidCircleFcn; public DrawSolidCapsuleFcn DrawSolidCapsuleFcn; - public DrawLineFcn drawLineFcn; + public DrawLineFcn DrawLineFcn; public DrawTransformFcn DrawTransformFcn; public DrawPointFcn DrawPointFcn; public DrawStringFcn DrawStringFcn; diff --git a/src/Box2D.NET/B2DistanceJoints.cs b/src/Box2D.NET/B2DistanceJoints.cs index 898cc6f0..e701601f 100644 --- a/src/Box2D.NET/B2DistanceJoints.cs +++ b/src/Box2D.NET/B2DistanceJoints.cs @@ -570,22 +570,22 @@ internal static void b2DrawDistanceJoint(B2DebugDraw draw, B2JointSim @base, in if (joint.minLength > B2_LINEAR_SLOP) { // draw.DrawPoint(pMin, 4.0f, c2, draw.context); - draw.drawLineFcn(b2Sub(pMin, offset), b2Add(pMin, offset), B2HexColor.b2_colorLightGreen, draw.context); + draw.DrawLineFcn(b2Sub(pMin, offset), b2Add(pMin, offset), B2HexColor.b2_colorLightGreen, draw.context); } if (joint.maxLength < B2_HUGE) { // draw.DrawPoint(pMax, 4.0f, c3, draw.context); - draw.drawLineFcn(b2Sub(pMax, offset), b2Add(pMax, offset), B2HexColor.b2_colorRed, draw.context); + draw.DrawLineFcn(b2Sub(pMax, offset), b2Add(pMax, offset), B2HexColor.b2_colorRed, draw.context); } if (joint.minLength > B2_LINEAR_SLOP && joint.maxLength < B2_HUGE) { - draw.drawLineFcn(pMin, pMax, B2HexColor.b2_colorGray, draw.context); + draw.DrawLineFcn(pMin, pMax, B2HexColor.b2_colorGray, draw.context); } } - draw.drawLineFcn(pA, pB, B2HexColor.b2_colorWhite, draw.context); + draw.DrawLineFcn(pA, pB, B2HexColor.b2_colorWhite, draw.context); draw.DrawPointFcn(pA, 4.0f, B2HexColor.b2_colorWhite, draw.context); draw.DrawPointFcn(pB, 4.0f, B2HexColor.b2_colorWhite, draw.context); diff --git a/src/Box2D.NET/B2DynamicTrees.cs b/src/Box2D.NET/B2DynamicTrees.cs index 970a4ba5..5b9ac3c0 100644 --- a/src/Box2D.NET/B2DynamicTrees.cs +++ b/src/Box2D.NET/B2DynamicTrees.cs @@ -53,8 +53,10 @@ internal static ushort b2MaxUInt16(ushort a, ushort b) } /// Constructing the tree initializes the node pool. - public static B2DynamicTree b2DynamicTree_Create() + public static B2DynamicTree b2DynamicTree_Create(int proxyCapacity = 16) { + int capacity = b2MaxInt(proxyCapacity, 16); + B2DynamicTree tree = new B2DynamicTree(); tree.Clear(); @@ -63,7 +65,8 @@ public static B2DynamicTree b2DynamicTree_Create() tree.root = B2_NULL_INDEX; - tree.nodeCapacity = 16; + // maximum node count for a full binary tree is 2 * leafCount - 1 + tree.nodeCapacity = 2 * capacity - 1; tree.nodeCount = 0; tree.nodes = b2Alloc(tree.nodeCapacity); diff --git a/src/Box2D.NET/b2FilterJointDef.cs b/src/Box2D.NET/B2FilterJointDef.cs similarity index 93% rename from src/Box2D.NET/b2FilterJointDef.cs rename to src/Box2D.NET/B2FilterJointDef.cs index 876d6d25..cb3fe84c 100644 --- a/src/Box2D.NET/b2FilterJointDef.cs +++ b/src/Box2D.NET/B2FilterJointDef.cs @@ -7,7 +7,7 @@ namespace Box2D.NET /// A filter joint is used to disable collision between two specific bodies. /// /// @ingroup filter_joint - public struct b2FilterJointDef + public struct B2FilterJointDef { /// Base joint definition public B2JointDef @base; diff --git a/src/Box2D.NET/B2FixedArray1.cs b/src/Box2D.NET/B2FixedArray1.cs index e7de8968..148efee3 100644 --- a/src/Box2D.NET/B2FixedArray1.cs +++ b/src/Box2D.NET/B2FixedArray1.cs @@ -18,16 +18,18 @@ public struct B2FixedArray1 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray1024.cs b/src/Box2D.NET/B2FixedArray1024.cs index afaeaf0f..a381979a 100644 --- a/src/Box2D.NET/B2FixedArray1024.cs +++ b/src/Box2D.NET/B2FixedArray1024.cs @@ -1041,16 +1041,18 @@ public struct B2FixedArray1024 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray11.cs b/src/Box2D.NET/B2FixedArray11.cs index 72fa8a32..1fce97de 100644 --- a/src/Box2D.NET/B2FixedArray11.cs +++ b/src/Box2D.NET/B2FixedArray11.cs @@ -28,16 +28,18 @@ public struct B2FixedArray11 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray12.cs b/src/Box2D.NET/B2FixedArray12.cs index eae31b1d..d2082878 100644 --- a/src/Box2D.NET/B2FixedArray12.cs +++ b/src/Box2D.NET/B2FixedArray12.cs @@ -29,16 +29,18 @@ public struct B2FixedArray12 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray16.cs b/src/Box2D.NET/B2FixedArray16.cs index 66b7e6f8..46b63b73 100644 --- a/src/Box2D.NET/B2FixedArray16.cs +++ b/src/Box2D.NET/B2FixedArray16.cs @@ -33,16 +33,18 @@ public struct B2FixedArray16 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray2.cs b/src/Box2D.NET/B2FixedArray2.cs index 63a027ad..32744691 100644 --- a/src/Box2D.NET/B2FixedArray2.cs +++ b/src/Box2D.NET/B2FixedArray2.cs @@ -25,16 +25,18 @@ public B2FixedArray2(T v0000, T v0001) _v0001 = v0001; } - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray24.cs b/src/Box2D.NET/B2FixedArray24.cs index 1862293c..cf1d6711 100644 --- a/src/Box2D.NET/B2FixedArray24.cs +++ b/src/Box2D.NET/B2FixedArray24.cs @@ -41,16 +41,18 @@ public struct B2FixedArray24 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray3.cs b/src/Box2D.NET/B2FixedArray3.cs index a6032cc8..66d1ccbe 100644 --- a/src/Box2D.NET/B2FixedArray3.cs +++ b/src/Box2D.NET/B2FixedArray3.cs @@ -20,16 +20,18 @@ public struct B2FixedArray3 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray32.cs b/src/Box2D.NET/B2FixedArray32.cs index b62b4608..c23f1d12 100644 --- a/src/Box2D.NET/B2FixedArray32.cs +++ b/src/Box2D.NET/B2FixedArray32.cs @@ -49,16 +49,18 @@ public struct B2FixedArray32 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray4.cs b/src/Box2D.NET/B2FixedArray4.cs index ebf1b90c..0005363d 100644 --- a/src/Box2D.NET/B2FixedArray4.cs +++ b/src/Box2D.NET/B2FixedArray4.cs @@ -21,16 +21,18 @@ public struct B2FixedArray4 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray64.cs b/src/Box2D.NET/B2FixedArray64.cs index e04bf163..51d60b93 100644 --- a/src/Box2D.NET/B2FixedArray64.cs +++ b/src/Box2D.NET/B2FixedArray64.cs @@ -81,16 +81,18 @@ public struct B2FixedArray64 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray7.cs b/src/Box2D.NET/B2FixedArray7.cs index c1a98850..7744ad61 100644 --- a/src/Box2D.NET/B2FixedArray7.cs +++ b/src/Box2D.NET/B2FixedArray7.cs @@ -24,16 +24,18 @@ public struct B2FixedArray7 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FixedArray8.cs b/src/Box2D.NET/B2FixedArray8.cs index 154e58bf..76d8bc8b 100644 --- a/src/Box2D.NET/B2FixedArray8.cs +++ b/src/Box2D.NET/B2FixedArray8.cs @@ -25,16 +25,18 @@ public struct B2FixedArray8 where T : unmanaged public int Length => Size; - public ref T this[int index] + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref T this[int index] { [MethodImpl(MethodImplOptions.AggressiveInlining)] get => ref AsSpanUnsafe()[index]; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal Span AsSpanUnsafe() + internal readonly Span AsSpanUnsafe() { - return MemoryMarshal.CreateSpan(ref _v0000, Size); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in _v0000), Size); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2FloatW.cs b/src/Box2D.NET/B2FloatW.cs index 776289dd..04462da2 100644 --- a/src/Box2D.NET/B2FloatW.cs +++ b/src/Box2D.NET/B2FloatW.cs @@ -3,6 +3,7 @@ // SPDX-License-Identifier: MIT using System; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; namespace Box2D.NET @@ -25,11 +26,13 @@ public B2FloatW(float x, float y, float z, float w) W = w; } - public ref float this[int index] => ref MemoryMarshal.CreateSpan(ref X, 4)[index]; + // readonly so an "in" or "ref readonly" receiver does not force a defensive + // copy. This matches Span.this[int], which is also a readonly ref T indexer. + public readonly ref float this[int index] => ref MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in X), 4)[index]; - public Span AsSpan() + public readonly Span AsSpan() { - return MemoryMarshal.CreateSpan(ref X, 4); + return MemoryMarshal.CreateSpan(ref Unsafe.AsRef(in X), 4); } } } \ No newline at end of file diff --git a/src/Box2D.NET/B2GraphColor.cs b/src/Box2D.NET/B2GraphColor.cs index 0f539bae..1bf1eba5 100644 --- a/src/Box2D.NET/B2GraphColor.cs +++ b/src/Box2D.NET/B2GraphColor.cs @@ -22,12 +22,14 @@ public struct B2GraphColor public B2Array contactSims; public B2Array jointSims; - // TODO: @ikpil, check union // transient + // TODO: @ikpil, check union // union //{ public ArraySegment wideConstraints; public ArraySegment overflowConstraints; //}; + + public int wideConstraintCount; } } diff --git a/src/Box2D.NET/B2Islands.cs b/src/Box2D.NET/B2Islands.cs index 2106db62..cfa07d27 100644 --- a/src/Box2D.NET/B2Islands.cs +++ b/src/Box2D.NET/B2Islands.cs @@ -52,7 +52,7 @@ public static B2Island b2CreateIsland(B2World world, int setIndex) island.joints = b2Array_Create(); island.constraintRemoveCount = 0; - ref B2IslandSim islandSim = ref b2Array_Add(ref set.islandSims); + ref B2IslandSim islandSim = ref b2Array_Emplace(ref set.islandSims); islandSim.islandId = islandId; return island; @@ -430,14 +430,14 @@ public static void b2SplitIsland(B2World world, int baseId) B2JointLink[] baseJoints = baseIsland.joints.data; int baseJointCapacity = baseIsland.joints.capacity; - B2ArenaAllocator alloc = world.arena; + B2StackAllocator alloc = world.stack; // No lock is needed because I ensure the allocator is not used while this task is active. // Allocate contactCounts and jointCounts before ranks so ranks can be freed first (LIFO arena). - ArraySegment parents = b2AllocateArenaItem(alloc, baseBodyCount, "parents"); - ArraySegment contactCounts = b2AllocateArenaItem(alloc, baseBodyCount, "contact counts"); - ArraySegment jointCounts = b2AllocateArenaItem(alloc, baseBodyCount, "joint counts"); - ArraySegment ranks = b2AllocateArenaItem(alloc, baseBodyCount, "ranks"); + ArraySegment parents = b2StackAlloc(alloc, baseBodyCount, "parents"); + ArraySegment contactCounts = b2StackAlloc(alloc, baseBodyCount, "contact counts"); + ArraySegment jointCounts = b2StackAlloc(alloc, baseBodyCount, "joint counts"); + ArraySegment ranks = b2StackAlloc(alloc, baseBodyCount, "ranks"); for (int i = 0; i < baseBodyCount; ++i) { parents[i] = i; @@ -507,7 +507,7 @@ public static void b2SplitIsland(B2World world, int baseId) } // Done with ranks - b2FreeArenaItem(alloc, ranks); + b2StackFree(alloc, ranks); ranks = null; // Flatten all parent indices and count connected components. @@ -525,9 +525,9 @@ public static void b2SplitIsland(B2World world, int baseId) if (componentCount == 1) { baseIsland.constraintRemoveCount = 0; - b2FreeArenaItem(alloc, jointCounts); - b2FreeArenaItem(alloc, contactCounts); - b2FreeArenaItem(alloc, parents); + b2StackFree(alloc, jointCounts); + b2StackFree(alloc, contactCounts); + b2StackFree(alloc, parents); return; } @@ -548,15 +548,15 @@ public static void b2SplitIsland(B2World world, int baseId) baseIsland = null; // Map from body index to new island index. Only set for root bodies. - ArraySegment rootMap = b2AllocateArenaItem(alloc, baseBodyCount, "root map"); + ArraySegment rootMap = b2StackAlloc(alloc, baseBodyCount, "root map"); for (int i = 0; i < baseBodyCount; ++i) { rootMap[i] = B2_NULL_INDEX; } - ArraySegment componentBodyCounts = b2AllocateArenaItem(alloc, componentCount, "component body counts"); - ArraySegment componentContactCounts = b2AllocateArenaItem(alloc, componentCount, "component contact counts"); - ArraySegment componentJointCounts = b2AllocateArenaItem(alloc, componentCount, "component joint counts"); + ArraySegment componentBodyCounts = b2StackAlloc(alloc, componentCount, "component body counts"); + ArraySegment componentContactCounts = b2StackAlloc(alloc, componentCount, "component contact counts"); + ArraySegment componentJointCounts = b2StackAlloc(alloc, componentCount, "component joint counts"); int islandCount = 0; // Find the root body for each body and create islands as needed. @@ -579,7 +579,7 @@ public static void b2SplitIsland(B2World world, int baseId) B2_ASSERT(islandCount == componentCount); // Map from new island index to island id - ArraySegment islandIds = b2AllocateArenaItem(alloc, islandCount, "island ids"); + ArraySegment islandIds = b2StackAlloc(alloc, islandCount, "island ids"); // Create new islands and reserve body/contact/joint arrays for (int i = 0; i < islandCount; ++i) @@ -661,14 +661,14 @@ public static void b2SplitIsland(B2World world, int baseId) b2Free(baseJoints, baseJointCapacity); // Free arena items in LIFO order - b2FreeArenaItem(alloc, islandIds); - b2FreeArenaItem(alloc, componentJointCounts); - b2FreeArenaItem(alloc, componentContactCounts); - b2FreeArenaItem(alloc, componentBodyCounts); - b2FreeArenaItem(alloc, rootMap); - b2FreeArenaItem(alloc, jointCounts); - b2FreeArenaItem(alloc, contactCounts); - b2FreeArenaItem(alloc, parents); + b2StackFree(alloc, islandIds); + b2StackFree(alloc, componentJointCounts); + b2StackFree(alloc, componentContactCounts); + b2StackFree(alloc, componentBodyCounts); + b2StackFree(alloc, rootMap); + b2StackFree(alloc, jointCounts); + b2StackFree(alloc, contactCounts); + b2StackFree(alloc, parents); } // Split an island because some contacts and/or joints have been removed. diff --git a/src/Box2D.NET/B2JointPrepareSpan.cs b/src/Box2D.NET/B2JointPrepareSpan.cs new file mode 100644 index 00000000..fa8281ad --- /dev/null +++ b/src/Box2D.NET/B2JointPrepareSpan.cs @@ -0,0 +1,14 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +namespace Box2D.NET +{ + // Similar for joints + public struct B2JointPrepareSpan + { + public int start; + public int count; + public B2JointSim[] joints; + } +} diff --git a/src/Box2D.NET/B2Joints.cs b/src/Box2D.NET/B2Joints.cs index e9ba7f5b..0d199d92 100644 --- a/src/Box2D.NET/B2Joints.cs +++ b/src/Box2D.NET/B2Joints.cs @@ -25,6 +25,7 @@ using static Box2D.NET.B2BroadPhases; using static Box2D.NET.B2Solvers; using static Box2D.NET.B2Ids; +using static Box2D.NET.B2BitSets; namespace Box2D.NET { @@ -68,9 +69,9 @@ public static B2MotorJointDef b2DefaultMotorJointDef() /// Use this to initialize your joint definition /// @ingroup filter_joint - public static b2FilterJointDef b2DefaultFilterJointDef() + public static B2FilterJointDef b2DefaultFilterJointDef() { - b2FilterJointDef def = new b2FilterJointDef(); + B2FilterJointDef def = new B2FilterJointDef(); def.@base = b2DefaultJointDef(); def.internalValue = B2_SECRET_COOKIE; return def; @@ -280,7 +281,7 @@ public static B2JointPair b2CreateJoint(B2World world, in B2JointDef def, B2Join joint.setIndex = (int)B2SolverSetType.b2_disabledSet; joint.localIndex = set.jointSims.count; - jointSim = b2Array_Add(ref set.jointSims); + jointSim = b2Array_Emplace(ref set.jointSims); //memset( jointSim, 0, sizeof( b2JointSim ) ); jointSim.Clear(); @@ -295,7 +296,7 @@ public static B2JointPair b2CreateJoint(B2World world, in B2JointDef def, B2Join joint.setIndex = (int)B2SolverSetType.b2_staticSet; joint.localIndex = set.jointSims.count; - jointSim = b2Array_Add(ref set.jointSims); + jointSim = b2Array_Emplace(ref set.jointSims); //memset( jointSim, 0, sizeof( b2JointSim ) ); jointSim.Clear(); @@ -331,7 +332,7 @@ public static B2JointPair b2CreateJoint(B2World world, in B2JointDef def, B2Join joint.setIndex = setIndex; joint.localIndex = set.jointSims.count; - jointSim = b2Array_Add(ref set.jointSims); + jointSim = b2Array_Emplace(ref set.jointSims); //memset( jointSim, 0, sizeof( b2JointSim ) ); jointSim.Clear(); @@ -482,8 +483,8 @@ public static B2JointId b2CreateMotorJoint(B2WorldId worldId, in B2MotorJointDef * @{ */ /// Create a filter joint. - /// @see b2FilterJointDef for details - public static B2JointId b2CreateFilterJoint(B2WorldId worldId, in b2FilterJointDef def) + /// @see B2FilterJointDef for details + public static B2JointId b2CreateFilterJoint(B2WorldId worldId, in B2FilterJointDef def) { B2_CHECK_DEF(def); B2World world = b2GetWorldFromId(worldId); @@ -1434,7 +1435,7 @@ internal static void b2SolveJoint(B2JointSim joint, B2StepContext context, bool } } - internal static void b2PrepareOverflowJoints(B2StepContext context) + internal static void b2PrepareJoints_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.prepare_joints, "PrepJoints", B2HexColor.b2_colorOldLace, true); @@ -1451,7 +1452,7 @@ internal static void b2PrepareOverflowJoints(B2StepContext context) b2TracyCZoneEnd(B2TracyCZone.prepare_joints); } - internal static void b2WarmStartOverflowJoints(B2StepContext context) + internal static void b2WarmStartJoints_Overflow(B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.prepare_joints, "PrepJoints", B2HexColor.b2_colorOldLace, true); @@ -1468,7 +1469,7 @@ internal static void b2WarmStartOverflowJoints(B2StepContext context) b2TracyCZoneEnd(B2TracyCZone.prepare_joints); } - internal static void b2SolveOverflowJoints(B2StepContext context, bool useBias) + internal static void b2SolveJoints_Overflow(B2StepContext context, bool useBias) { b2TracyCZoneNC(B2TracyCZone.solve_joints, "Solve Overflow Joints", B2HexColor.b2_colorLemonChiffon, true); @@ -1485,6 +1486,94 @@ internal static void b2SolveOverflowJoints(B2StepContext context, bool useBias) b2TracyCZoneEnd(B2TracyCZone.solve_joints); } + internal static void b2PrepareJointsTask(B2SolverBlock block, B2StepContext context) + { + b2TracyCZoneNC(B2TracyCZone.prepare_joints, "PrepJoints", B2HexColor.b2_colorOldLace, true); + + B2JointPrepareSpan[] spans = context.jointPrepareSpans; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while (spans[colorIndex + 1].start <= index) + { + colorIndex += 1; + } + + // Loop over block + while (index < endIndex) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b2MinInt(spans[colorIndex + 1].start, endIndex); + B2JointSim[] joints = spans[colorIndex].joints; + + // Loop over color + for (; index < colorEndIndex; ++index) + { + B2_ASSERT(0 <= index - colorStart && index - colorStart < spans[colorIndex].count); + B2JointSim joint = joints[index - colorStart]; + b2PrepareJoint(joint, context); + } + + // Advance to next color + colorIndex += 1; + } + + b2TracyCZoneEnd(B2TracyCZone.prepare_joints); + } + + internal static void b2WarmStartJointsTask(B2SolverBlock block, B2StepContext context) + { + b2TracyCZoneNC(B2TracyCZone.warm_joints, "WarmJoints", B2HexColor.b2_colorGold, true); + + ref B2GraphColor color = ref context.graph.colors[block.colorIndex]; + B2JointSim[] joints = color.jointSims.data; + + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) + { + B2JointSim joint = joints[i]; + b2WarmStartJoint(joint, context); + } + + b2TracyCZoneEnd(B2TracyCZone.warm_joints); + } + + internal static void b2SolveJointsTask(B2SolverBlock block, B2StepContext context, bool useBias, int workerIndex) + { + b2TracyCZoneNC(B2TracyCZone.solve_joints, "SolveJoints", B2HexColor.b2_colorLemonChiffon, true); + + ref B2GraphColor color = ref context.graph.colors[block.colorIndex]; + B2JointSim[] joints = color.jointSims.data; + + B2_ASSERT(0 <= block.startIndex && block.startIndex + block.count <= color.jointSims.count); + + ref B2BitSet jointStateBitSet = ref context.world.taskContexts.data[workerIndex].jointStateBitSet; + + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) + { + B2JointSim joint = joints[i]; + b2SolveJoint(joint, context, useBias); + + if (useBias && (joint.forceThreshold < float.MaxValue || joint.torqueThreshold < float.MaxValue) && + b2GetBit(ref jointStateBitSet, joint.jointId) == false) + { + float force, torque; + b2GetJointReaction(joint, context.inv_h, out force, out torque); + + // Check thresholds. A zero threshold means all awake joints get reported. + if (force >= joint.forceThreshold || torque >= joint.torqueThreshold) + { + // Flag this joint for processing. + b2SetBit(ref jointStateBitSet, joint.jointId); + } + } + } + + b2TracyCZoneEnd(B2TracyCZone.solve_joints); + } + internal static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) { B2Body bodyA = b2Array_Get(ref world.bodies, joint.edges[0].bodyId); @@ -1512,13 +1601,13 @@ internal static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) break; case B2JointType.b2_filterJoint: - draw.drawLineFcn(pA, pB, B2HexColor.b2_colorGold, draw.context); + draw.DrawLineFcn(pA, pB, B2HexColor.b2_colorGold, draw.context); break; case B2JointType.b2_motorJoint: draw.DrawPointFcn(pA, 8.0f, B2HexColor.b2_colorYellowGreen, draw.context); draw.DrawPointFcn(pB, 8.0f, B2HexColor.b2_colorPlum, draw.context); - draw.drawLineFcn(pA, pB, B2HexColor.b2_colorLightGray, draw.context); + draw.DrawLineFcn(pA, pB, B2HexColor.b2_colorLightGray, draw.context); break; case B2JointType.b2_prismaticJoint: @@ -1538,9 +1627,9 @@ internal static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) break; default: - draw.drawLineFcn(transformA.p, pA, color, draw.context); - draw.drawLineFcn(pA, pB, color, draw.context); - draw.drawLineFcn(transformB.p, pB, color, draw.context); + draw.DrawLineFcn(transformA.p, pA, color, draw.context); + draw.DrawLineFcn(pA, pB, color, draw.context); + draw.DrawLineFcn(transformB.p, pB, color, draw.context); break; } @@ -1550,7 +1639,7 @@ internal static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) if (colorIndex != B2_NULL_INDEX) { B2Vec2 p = b2Lerp(pA, pB, 0.5f); - draw.DrawPointFcn(p, 5.0f, b2_graphColors[colorIndex], draw.context); + draw.DrawPointFcn(p, 5.0f, b2GetGraphColor(colorIndex), draw.context); } } @@ -1560,7 +1649,7 @@ internal static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) float torque = b2GetJointConstraintTorque(world, joint); B2Vec2 p = b2Lerp(pA, pB, 0.5f); - draw.drawLineFcn(p, b2MulAdd(p, 0.001f, force), B2HexColor.b2_colorAzure, draw.context); + draw.DrawLineFcn(p, b2MulAdd(p, 0.001f, force), B2HexColor.b2_colorAzure, draw.context); string result = $"f = [{force.X:g}, {force.Y:g}], t = {torque:g}"; draw.DrawStringFcn(p, result, B2HexColor.b2_colorAzure, draw.context); diff --git a/src/Box2D.NET/B2MotorJoints.cs b/src/Box2D.NET/B2MotorJoints.cs index 0b204005..1820d110 100644 --- a/src/Box2D.NET/B2MotorJoints.cs +++ b/src/Box2D.NET/B2MotorJoints.cs @@ -185,13 +185,11 @@ internal static float b2GetMotorJointTorque(B2World world, B2JointSim @base) // Identity used: // w k % (rx i + ry j) = w * (-ry i + rx j) - // Point-to-point constraint - // C = p2 - p1 - // Cdot = v2 - v1 - // = v2 + cross(w2, r2) - v1 - cross(w1, r1) - // J = [-I -r1_skew I r2_skew ] - // Identity used: - // w k % (rx i + ry j) = w * (-ry i + rx j) + // Angle constraint + // C = angle2 - angle1 - referenceAngle + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 internal static void b2PrepareMotorJoint(B2JointSim @base, B2StepContext context) { B2_ASSERT(@base.type == B2JointType.b2_motorJoint); diff --git a/src/Box2D.NET/B2PrismaticJoints.cs b/src/Box2D.NET/B2PrismaticJoints.cs index 9f2bb2e5..2082d85a 100644 --- a/src/Box2D.NET/B2PrismaticJoints.cs +++ b/src/Box2D.NET/B2PrismaticJoints.cs @@ -275,25 +275,16 @@ internal static float b2GetPrismaticJointTorque(B2World world, B2JointSim @base) // So: // Cdot + max(C1, 0)/h >= 0 - // Linear constraint (point-to-line) - // d = pB - pA = xB + rB - xA - rA - // C = dot(perp, d) - // Cdot = dot(d, cross(wA, perp)) + dot(perp, vB + cross(wB, rB) - vA - cross(wA, rA)) - // = -dot(perp, vA) - dot(cross(rA + d, perp), wA) + dot(perp, vB) + dot(cross(rB, perp), vB) - // J = [-perp, -cross(rA + d, perp), perp, cross(rB, perp)] + // Block Solver + // We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer. // - // Angular constraint - // C = aB - aA + a_initial - // Cdot = wB - wA - // J = [0 0 -1 0 0 1] - // - // K = J * invM * JT + // The Jacobian has 2 rows: + // J = [-uT -s1 uT s2] // linear + // [0 -1 0 1] // angular // - // J = [-a -sA a sB] - // [0 -1 0 1] - // a = perp - // sA = cross(rA + d, a) = cross(pB - xA, a) - // sB = cross(rB, a) = cross(pB - xB, a) + // u = perp + // s1 = cross(d + r1, u), s2 = cross(r2, u) + // a1 = cross(d + r1, v), a2 = cross(r2, v) internal static void b2PreparePrismaticJoint(B2JointSim @base, B2StepContext context) { B2_ASSERT(@base.type == B2JointType.b2_prismaticJoint); @@ -700,7 +691,7 @@ internal static void b2DrawPrismaticJoint(B2DebugDraw draw, B2JointSim @base, in B2Transform frameB = b2MulTransforms(transformB, @base.localFrameB); B2Vec2 axisA = b2RotateVector(frameA.q, new B2Vec2(1.0f, 0.0f)); - draw.drawLineFcn(frameA.p, frameB.p, B2HexColor.b2_colorDimGray, draw.context); + draw.DrawLineFcn(frameA.p, frameB.p, B2HexColor.b2_colorDimGray, draw.context); if (joint.enableLimit) { @@ -708,13 +699,13 @@ internal static void b2DrawPrismaticJoint(B2DebugDraw draw, B2JointSim @base, in B2Vec2 lower = b2MulAdd(frameA.p, joint.lowerTranslation, axisA); B2Vec2 upper = b2MulAdd(frameA.p, joint.upperTranslation, axisA); B2Vec2 perp = b2LeftPerp(axisA); - draw.drawLineFcn(lower, upper, B2HexColor.b2_colorGray, draw.context); - draw.drawLineFcn(b2MulSub(lower, b, perp), b2MulAdd(lower, b, perp), B2HexColor.b2_colorGreen, draw.context); - draw.drawLineFcn(b2MulSub(upper, b, perp), b2MulAdd(upper, b, perp), B2HexColor.b2_colorRed, draw.context); + draw.DrawLineFcn(lower, upper, B2HexColor.b2_colorGray, draw.context); + draw.DrawLineFcn(b2MulSub(lower, b, perp), b2MulAdd(lower, b, perp), B2HexColor.b2_colorGreen, draw.context); + draw.DrawLineFcn(b2MulSub(upper, b, perp), b2MulAdd(upper, b, perp), B2HexColor.b2_colorRed, draw.context); } else { - draw.drawLineFcn(b2MulSub(frameA.p, 1.0f, axisA), b2MulAdd(frameA.p, 1.0f, axisA), B2HexColor.b2_colorGray, draw.context); + draw.DrawLineFcn(b2MulSub(frameA.p, 1.0f, axisA), b2MulAdd(frameA.p, 1.0f, axisA), B2HexColor.b2_colorGray, draw.context); } if (joint.enableSpring) diff --git a/src/Box2D.NET/B2Profile.cs b/src/Box2D.NET/B2Profile.cs index 0cdc955c..feb42f50 100644 --- a/src/Box2D.NET/B2Profile.cs +++ b/src/Box2D.NET/B2Profile.cs @@ -12,8 +12,8 @@ public struct B2Profile public float pairs; public float collide; public float solve; - public float prepareStages; - public float solveConstraints; + public float solverSetup; + public float constraints; public float prepareConstraints; public float integrateVelocities; public float warmStart; diff --git a/src/Box2D.NET/B2RevoluteJoints.cs b/src/Box2D.NET/B2RevoluteJoints.cs index 6370c62e..e5b0a7bf 100644 --- a/src/Box2D.NET/B2RevoluteJoints.cs +++ b/src/Box2D.NET/B2RevoluteJoints.cs @@ -529,10 +529,10 @@ internal static void b2DrawRevoluteJoint(B2DebugDraw draw, B2JointSim @base, in B2Vec2 rx = new B2Vec2(radius, 0.0f); B2Vec2 r = b2RotateVector(frameA.q, rx); - draw.drawLineFcn(frameA.p, b2Add(frameA.p, r), B2HexColor.b2_colorGray, draw.context); + draw.DrawLineFcn(frameA.p, b2Add(frameA.p, r), B2HexColor.b2_colorGray, draw.context); r = b2RotateVector(frameB.q, rx); - draw.drawLineFcn(frameB.p, b2Add(frameB.p, r), B2HexColor.b2_colorBlue, draw.context); + draw.DrawLineFcn(frameB.p, b2Add(frameB.p, r), B2HexColor.b2_colorBlue, draw.context); if (draw.drawJointExtras) { @@ -552,21 +552,21 @@ internal static void b2DrawRevoluteJoint(B2DebugDraw draw, B2JointSim @base, in B2Rot rotHi = b2MulRot(frameA.q, b2MakeRot(upperAngle)); B2Vec2 rhi = b2RotateVector(rotHi, rx); - draw.drawLineFcn(frameB.p, b2Add(frameB.p, rlo), B2HexColor.b2_colorGreen, draw.context); - draw.drawLineFcn(frameB.p, b2Add(frameB.p, rhi), B2HexColor.b2_colorRed, draw.context); + draw.DrawLineFcn(frameB.p, b2Add(frameB.p, rlo), B2HexColor.b2_colorGreen, draw.context); + draw.DrawLineFcn(frameB.p, b2Add(frameB.p, rhi), B2HexColor.b2_colorRed, draw.context); } if (joint.enableSpring) { B2Rot q = b2MulRot(frameA.q, b2MakeRot(joint.targetAngle)); B2Vec2 v = b2RotateVector(q, rx); - draw.drawLineFcn(frameB.p, b2Add(frameB.p, v), B2HexColor.b2_colorViolet, draw.context); + draw.DrawLineFcn(frameB.p, b2Add(frameB.p, v), B2HexColor.b2_colorViolet, draw.context); } B2HexColor color = B2HexColor.b2_colorGold; - draw.drawLineFcn(transformA.p, frameA.p, color, draw.context); - draw.drawLineFcn(frameA.p, frameB.p, color, draw.context); - draw.drawLineFcn(transformB.p, frameB.p, color, draw.context); + draw.DrawLineFcn(transformA.p, frameA.p, color, draw.context); + draw.DrawLineFcn(frameA.p, frameB.p, color, draw.context); + draw.DrawLineFcn(transformB.p, frameB.p, color, draw.context); // char buffer[32]; // sprintf(buffer, "%.1f", b2Length(joint.impulse)); diff --git a/src/Box2D.NET/B2Sensors.cs b/src/Box2D.NET/B2Sensors.cs index 8a921805..2132d454 100644 --- a/src/Box2D.NET/B2Sensors.cs +++ b/src/Box2D.NET/B2Sensors.cs @@ -109,7 +109,7 @@ internal static bool b2SensorQueryCallback(int proxyId, ulong userData, ref B2Se // Record the overlap B2Sensor sensor = queryContext.sensor; - ref B2Visitor shapeRef = ref b2Array_Add(ref sensor.overlaps2); + ref B2Visitor shapeRef = ref b2Array_Emplace(ref sensor.overlaps2); shapeRef.shapeId = shapeId; shapeRef.generation = otherShape.generation; diff --git a/src/Box2D.NET/B2Shapes.cs b/src/Box2D.NET/B2Shapes.cs index 847ccf82..3587a962 100644 --- a/src/Box2D.NET/B2Shapes.cs +++ b/src/Box2D.NET/B2Shapes.cs @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT @@ -600,20 +600,30 @@ public static void b2DestroyChain(B2ChainId chainId) B2Body body = b2Array_Get(ref world.bodies, chain.bodyId); - // TODO: @ikpil, check! // Remove the chain from the body's singly linked list. + // C walks a pointer-to-pointer so it can write through to the previous node. + // C# tracks the previous index instead and writes to whichever slot holds the link. int chainIdPtr = body.headChainId; + int prevChainId = B2_NULL_INDEX; bool found = false; while (chainIdPtr != B2_NULL_INDEX) { if (chainIdPtr == chain.id) { - chainIdPtr = chain.nextChainId; - body.headChainId = chain.nextChainId; + if (prevChainId == B2_NULL_INDEX) + { + body.headChainId = chain.nextChainId; + } + else + { + world.chainShapes.data[prevChainId].nextChainId = chain.nextChainId; + } + found = true; break; } + prevChainId = chainIdPtr; chainIdPtr = world.chainShapes.data[chainIdPtr].nextChainId; } @@ -1139,7 +1149,7 @@ public static bool b2Shape_TestPoint(B2ShapeId shapeId, B2Vec2 point) // todo_erin untested /// Ray cast a shape directly - internal static B2CastOutput b2Shape_RayCast(B2ShapeId shapeId, in B2RayCastInput input) + public static B2CastOutput b2Shape_RayCast(B2ShapeId shapeId, in B2RayCastInput input) { B2World world = b2GetWorld(shapeId.world0); B2Shape shape = b2GetShape(world, shapeId); @@ -1433,7 +1443,7 @@ public static void b2Shape_EnableContactEvents(B2ShapeId shapeId, bool flag) shape.enableContactEvents = flag; } /// Returns true if contact events are enabled - internal static bool b2Shape_AreContactEventsEnabled(B2ShapeId shapeId) + public static bool b2Shape_AreContactEventsEnabled(B2ShapeId shapeId) { B2World world = b2GetWorld(shapeId.world0); B2Shape shape = b2GetShape(world, shapeId); @@ -1454,7 +1464,7 @@ public static void b2Shape_EnablePreSolveEvents(B2ShapeId shapeId, bool flag) shape.enablePreSolveEvents = flag; } /// Returns true if pre-solve events are enabled - internal static bool b2Shape_ArePreSolveEventsEnabled(B2ShapeId shapeId) + public static bool b2Shape_ArePreSolveEventsEnabled(B2ShapeId shapeId) { B2World world = b2GetWorld(shapeId.world0); B2Shape shape = b2GetShape(world, shapeId); @@ -1474,7 +1484,7 @@ public static void b2Shape_EnableHitEvents(B2ShapeId shapeId, bool flag) shape.enableHitEvents = flag; } /// Returns true if hit events are enabled - internal static bool b2Shape_AreHitEventsEnabled(B2ShapeId shapeId) + public static bool b2Shape_AreHitEventsEnabled(B2ShapeId shapeId) { B2World world = b2GetWorld(shapeId.world0); B2Shape shape = b2GetShape(world, shapeId); @@ -1832,7 +1842,7 @@ public static B2AABB b2Shape_GetAABB(B2ShapeId shapeId) } /// Compute the mass data for a shape - internal static B2MassData b2Shape_ComputeMassData(B2ShapeId shapeId) + public static B2MassData b2Shape_ComputeMassData(B2ShapeId shapeId) { B2World world = b2GetWorld(shapeId.world0); if (world == null) diff --git a/src/Box2D.NET/B2SolverBlock.cs b/src/Box2D.NET/B2SolverBlock.cs index 6d25e719..7bfdb9ae 100644 --- a/src/Box2D.NET/B2SolverBlock.cs +++ b/src/Box2D.NET/B2SolverBlock.cs @@ -1,56 +1,62 @@ -// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT +// Solver work is partitioned into fixed-size blocks that worker threads claim +// in parallel via atomic CAS on a per-block syncIndex. The descriptor (b2SolverBlock) +// and the atomic counter sit in a wrapping b2SyncBlock so the CAS-winner can +// pass the descriptor by value into stage tasks without aliasing the atomic +// memory other threads are CAS-writing. Three properties of this design +// matter for performance: +// +// 1. Distributed contention. Per-block atomic syncIndex avoids the cache line stampede +// that a single shared fetch_add counter would cause. Once a worker +// settles into a block range, its CAS targets live in its own L1. +// +// 2. Monotonic syncIndex across iterations. Iterative stages (warm start, +// solve, relax) reuse the same block array every sub-step iteration. +// syncIndex grows each iteration; workers CAS (prev, prev+1), so the +// main thread never touches any per-block state between iterations. +// Non-iterative stages simply use syncIndex 1. +// +// 3. L2 affinity across iterations. Each worker picks a start offset from +// its workerIndex, then scans forward and (after wrap) backward: +// +// blocks: [0] [1] [2] [3] [4] [5] [6] [7] +// ^ ^ ^ ^ +// W0 W1 W2 W3 <- start offsets +// +// W0 claims 0,1,2,3 (forward), W1 claims 4,5, etc. Under balanced load +// each worker re-hits the same block range every iteration, keeping that +// range's hot data resident in its L2. A failed CAS means a neighbour +// already claimed the block, so the stealing worker stops -- preserving +// locality under mild imbalance while still draining the queue. +// +// A graph color stage lays out joint blocks first, then contact blocks: +// +// stage->blocks -> +// +------+------+------+------+------+------+------+ +// | J0 | J1 | J2 | C0 | C1 | C2 | C3 | +// +------+------+------+------+------+------+------+ +// <-- graphJointBlocks --><---- graphContactBlocks ----> +// +// Each block carries its type so the dispatcher routes J-blocks to the joint +// solver and C-blocks to the SIMD contact solver; both kinds run concurrently +// within the stage -- no barrier between them. The type tag lives on the +// block (not the stage) so that mixed-type stages can keep the concurrency. +// +// The solver threading model is inspired by https://github.com/bepu/bepuphysics2 + namespace Box2D.NET { - // Solver work is partitioned into fixed-size blocks that worker threads claim - // in parallel via atomic CAS on each block's own syncIndex. Three properties - // of this design matter for performance: - // - // 1. Distributed contention. Per-block atomic syncIndex avoids the cache line stampede - // that a single shared fetch_add counter would cause. Once a worker - // settles into a block range, its CAS targets live in its own L1. - // - // 2. Monotonic syncIndex across iterations. Iterative stages (warm start, - // solve, relax) reuse the same block array every sub-step iteration. - // syncIndex grows each iteration; workers CAS (prev, prev+1), so the - // main thread never touches any per-block state between iterations. - // Non-iterative stages simply use syncIndex 1. - // - // 3. L2 affinity across iterations. Each worker picks a start offset from - // its workerIndex, then scans forward and (after wrap) backward: - // - // blocks: [0] [1] [2] [3] [4] [5] [6] [7] - // ^ ^ ^ ^ - // W0 W1 W2 W3 <- start offsets - // - // W0 claims 0,1,2,3 (forward), W1 claims 4,5, etc. Under balanced load - // each worker re-hits the same block range every iteration, keeping that - // range's hot data resident in its L2. A failed CAS means a neighbour - // already claimed the block, so the stealing worker stops -- preserving - // locality under mild imbalance while still draining the queue. - // - // A graph color stage lays out joint blocks first, then contact blocks: - // - // stage->blocks -> - // +------+------+------+------+------+------+------+ - // | J0 | J1 | J2 | C0 | C1 | C2 | C3 | - // +------+------+------+------+------+------+------+ - // <-- graphJointBlocks --><---- graphContactBlocks ----> - // - // Each block carries its type so the dispatcher routes J-blocks to the joint - // solver and C-blocks to the SIMD contact solver; both kinds run concurrently - // within the stage -- no barrier between them. The type tag lives on the - // block (not the stage) so that mixed-type stages can keep the concurrency. - public class B2SolverBlock + // Solver block describes a multithreaded unit of work. + public struct B2SolverBlock { public int startIndex; public ushort count; // b2SolverBlockType - public short blockType; - - public B2AtomicInt syncIndex; + public byte blockType; + public byte colorIndex; } } diff --git a/src/Box2D.NET/B2SolverSets.cs b/src/Box2D.NET/B2SolverSets.cs index 36ef9f6d..1501304c 100644 --- a/src/Box2D.NET/B2SolverSets.cs +++ b/src/Box2D.NET/B2SolverSets.cs @@ -69,11 +69,11 @@ internal static void b2WakeSolverSet(B2World world, int setIndex) // Reset sleep timer body.sleepTime = 0.0f; - ref B2BodySim simDst = ref b2Array_Add(ref awakeSet.bodySims); + ref B2BodySim simDst = ref b2Array_Emplace(ref awakeSet.bodySims); //memcpy( simDst, simSrc, sizeof( b2BodySim ) ); simDst.CopyFrom(simSrc); - ref B2BodyState state = ref b2Array_Add(ref awakeSet.bodyStates); + ref B2BodyState state = ref b2Array_Emplace(ref awakeSet.bodyStates); //*state = b2_identityBodyState; state.CopyFrom(b2_identityBodyState); state.flags = body.flags; @@ -102,7 +102,7 @@ internal static void b2WakeSolverSet(B2World world, int setIndex) contact.setIndex = (int)B2SolverSetType.b2_awakeSet; contact.localIndex = awakeSet.contactSims.count; - ref B2ContactSim awakeContactSim = ref b2Array_Add(ref awakeSet.contactSims); + ref B2ContactSim awakeContactSim = ref b2Array_Emplace(ref awakeSet.contactSims); //memcpy( awakeContactSim, contactSim, sizeof( b2ContactSim ) ); awakeContactSim.CopyFrom(contactSim); @@ -159,7 +159,7 @@ internal static void b2WakeSolverSet(B2World world, int setIndex) B2Island island = b2Array_Get(ref world.islands, islandSrc.islandId); island.setIndex = (int)B2SolverSetType.b2_awakeSet; island.localIndex = awakeSet.islandSims.count; - ref B2IslandSim islandDst = ref b2Array_Add(ref awakeSet.islandSims); + ref B2IslandSim islandDst = ref b2Array_Emplace(ref awakeSet.islandSims); //memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); islandDst.CopyFrom(islandSrc); } @@ -237,7 +237,7 @@ internal static void b2TrySleepIsland(B2World world, int islandId) // move body sim to sleep set int sleepBodyIndex = sleepSet.bodySims.count; - ref B2BodySim sleepBodySim = ref b2Array_Add(ref sleepSet.bodySims); + ref B2BodySim sleepBodySim = ref b2Array_Emplace(ref sleepSet.bodySims); //memcpy( sleepBodySim, awakeSim, sizeof( b2BodySim ) ); sleepBodySim.CopyFrom(awakeSim); @@ -294,7 +294,7 @@ internal static void b2TrySleepIsland(B2World world, int islandId) // move the non-touching contact to the disabled set contact.setIndex = (int)B2SolverSetType.b2_disabledSet; contact.localIndex = disabledSet.contactSims.count; - ref B2ContactSim disabledContactSim = ref b2Array_Add(ref disabledSet.contactSims); + ref B2ContactSim disabledContactSim = ref b2Array_Emplace(ref disabledSet.contactSims); //memcpy( disabledContactSim, contactSim, sizeof( b2ContactSim ) ); disabledContactSim.CopyFrom(contactSim); @@ -338,7 +338,7 @@ internal static void b2TrySleepIsland(B2World world, int islandId) B2ContactSim awakeContactSim = b2Array_Get(ref color.contactSims, localIndex); int sleepContactIndex = sleepSet.contactSims.count; - ref B2ContactSim sleepContactSim = ref b2Array_Add(ref sleepSet.contactSims); + ref B2ContactSim sleepContactSim = ref b2Array_Emplace(ref sleepSet.contactSims); //memcpy( sleepContactSim, awakeContactSim, sizeof( b2ContactSim ) ); sleepContactSim.CopyFrom(awakeContactSim); @@ -385,7 +385,7 @@ internal static void b2TrySleepIsland(B2World world, int islandId) } int sleepJointIndex = sleepSet.jointSims.count; - ref B2JointSim sleepJointSim = ref b2Array_Add(ref sleepSet.jointSims); + ref B2JointSim sleepJointSim = ref b2Array_Emplace(ref sleepSet.jointSims); //memcpy( sleepJointSim, awakeJointSim, sizeof( b2JointSim ) ); sleepJointSim.CopyFrom(awakeJointSim); @@ -411,7 +411,7 @@ internal static void b2TrySleepIsland(B2World world, int islandId) B2_ASSERT(island.setIndex == (int)B2SolverSetType.b2_awakeSet); int islandIndex = island.localIndex; - ref B2IslandSim sleepIsland = ref b2Array_Add(ref sleepSet.islandSims); + ref B2IslandSim sleepIsland = ref b2Array_Emplace(ref sleepSet.islandSims); sleepIsland.islandId = islandId; int movedIslandIndex = b2Array_RemoveSwap(ref awakeSet.islandSims, islandIndex); @@ -472,7 +472,7 @@ internal static void b2MergeSolverSets(B2World world, int setId1, int setId2) body.setIndex = setId1; body.localIndex = set1.bodySims.count; - ref B2BodySim simDst = ref b2Array_Add(ref set1.bodySims); + ref B2BodySim simDst = ref b2Array_Emplace(ref set1.bodySims); //memcpy( simDst, simSrc, sizeof( b2BodySim ) ); simDst.CopyFrom(simSrc); } @@ -490,7 +490,7 @@ internal static void b2MergeSolverSets(B2World world, int setId1, int setId2) contact.setIndex = setId1; contact.localIndex = set1.contactSims.count; - ref B2ContactSim contactDst = ref b2Array_Add(ref set1.contactSims); + ref B2ContactSim contactDst = ref b2Array_Emplace(ref set1.contactSims); //memcpy( contactDst, contactSrc, sizeof( b2ContactSim ) ); contactDst.CopyFrom(contactSrc); } @@ -508,7 +508,7 @@ internal static void b2MergeSolverSets(B2World world, int setId1, int setId2) joint.setIndex = setId1; joint.localIndex = set1.jointSims.count; - ref B2JointSim jointDst = ref b2Array_Add(ref set1.jointSims); + ref B2JointSim jointDst = ref b2Array_Emplace(ref set1.jointSims); //memcpy( jointDst, jointSrc, sizeof( b2JointSim ) ); jointDst.CopyFrom(jointSrc); } @@ -526,7 +526,7 @@ internal static void b2MergeSolverSets(B2World world, int setId1, int setId2) island.setIndex = setId1; island.localIndex = set1.islandSims.count; - ref B2IslandSim islandDst = ref b2Array_Add(ref set1.islandSims); + ref B2IslandSim islandDst = ref b2Array_Emplace(ref set1.islandSims); //memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); islandDst.CopyFrom(islandSrc); } @@ -549,7 +549,7 @@ internal static void b2TransferBody(B2World world, B2SolverSet targetSet, B2Solv B2BodySim sourceSim = b2Array_Get(ref sourceSet.bodySims, sourceIndex); int targetIndex = targetSet.bodySims.count; - ref B2BodySim targetSim = ref b2Array_Add(ref targetSet.bodySims); + ref B2BodySim targetSim = ref b2Array_Emplace(ref targetSet.bodySims); //memcpy( targetSim, sourceSim, sizeof( b2BodySim ) ); targetSim.CopyFrom(sourceSim); @@ -565,7 +565,7 @@ internal static void b2TransferBody(B2World world, B2SolverSet targetSet, B2Solv } else if (targetSet.setIndex == (int)B2SolverSetType.b2_awakeSet) { - ref B2BodyState state = ref b2Array_Add(ref targetSet.bodyStates); + ref B2BodyState state = ref b2Array_Emplace(ref targetSet.bodyStates); //*state = b2_identityBodyState; state.CopyFrom(b2_identityBodyState); state.flags = body.flags; @@ -612,7 +612,7 @@ internal static void b2TransferJoint(B2World world, B2SolverSet targetSet, B2Sol joint.localIndex = targetSet.jointSims.count; joint.colorIndex = B2_NULL_INDEX; - ref B2JointSim targetSim = ref b2Array_Add(ref targetSet.jointSims); + ref B2JointSim targetSim = ref b2Array_Emplace(ref targetSet.jointSims); //memcpy( targetSim, sourceSim, sizeof( b2JointSim ) ); targetSim.CopyFrom(sourceSim); } diff --git a/src/Box2D.NET/B2SolverStage.cs b/src/Box2D.NET/B2SolverStage.cs index 13068ae3..dcb10063 100644 --- a/src/Box2D.NET/B2SolverStage.cs +++ b/src/Box2D.NET/B2SolverStage.cs @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT @@ -10,13 +10,10 @@ namespace Box2D.NET // Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration. public class B2SolverStage { + public ArraySegment blocks; public B2SolverStageType type; - public ArraySegment blocks; public int blockCount; - - public int colorIndex; - - // todo consider false sharing of this atomic + public byte colorIndex; public B2AtomicInt completionCount; } } diff --git a/src/Box2D.NET/B2SolverStageType.cs b/src/Box2D.NET/B2SolverStageType.cs index 269b53c8..3a60bb3b 100644 --- a/src/Box2D.NET/B2SolverStageType.cs +++ b/src/Box2D.NET/B2SolverStageType.cs @@ -4,6 +4,8 @@ namespace Box2D.NET { + // Solver stages. Prepare joints and prepare contacts are split up + // because there is no need to store joint impulses. public enum B2SolverStageType { b2_stagePrepareJoints, diff --git a/src/Box2D.NET/B2Solvers.cs b/src/Box2D.NET/B2Solvers.cs index 55a0bdae..f89a1a93 100644 --- a/src/Box2D.NET/B2Solvers.cs +++ b/src/Box2D.NET/B2Solvers.cs @@ -2,10 +2,8 @@ // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT -// Compare to SDL_CPUPauseInstruction - using System; -using System.Threading.Tasks; +using System.Threading; using static Box2D.NET.B2Tables; using static Box2D.NET.B2Arrays; using static Box2D.NET.B2Atomics; @@ -102,27 +100,27 @@ public static B2Softness b2MakeSoft(float hertz, float zeta, float h) internal static void b2Pause() { - // TODO: @ikpil, check sleep or yield - Task.Yield(); + // C uses _mm_pause() / __yield(). Thread.SpinWait(1) emits the same + // pause/yield hint on x86 and ARM. Task.Yield() was a no-op here because + // the returned awaitable was never awaited. + Thread.SpinWait(1); } // Integrate velocities and apply damping - internal static void b2IntegrateVelocitiesTask(int startIndex, int endIndex, B2StepContext context) + internal static void b2IntegrateVelocitiesTask(B2SolverBlock block, B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.integrate_velocity, "IntVel", B2HexColor.b2_colorDeepPink, true); B2BodyState[] states = context.states; B2BodySim[] sims = context.sims; + B2_VALIDATE(block.startIndex + block.count <= context.world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].bodyStates.count); + B2Vec2 gravity = context.world.gravity; float h = context.h; - float maxLinearSpeed = context.maxLinearVelocity; - float maxAngularSpeed = B2_MAX_ROTATION * context.inv_dt; - float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; - float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; - for (int i = startIndex; i < endIndex; ++i) + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) { B2BodySim sim = sims[i]; B2BodyState state = states[i]; @@ -151,37 +149,6 @@ internal static void b2IntegrateVelocitiesTask(int startIndex, int endIndex, B2S v = b2MulAdd(linearVelocityDelta, linearDamping, v); w = angularVelocityDelta + angularDamping * w; - // Clamp to max linear speed - if (b2Dot(v, v) > maxLinearSpeedSquared) - { - float ratio = maxLinearSpeed / b2Length(v); - v = b2MulSV(ratio, v); - sim.flags |= (int)B2BodyFlags.b2_isSpeedCapped; - } - - // Clamp to max angular speed - if (w * w > maxAngularSpeedSquared && (sim.flags & (uint)B2BodyFlags.b2_allowFastRotation) == 0) - { - float ratio = maxAngularSpeed / b2AbsFloat(w); - w *= ratio; - sim.flags |= (uint)B2BodyFlags.b2_isSpeedCapped; - } - - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearX)) - { - v.X = 0.0f; - } - - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearY)) - { - v.Y = 0.0f; - } - - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockAngularZ)) - { - w = 0.0f; - } - state.linearVelocity = v; state.angularVelocity = w; } @@ -189,103 +156,49 @@ internal static void b2IntegrateVelocitiesTask(int startIndex, int endIndex, B2S b2TracyCZoneEnd(B2TracyCZone.integrate_velocity); } - internal static void b2PrepareJointsTask(int startIndex, int endIndex, B2StepContext context) - { - b2TracyCZoneNC(B2TracyCZone.prepare_joints, "PrepJoints", B2HexColor.b2_colorOldLace, true); - - ArraySegment joints = context.joints; - - for (int i = startIndex; i < endIndex; ++i) - { - B2JointSim joint = joints[i]; - b2PrepareJoint(joint, context); - } - - b2TracyCZoneEnd(B2TracyCZone.prepare_joints); - } - - internal static void b2WarmStartJointsTask(int startIndex, int endIndex, B2StepContext context, int colorIndex) - { - b2TracyCZoneNC(B2TracyCZone.warm_joints, "WarmJoints", B2HexColor.b2_colorGold, true); - - ref B2GraphColor color = ref context.graph.colors[colorIndex]; - B2JointSim[] joints = color.jointSims.data; - B2_ASSERT(0 <= startIndex && startIndex < color.jointSims.count); - B2_ASSERT(startIndex <= endIndex && endIndex <= color.jointSims.count); - - for (int i = startIndex; i < endIndex; ++i) - { - B2JointSim joint = joints[i]; - b2WarmStartJoint(joint, context); - } - - b2TracyCZoneEnd(B2TracyCZone.warm_joints); - } - - static void b2SolveJointsTask(int startIndex, int endIndex, B2StepContext context, int colorIndex, bool useBias, - int workerIndex) - { - b2TracyCZoneNC(B2TracyCZone.solve_joints, "SolveJoints", B2HexColor.b2_colorLemonChiffon, true); - - ref B2GraphColor color = ref context.graph.colors[colorIndex]; - B2JointSim[] joints = color.jointSims.data; - B2_ASSERT(0 <= startIndex && startIndex < color.jointSims.count); - B2_ASSERT(startIndex <= endIndex && endIndex <= color.jointSims.count); - - ref B2BitSet jointStateBitSet = ref context.world.taskContexts.data[workerIndex].jointStateBitSet; - - for (int i = startIndex; i < endIndex; ++i) - { - B2JointSim joint = joints[i]; - b2SolveJoint(joint, context, useBias); - - if (useBias && - (joint.forceThreshold < float.MaxValue || joint.torqueThreshold < float.MaxValue) && - b2GetBit(ref jointStateBitSet, joint.jointId) == false) - { - float force, torque; - b2GetJointReaction(joint, context.inv_h, out force, out torque); - - // Check thresholds. A zero threshold means all awake joints get reported. - if (force >= joint.forceThreshold || torque >= joint.torqueThreshold) - { - // Flag this joint for processing. - b2SetBit(ref jointStateBitSet, joint.jointId); - } - } - } - - b2TracyCZoneEnd(B2TracyCZone.solve_joints); - } - - internal static void b2IntegratePositionsTask(int startIndex, int endIndex, B2StepContext context) + internal static void b2IntegratePositionsTask(B2SolverBlock block, B2StepContext context) { b2TracyCZoneNC(B2TracyCZone.integrate_positions, "IntPos", B2HexColor.b2_colorDarkSeaGreen, true); + B2_VALIDATE(block.startIndex + block.count <= context.world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].bodyStates.count); + B2BodyState[] states = context.states; float h = context.h; + float maxLinearSpeed = context.maxLinearVelocity; + float maxAngularSpeed = B2_MAX_ROTATION * context.inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; - B2_ASSERT(startIndex <= endIndex); - - for (int i = startIndex; i < endIndex; ++i) + for (int i = block.startIndex; i < block.startIndex + block.count; ++i) { B2BodyState state = states[i]; - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearX)) - { - state.linearVelocity.X = 0.0f; - } + B2Vec2 v = state.linearVelocity; + float w = state.angularVelocity; + + // Motion locks - these can be viewed as a constraint that comes last + v.X = 0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearX) ? 0.0f : v.X; + v.Y = 0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearY) ? 0.0f : v.Y; + w = 0 != (state.flags & (uint)B2BodyFlags.b2_lockAngularZ) ? 0.0f : w; - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearY)) + // Clamp to max linear speed + if (b2Dot(v, v) > maxLinearSpeedSquared) { - state.linearVelocity.Y = 0.0f; + float ratio = maxLinearSpeed / b2Length(v); + v = b2MulSV(ratio, v); + state.flags |= (uint)B2BodyFlags.b2_isSpeedCapped; } - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockAngularZ)) + // Clamp to max angular speed + if (w * w > maxAngularSpeedSquared && (state.flags & (uint)B2BodyFlags.b2_allowFastRotation) == 0) { - state.angularVelocity = 0.0f; + float ratio = maxAngularSpeed / b2AbsFloat(w); + w *= ratio; + state.flags |= (uint)B2BodyFlags.b2_isSpeedCapped; } + state.linearVelocity = v; + state.angularVelocity = w; state.deltaPosition = b2MulAdd(state.deltaPosition, h, state.linearVelocity); state.deltaRotation = b2IntegrateRotation(state.deltaRotation, h * state.angularVelocity); } @@ -323,9 +236,8 @@ internal static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref return true; } - bool isSensor = shape.sensorIndex != B2_NULL_INDEX; - // Skip sensors unless the shapes want sensor events + bool isSensor = shape.sensorIndex != B2_NULL_INDEX; if (isSensor && (shape.enableSensorEvents == false || fastShape.enableSensorEvents == false)) { return true; @@ -480,7 +392,6 @@ internal static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref if (didHit && (shape.enablePreSolveEvents || fastShape.enablePreSolveEvents) && world.preSolveFcn != null) { - // Pre-solve is expensive because I need to compute a temporary manifold B2ShapeId shapeIdA = new B2ShapeId(shape.id + 1, world.worldId, shape.generation); B2ShapeId shapeIdB = new B2ShapeId(fastShape.id + 1, world.worldId, fastShape.generation); didHit = world.preSolveFcn(shapeIdA, shapeIdB, output.point, output.normal, world.preSolveContext); @@ -497,6 +408,7 @@ internal static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref return true; } + // Continuous collision of dynamic versus static internal static void b2SolveContinuous(B2World world, int bodySimIndex, B2TaskContext taskContext) { b2TracyCZoneNC(B2TracyCZone.ccd, "CCD", B2HexColor.b2_colorDarkGoldenRod, true); @@ -660,63 +572,46 @@ internal static void b2SolveContinuous(B2World world, int bodySimIndex, B2TaskCo b2TracyCZoneEnd(B2TracyCZone.ccd); } - internal static void b2FinalizeBodiesTask(int startIndex, int endIndex, int threadIndex, object context) + // Implements b2ParallelForCallback + internal static void b2FinalizeBodiesTask(int startIndex, int endIndex, int workerIndex, object context) { - b2TracyCZoneNC(B2TracyCZone.finalize_transforms, "Transforms", B2HexColor.b2_colorMediumSeaGreen, true); + b2TracyCZoneNC(B2TracyCZone.finalize_transforms, "Finalize", B2HexColor.b2_colorMediumSeaGreen, true); B2StepContext stepContext = context as B2StepContext; B2World world = stepContext.world; - - bool enableSleep = world.enableSleep; + B2Body[] bodies = world.bodies.data; B2BodyState[] states = stepContext.states; B2BodySim[] sims = stepContext.sims; - B2Body[] bodies = world.bodies.data; + + B2_ASSERT(endIndex <= world.bodyMoveEvents.count); + + bool enableSleep = world.enableSleep; + bool enableContinuous = world.enableContinuous; float timeStep = stepContext.dt; float invTimeStep = stepContext.inv_dt; - ushort worldId = world.worldId; // The body move event array should already have the correct size - B2_ASSERT(endIndex <= world.bodyMoveEvents.count); B2BodyMoveEvent[] moveEvents = world.bodyMoveEvents.data; - B2TaskContext taskContext = world.taskContexts.data[threadIndex]; + B2TaskContext taskContext = world.taskContexts.data[workerIndex]; ref B2BitSet enlargedSimBitSet = ref taskContext.enlargedSimBitSet; ref B2BitSet awakeIslandBitSet = ref taskContext.awakeIslandBitSet; - bool enableContinuous = world.enableContinuous; - float speculativeDistance = B2_SPECULATIVE_DISTANCE; - B2_ASSERT(startIndex <= endIndex); - for (int simIndex = startIndex; simIndex < endIndex; ++simIndex) { B2BodyState state = states[simIndex]; B2BodySim sim = sims[simIndex]; - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearX)) - { - state.linearVelocity.X = 0.0f; - } - - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockLinearY)) - { - state.linearVelocity.Y = 0.0f; - } - - if (0 != (state.flags & (uint)B2BodyFlags.b2_lockAngularZ)) - { - state.angularVelocity = 0.0f; - } - B2Vec2 v = state.linearVelocity; float w = state.angularVelocity; - if (b2IsValidVec2(v) == false) + if (b2IsValidVec2(v) == false || b2IsValidFloat(w) == false) { B2Body debugBody = bodies[sim.bodyId]; - b2Log($"bad body: {debugBody.name}\n"); + b2Log($"unstable: {debugBody.name}\n"); } B2_ASSERT(b2IsValidVec2(v)); @@ -733,7 +628,6 @@ internal static void b2FinalizeBodiesTask(int startIndex, int endIndex, int thre // Position correction is not as important for sleep as true velocity. float positionSleepFactor = 0.5f; - float sleepVelocity = b2MaxFloat(maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition); // reset state deltas @@ -759,14 +653,18 @@ internal static void b2FinalizeBodiesTask(int startIndex, int endIndex, int thre 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)); + body.flags |= (state.flags & (uint)(B2BodyFlags.b2_isSpeedCapped | B2BodyFlags.b2_hadTimeOfImpact)); sim.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped | (uint)B2BodyFlags.b2_hadTimeOfImpact); + state.flags &= ~((uint)B2BodyFlags.b2_isFast | (uint)B2BodyFlags.b2_isSpeedCapped | (uint)B2BodyFlags.b2_hadTimeOfImpact); if (enableSleep == false || body.enableSleep == false || sleepVelocity > body.sleepThreshold) { // Body is not sleepy body.sleepTime = 0.0f; - if (body.type == B2BodyType.b2_dynamicBody && enableContinuous && maxVelocity * timeStep > 0.5f * sim.minExtent) + const float safetyFactor = 0.5f; + float maxMotion = b2MaxFloat(maxDeltaPosition, maxVelocity * timeStep); + if (body.type == B2BodyType.b2_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim.minExtent) { // This flag is only retained for debug draw sim.flags |= (uint)B2BodyFlags.b2_isFast; @@ -869,83 +767,74 @@ internal static void b2FinalizeBodiesTask(int startIndex, int endIndex, int thre b2TracyCZoneEnd(B2TracyCZone.finalize_transforms); } -/* - public enum b2SolverStageType -{ - b2_stagePrepareJoints, - b2_stagePrepareContacts, - b2_stageIntegrateVelocities, - b2_stageWarmStart, - b2_stageSolve, - b2_stageIntegratePositions, - b2_stageRelax, - b2_stageRestitution, - b2_stageStoreImpulses -} b2SolverStageType; - -public enum b2SolverBlockType -{ - b2_bodyBlock, - b2_jointBlock, - b2_contactBlock, - b2_graphJointBlock, - b2_graphContactBlock -} b2SolverBlockType; -*/ - - // Compute the number of work blocks needed given an item count and desired block size. - // If there are too many blocks for the worker count, the block size is enlarged. - internal static int b2ComputeBlockCount(int itemCount, int defaultBlockSize, int maxBlockCount) + // A block is a range of tasks, a start index and count as a sub-array. Each worker receives at + // most M blocks of work. The workers may receive less blocks if there is not sufficient work. + // Each block of work has a minimum number of elements (block size). This in turn may limit the + // number of blocks. If there are many elements then the block size is increased so there are + // still at most M blocks of work per worker. M is a tunable number that has two goals: + // 1. keep M small to reduce overhead + // 2. keep M large enough for other workers to be able to steal work + // The block size is a power of two to make math efficient. + internal static B2BlockDim b2ComputeBlockCount(int itemCount, int minSize, int maxBlockCount) { + B2BlockDim dim = new B2BlockDim(); if (itemCount == 0) { - return 0; + return dim; } - if (itemCount > defaultBlockSize * maxBlockCount) + if (itemCount <= minSize * maxBlockCount) + { + dim.size = minSize; + } + else { - return maxBlockCount; + dim.size = (itemCount + maxBlockCount - 1) / maxBlockCount; } - return ((itemCount - 1) / defaultBlockSize) + 1; + dim.count = (itemCount + dim.size - 1) / dim.size; + + B2_ASSERT(dim.count >= 1); + B2_ASSERT(dim.size * dim.count >= itemCount); + + return dim; } // Initialize solver blocks for a contiguous range of items. Computes block size internally - // from the same parameters used by b2ComputeBlockCount. - internal static void b2InitBlocks(ArraySegment blocks, int blockCount, int itemCount, int defaultBlockSize, int maxBlockCount, B2SolverBlockType blockType) + // from the same parameters used by b2ComputeBlockCount. The atomic claim counter is zeroed + // so workers can CAS (0, 1) on the first stage that owns these blocks. + internal static void b2InitBlocks(ArraySegment blocks, B2BlockDim dim, int itemCount, B2SolverBlockType blockType, byte colorIndex) { - if (blockCount == 0) + if (dim.count == 0) { return; } + B2_ASSERT(itemCount >= dim.count); + // Compute the number of elements per block - int blockSize; - if (itemCount > defaultBlockSize * maxBlockCount) - { - blockSize = itemCount / maxBlockCount; - } - else - { - blockSize = defaultBlockSize; - } + int blockSize = dim.size; // Simulation too big B2_ASSERT(blockSize <= ushort.MaxValue); - for (int i = 0; i < blockCount; ++i) + for (int i = 0; i < dim.count; ++i) { - blocks[i].startIndex = i * blockSize; - blocks[i].count = (ushort)blockSize; - blocks[i].blockType = (short)blockType; + blocks[i].block.startIndex = i * blockSize; + blocks[i].block.count = (ushort)blockSize; + blocks[i].block.blockType = (byte)blockType; + blocks[i].block.colorIndex = colorIndex; b2AtomicStoreInt(ref blocks[i].syncIndex, 0); } // The last block may not be full - blocks[blockCount - 1].count = (ushort)(itemCount - (blockCount - 1) * blockSize); + blocks[dim.count - 1].block.count = (ushort)(itemCount - (dim.count - 1) * blockSize); + + B2_VALIDATE(blocks[dim.count - 1].block.count <= blockSize); + B2_VALIDATE((dim.count - 1) * dim.size + blocks[dim.count - 1].block.count == itemCount); } - internal static int b2InitStage(int stageIndex, ArraySegment stages, B2SolverStageType type, ArraySegment blocks, int blockCount, int colorIndex) + internal static int b2InitStage(int stageIndex, ArraySegment stages, B2SolverStageType type, ArraySegment blocks, int blockCount, byte colorIndex) { B2SolverStage stage = stages[stageIndex]; stage.type = type; @@ -957,15 +846,17 @@ internal static int b2InitStage(int stageIndex, ArraySegment stag } // Initialize one stage per color for each iteration. Used for warm start, solve, relax, and restitution. + // All iterations of a given color share the same b2SyncBlock array so the per-block syncIndex + // grows monotonically across stages within that color. internal static int b2InitColorStages(int stageIndex, ArraySegment stages, B2SolverStageType type, int iterations, - int activeColorCount, ArraySegment[] graphColorBlocks, ReadOnlySpan colorBlockCounts, + int activeColorCount, ArraySegment[] colorBlocks, ReadOnlySpan colorBlockCounts, ReadOnlySpan activeColorIndices) { for (int j = 0; j < iterations; ++j) { for (int i = 0; i < activeColorCount; ++i) { - stageIndex = b2InitStage(stageIndex, stages, type, graphColorBlocks[i], colorBlockCounts[i], activeColorIndices[i]); + stageIndex = b2InitStage(stageIndex, stages, type, colorBlocks[i], colorBlockCounts[i], (byte)activeColorIndices[i]); } } @@ -976,31 +867,29 @@ internal static void b2ExecuteBlock(B2SolverStage stage, B2StepContext context, { B2SolverStageType stageType = stage.type; B2SolverBlockType blockType = (B2SolverBlockType)block.blockType; - int startIndex = block.startIndex; - int endIndex = startIndex + block.count; switch (stageType) { case B2SolverStageType.b2_stagePrepareJoints: - b2PrepareJointsTask(startIndex, endIndex, context); + b2PrepareJointsTask(block, context); break; case B2SolverStageType.b2_stagePrepareContacts: - b2PrepareContactsTask(startIndex, endIndex, context); + b2PrepareContactsTask(block, context); break; case B2SolverStageType.b2_stageIntegrateVelocities: - b2IntegrateVelocitiesTask(startIndex, endIndex, context); + b2IntegrateVelocitiesTask(block, context); break; case B2SolverStageType.b2_stageWarmStart: if (blockType == B2SolverBlockType.b2_graphContactBlock) { - b2WarmStartContactsTask(startIndex, endIndex, context, stage.colorIndex); + b2WarmStartContactsTask(block, context); } else if (blockType == B2SolverBlockType.b2_graphJointBlock) { - b2WarmStartJointsTask(startIndex, endIndex, context, stage.colorIndex); + b2WarmStartJointsTask(block, context); } break; @@ -1009,30 +898,30 @@ internal static void b2ExecuteBlock(B2SolverStage stage, B2StepContext context, if (blockType == B2SolverBlockType.b2_graphContactBlock) { bool useBias = true; - b2SolveContactsTask(startIndex, endIndex, context, stage.colorIndex, useBias); + b2SolveContactsTask(block, context, useBias); } else if (blockType == B2SolverBlockType.b2_graphJointBlock) { bool useBias = true; - b2SolveJointsTask(startIndex, endIndex, context, stage.colorIndex, useBias, workerIndex); + b2SolveJointsTask(block, context, useBias, workerIndex); } break; case B2SolverStageType.b2_stageIntegratePositions: - b2IntegratePositionsTask(startIndex, endIndex, context); + b2IntegratePositionsTask(block, context); break; case B2SolverStageType.b2_stageRelax: if (blockType == B2SolverBlockType.b2_graphContactBlock) { bool useBias = false; - b2SolveContactsTask(startIndex, endIndex, context, stage.colorIndex, useBias); + b2SolveContactsTask(block, context, useBias); } else if (blockType == B2SolverBlockType.b2_graphJointBlock) { bool useBias = false; - b2SolveJointsTask(startIndex, endIndex, context, stage.colorIndex, useBias, workerIndex); + b2SolveJointsTask(block, context, useBias, workerIndex); } break; @@ -1040,17 +929,18 @@ internal static void b2ExecuteBlock(B2SolverStage stage, B2StepContext context, case B2SolverStageType.b2_stageRestitution: if (blockType == B2SolverBlockType.b2_graphContactBlock) { - b2ApplyRestitutionTask(startIndex, endIndex, context, stage.colorIndex); + b2ApplyRestitutionTask(block, context); } break; case B2SolverStageType.b2_stageStoreImpulses: - b2StoreImpulsesTask(startIndex, endIndex, context); + b2StoreImpulsesTask(block, context, workerIndex); break; } } + // This staggers the worker start indices so they avoid touching the same solver blocks internal static int GetWorkerStartIndex(int workerIndex, int blockCount, int workerCount) { if (blockCount <= workerCount) @@ -1063,14 +953,14 @@ internal static int GetWorkerStartIndex(int workerIndex, int blockCount, int wor return blocksPerWorker * workerIndex + b2MinInt(remainder, workerIndex); } + // Execute a stage, which is an array of solver blocks, each controlled with an atomic sync index. + // Each worker starts at its home index and sweeps the ring, CAS-claiming any unclaimed blocks. internal static void b2ExecuteStage(B2SolverStage stage, B2StepContext context, int previousSyncIndex, int syncIndex, int workerIndex) { int completedCount = 0; - ArraySegment blocks = stage.blocks; + ArraySegment blocks = stage.blocks; int blockCount = stage.blockCount; - int expectedSyncIndex = previousSyncIndex; - int startIndex = GetWorkerStartIndex(workerIndex, blockCount, context.workerCount); if (startIndex == B2_NULL_INDEX) { @@ -1080,50 +970,30 @@ internal static void b2ExecuteStage(B2SolverStage stage, B2StepContext context, B2_ASSERT(0 <= startIndex && startIndex < blockCount); int blockIndex = startIndex; - - while (b2AtomicCompareExchangeInt(ref blocks[blockIndex].syncIndex, expectedSyncIndex, syncIndex) == true) + for (int i = 0; i < blockCount; ++i) { - B2_ASSERT(stage.type != B2SolverStageType.b2_stagePrepareContacts || syncIndex < 2); - - B2_ASSERT(completedCount < blockCount); + if (b2AtomicCompareExchangeInt(ref blocks[blockIndex].syncIndex, previousSyncIndex, syncIndex)) + { + B2_ASSERT(stage.type != B2SolverStageType.b2_stagePrepareContacts || syncIndex < 2); + B2_ASSERT(completedCount < blockCount); - b2ExecuteBlock(stage, context, blocks[blockIndex], workerIndex); + // Pass the descriptor by value -- the wrapping b2SyncBlock holds the atomic + // syncIndex but we only copy .block, so the struct copy never aliases the CAS target. + b2ExecuteBlock(stage, context, blocks[blockIndex].block, workerIndex); + completedCount += 1; + } - completedCount += 1; blockIndex += 1; if (blockIndex >= blockCount) { - // Keep looking for work blockIndex = 0; } - - expectedSyncIndex = previousSyncIndex; - } - - // Search backwards for blocks - blockIndex = startIndex - 1; - while (true) - { - if (blockIndex < 0) - { - blockIndex = blockCount - 1; - } - - expectedSyncIndex = previousSyncIndex; - - if (b2AtomicCompareExchangeInt(ref blocks[blockIndex].syncIndex, expectedSyncIndex, syncIndex) == false) - { - break; - } - - b2ExecuteBlock(stage, context, blocks[blockIndex], workerIndex); - completedCount += 1; - blockIndex -= 1; } b2AtomicFetchAddInt(ref stage.completionCount, completedCount); } + // Execute a stage on worker 0 (main thread). internal static void b2ExecuteMainStage(B2SolverStage stage, B2StepContext context, uint syncBits) { int blockCount = stage.blockCount; @@ -1136,7 +1006,7 @@ internal static void b2ExecuteMainStage(B2SolverStage stage, B2StepContext conte if (blockCount == 1) { - b2ExecuteBlock(stage, context, stage.blocks[0], workerIndex); + b2ExecuteBlock(stage, context, stage.blocks[0].block, workerIndex); } else { @@ -1148,7 +1018,7 @@ internal static void b2ExecuteMainStage(B2SolverStage stage, B2StepContext conte b2ExecuteStage(stage, context, previousSyncIndex, syncIndex, workerIndex); - // todo consider using the cycle counter as well + // Spin waiting for thieves to finish while (b2AtomicLoadInt(ref stage.completionCount) != blockCount) { b2Pause(); @@ -1158,6 +1028,7 @@ internal static void b2ExecuteMainStage(B2SolverStage stage, B2StepContext conte } } + // Parallel solver task internal static void b2SolverTask(object taskContext) { B2WorkerContext workerContext = taskContext as B2WorkerContext; @@ -1169,15 +1040,29 @@ internal static void b2SolverTask(object taskContext) if (workerIndex == 0) { + // The orchestrator slot is a race. The calling thread of b2World_Step also enters here + // as worker 0, so progress is guaranteed even if the user's task system schedules tasks + // out of order, has fewer threads than workerCount, or runs the task synchronously + // inside enqueueTaskFcn. Whoever wins the CAS becomes the orchestrator; the loser + // returns and lets the spinner-only path handle workers >0. + if (b2AtomicCompareExchangeInt(ref context.mainClaimed, 0, 1) == false) + { + return; + } + // Main thread synchronizes the workers and does work itself. // - // Stages are re-used by loops so that I don't need more stages for large iteration counts. + // This single task is able to fully complete all work even if all other workers are + // blocked, so a fully serial task system still drives the simulation forward. + + // Stages are re-used by loops so that I don't need more stages for large substep counts. // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. // The stage index and sync indices are combined in to sync bits for atomic synchronization. // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This // setup makes this easy to do. /* + Stage sequence b2_stagePrepareJoints, b2_stagePrepareContacts, b2_stageIntegrateVelocities, @@ -1194,7 +1079,7 @@ internal static void b2SolverTask(object taskContext) int bodySyncIndex = 1; int stageIndex = 0; - // This stage loops over all awake joints + // Prepare joint constraints uint jointSyncIndex = 1; uint syncBits = (jointSyncIndex << 16) | (uint)stageIndex; B2_ASSERT(stages[stageIndex].type == B2SolverStageType.b2_stagePrepareJoints); @@ -1202,7 +1087,7 @@ internal static void b2SolverTask(object taskContext) stageIndex += 1; jointSyncIndex += 1; - // This stage loops over all contact constraints + // Prepare contact constraints uint contactSyncIndex = 1; syncBits = (contactSyncIndex << 16) | (uint)stageIndex; B2_ASSERT(stages[stageIndex].type == B2SolverStageType.b2_stagePrepareContacts); @@ -1210,15 +1095,13 @@ internal static void b2SolverTask(object taskContext) stageIndex += 1; contactSyncIndex += 1; - int graphSyncIndex = 1; - // Single-threaded overflow work. These constraints don't fit in the graph coloring. - // todo these could be prepared in parallel - b2PrepareOverflowJoints(context); - b2PrepareOverflowContacts(context); + b2PrepareJoints_Overflow(context); + b2PrepareContacts_Overflow(context); profile.prepareConstraints += b2GetMillisecondsAndReset(ref ticks); + int graphSyncIndex = 1; int subStepCount = context.subStepCount; for (int subStepIndex = 0; subStepIndex < subStepCount; ++subStepIndex) { @@ -1226,7 +1109,7 @@ internal static void b2SolverTask(object taskContext) // syncBits still increases monotonically because the upper bits increase each iteration int iterationStageIndex = stageIndex; - // integrate velocities + // Integrate velocities syncBits = (uint)((bodySyncIndex << 16) | iterationStageIndex); B2_ASSERT(stages[iterationStageIndex].type == B2SolverStageType.b2_stageIntegrateVelocities); b2ExecuteMainStage(stages[iterationStageIndex], context, syncBits); @@ -1235,9 +1118,9 @@ internal static void b2SolverTask(object taskContext) profile.integrateVelocities += b2GetMillisecondsAndReset(ref ticks); - // warm start constraints - b2WarmStartOverflowJoints(context); - b2WarmStartOverflowContacts(context); + // Warm start constraints + b2WarmStartJoints_Overflow(context); + b2WarmStartContacts_Overflow(context); for (int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex) { @@ -1251,14 +1134,13 @@ internal static void b2SolverTask(object taskContext) profile.warmStart += b2GetMillisecondsAndReset(ref ticks); - // solve constraints + // Solve constraints bool useBias = true; - for (int j = 0; j < ITERATIONS; ++j) { - // Overflow constraints have lower priority - b2SolveOverflowJoints(context, useBias); - b2SolveOverflowContacts(context, useBias); + // Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic. + b2SolveJoints_Overflow(context, useBias); + b2SolveContacts_Overflow(context, useBias); for (int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex) { @@ -1273,7 +1155,7 @@ internal static void b2SolverTask(object taskContext) profile.solveImpulses += b2GetMillisecondsAndReset(ref ticks); - // integrate positions + // Integrate positions B2_ASSERT(stages[iterationStageIndex].type == B2SolverStageType.b2_stageIntegratePositions); syncBits = (uint)((bodySyncIndex << 16) | iterationStageIndex); b2ExecuteMainStage(stages[iterationStageIndex], context, syncBits); @@ -1282,12 +1164,12 @@ internal static void b2SolverTask(object taskContext) profile.integratePositions += b2GetMillisecondsAndReset(ref ticks); - // relax constraints + // Relax constraints useBias = false; for (int j = 0; j < RELAX_ITERATIONS; ++j) { - b2SolveOverflowJoints(context, useBias); - b2SolveOverflowContacts(context, useBias); + b2SolveJoints_Overflow(context, useBias); + b2SolveContacts_Overflow(context, useBias); for (int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex) { @@ -1303,13 +1185,13 @@ internal static void b2SolverTask(object taskContext) profile.relaxImpulses += b2GetMillisecondsAndReset(ref ticks); } - // advance the stage according to the sub-stepping tasks just completed + // Advance the stage according to the sub-stepping tasks just completed // integrate velocities / warm start / solve / integrate positions / relax stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; // Restitution { - b2ApplyOverflowRestitution(context); + b2ApplyRestitution_Overflow(context); int iterStageIndex = stageIndex; for (int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex) @@ -1326,7 +1208,8 @@ internal static void b2SolverTask(object taskContext) profile.applyRestitution += b2GetMillisecondsAndReset(ref ticks); - b2StoreOverflowImpulses(context); + // Store impulses + b2StoreImpulses_Overflow(context); syncBits = (contactSyncIndex << 16) | (uint)stageIndex; B2_ASSERT(stages[stageIndex].type == B2SolverStageType.b2_stageStoreImpulses); @@ -1348,6 +1231,7 @@ internal static void b2SolverTask(object taskContext) { // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for // parallel simulation with graph coloring. + // todo improve this spinner uint syncBits; int spinCount = 0; while ((syncBits = b2AtomicLoadU32(ref context.atomicSyncBits)) == lastSyncBits) @@ -1395,12 +1279,12 @@ internal static void b2SolverTask(object taskContext) } } - internal static void b2BulletBodyTask(int startIndex, int endIndex, int threadIndex, object context) + internal static void b2BulletBodyTask(int startIndex, int endIndex, int workerIndex, object context) { b2TracyCZoneNC(B2TracyCZone.bullet_body_task, "Bullet", B2HexColor.b2_colorLightSkyBlue, true); B2StepContext stepContext = context as B2StepContext; - B2TaskContext taskContext = b2Array_Get(ref stepContext.world.taskContexts, threadIndex); + B2TaskContext taskContext = b2Array_Get(ref stepContext.world.taskContexts, workerIndex); B2_ASSERT(startIndex <= endIndex); @@ -1417,6 +1301,7 @@ internal static void b2BulletBodyTask(int startIndex, int endIndex, int threadIn // Solve with graph coloring internal static void b2Solve(B2World world, B2StepContext stepContext) { + // Only count steps that advance the simulation world.stepIndex += 1; // Are there any awake bodies? This scenario should not be important for profiling. @@ -1424,26 +1309,18 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) int awakeBodyCount = awakeSet.bodySims.count; if (awakeBodyCount == 0) { - // Nothing to simulate, however the tree rebuild must be finished. - if (world.userTreeTask != null) - { - world.finishTaskFcn(world.userTreeTask, world.userTaskContext); - world.userTreeTask = null; - world.activeTaskCount -= 1; - } - b2ValidateNoEnlarged(world.broadPhase); return; } // Solve constraints using graph coloring { + b2TracyCZoneNC(B2TracyCZone.solver_setup, "Solver Setup", B2HexColor.b2_colorDarkOrange, true); + ulong setupTicks = b2GetTicks(); + // Prepare buffers for bullets b2AtomicStoreInt(ref stepContext.bulletBodyCount, 0); - stepContext.bulletBodies = b2AllocateArenaItem(world.arena, awakeBodyCount, "bullet bodies"); - - b2TracyCZoneNC(B2TracyCZone.prepare_stages, "Prepare Stages", B2HexColor.b2_colorDarkOrange, true); - ulong prepareTicks = b2GetTicks(); + stepContext.bulletBodies = b2StackAlloc(world.stack, awakeBodyCount, "bullet bodies"); ref B2ConstraintGraph graph = ref world.constraintGraph; B2GraphColor[] colors = graph.colors; @@ -1452,7 +1329,6 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) stepContext.states = awakeSet.bodyStates.data; // count contacts, joints, and colors - int awakeJointCount = 0; int activeColorCount = 0; for (int i = 0; i < B2_GRAPH_COLOR_COUNT - 1; ++i) { @@ -1460,7 +1336,6 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) int perColorJointCount = colors[i].jointSims.count; int occupancyCount = perColorContactCount + perColorJointCount; activeColorCount += occupancyCount > 0 ? 1 : 0; - awakeJointCount += perColorJointCount; } // prepare for move events @@ -1468,12 +1343,15 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) int workerCount = world.workerCount; - // 4 is a small power of two that allows for meaningful work stealing - const int blocksPerWorker = 4; - int maxBlockCount = blocksPerWorker * workerCount; + // Target 4 blocks per worker to allow work stealing + int maxBlockCount = 4 * workerCount; - // Configure blocks for tasks that parallel-for bodies - int bodyBlockCount = b2ComputeBlockCount(awakeBodyCount, 1 << 5, maxBlockCount); + // Body blocks are for parallel iteration over bodies directly (integration, update transforms) + int minBodiesPerBlock = 32; + B2BlockDim bodyDim = b2ComputeBlockCount(awakeBodyCount, minBodiesPerBlock, maxBlockCount); + + const int minContactsPerBlock = 4; + const int minJointsPerBlock = 4; B2_ASSERT(B2FixedArray24.Size == B2_GRAPH_COLOR_COUNT); @@ -1482,65 +1360,76 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) B2FixedArray24 arrayActiveColorIndices = new B2FixedArray24(); B2FixedArray24 arrayColorContactCounts = new B2FixedArray24(); B2FixedArray24 arrayColorJointCounts = new B2FixedArray24(); - B2FixedArray24 arrayColorBlockCounts = new B2FixedArray24(); + B2FixedArray24 arrayGraphContactDims = new B2FixedArray24(); + B2FixedArray24 arrayGraphJointDims = new B2FixedArray24(); Span activeColorIndices = arrayActiveColorIndices.AsSpan(); Span colorContactCounts = arrayColorContactCounts.AsSpan(); Span colorJointCounts = arrayColorJointCounts.AsSpan(); - Span colorBlockCounts = arrayColorBlockCounts.AsSpan(); + Span graphContactDims = arrayGraphContactDims.AsSpan(); + Span graphJointDims = arrayGraphJointDims.AsSpan(); int graphBlockCount = 0; // c is the active color index int wideContactCount = 0; + int jointCount = 0; int c = 0; for (int i = 0; i < B2_GRAPH_COLOR_COUNT - 1; ++i) { int colorContactCount = colors[i].contactSims.count; int colorJointCount = colors[i].jointSims.count; - if (colorContactCount + colorJointCount > 0) + if (colorContactCount + colorJointCount == 0) { - activeColorIndices[c] = i; + continue; + } - // Ceiling for wide constraint count - int colorContactCountW = colorContactCount > 0 ? ((colorContactCount - 1) >> B2_SIMD_SHIFT) + 1 : 0; - colorContactCounts[c] = colorContactCountW; - colorJointCounts[c] = colorJointCount; + activeColorIndices[c] = i; - int colorContactBlockCount = b2ComputeBlockCount(colorContactCountW, blocksPerWorker, maxBlockCount); - int colorJointBlockCount = b2ComputeBlockCount(colorJointCount, blocksPerWorker, maxBlockCount); - colorBlockCounts[c] = colorContactBlockCount + colorJointBlockCount; + // Ceiling for wide constraint count + int colorContactCountW = colorContactCount > 0 ? ((colorContactCount - 1) >> B2_SIMD_SHIFT) + 1 : 0; + wideContactCount += colorContactCountW; + colorContactCounts[c] = colorContactCountW; - graphBlockCount += colorBlockCounts[c]; - wideContactCount += colorContactCountW; - c += 1; - } + colorJointCounts[c] = colorJointCount; + jointCount += colorJointCount; + + // Graph solver block dimensions + graphContactDims[c] = b2ComputeBlockCount(colorContactCountW, minContactsPerBlock, maxBlockCount); + graphJointDims[c] = b2ComputeBlockCount(colorJointCount, minJointsPerBlock, maxBlockCount); + graphBlockCount += graphContactDims[c].count + graphJointDims[c].count; + + c += 1; } activeColorCount = c; - // Gather contact pointers for easy parallel-for traversal. Some may be NULL due to SIMD remainders. - ArraySegment contacts = - b2AllocateArenaItem(world.arena, B2_SIMD_WIDTH * wideContactCount, "contact pointers"); - - // Gather joint pointers for easy parallel-for traversal. - ArraySegment joints = - b2AllocateArenaItem(world.arena, awakeJointCount, "joint pointers"); + // Prepare and store run as one flat parallel-for over the entire wide constraint range, + // partitioned into uniformly sized blocks. Color info is consulted inside the task via + // a small span array, so blocks do not need to honor color boundaries here. + B2BlockDim contactPrepareDim = b2ComputeBlockCount(wideContactCount, minContactsPerBlock, maxBlockCount); + B2BlockDim jointPrepareDim = b2ComputeBlockCount(jointCount, minJointsPerBlock, maxBlockCount); B2_ASSERT(B2FixedArray4.Size == B2_SIMD_WIDTH); int wideContactConstraintByteCount = b2GetWideContactConstraintByteCount(); ArraySegment wideContactConstraints = - b2AllocateArenaItem(world.arena, wideContactCount /** wideContactConstraintByteCount */, "contact constraint"); + b2StackAlloc(world.stack, wideContactCount /** wideContactConstraintByteCount */, "contact constraint"); - int overflowContactCount = colors[B2_OVERFLOW_INDEX].contactSims.count; - ArraySegment overflowContactConstraints = b2AllocateArenaItem( - world.arena, overflowContactCount, "overflow contact constraint"); + ref B2GraphColor overflow = ref colors[B2_OVERFLOW_INDEX]; + int overflowCount = overflow.contactSims.count; + ArraySegment overflowContacts = + b2StackAlloc(world.stack, overflowCount, "overflow contact constraint"); + overflow.overflowConstraints = overflowContacts; - graph.colors[B2_OVERFLOW_INDEX].overflowConstraints = overflowContactConstraints; + // Build the span table for the flat prepare/store parallel-for while I slice the + // wide constraint buffer across colors. One entry per active color plus a sentinel + // at wideContactCount. + B2ContactPrepareSpan[] contactPrepareSpans = new B2ContactPrepareSpan[B2_GRAPH_COLOR_COUNT + 1]; + B2JointPrepareSpan[] jointPrepareSpans = new B2JointPrepareSpan[B2_GRAPH_COLOR_COUNT + 1]; - // Distribute transient constraints to each graph color and build flat arrays of contact and joint pointers + // Distribute transient constraints to each graph color and prepare spans { - int contactBase = 0; + int wideBase = 0; int jointBase = 0; for (int i = 0; i < activeColorCount; ++i) { @@ -1548,50 +1437,49 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) ref B2GraphColor color = ref colors[j]; int colorContactCount = color.contactSims.count; + contactPrepareSpans[i].start = wideBase; + contactPrepareSpans[i].count = colorContactCount; + contactPrepareSpans[i].contacts = color.contactSims.data; if (colorContactCount == 0) { color.wideConstraints = null; + color.wideConstraintCount = 0; } else { - color.wideConstraints = wideContactConstraints.Slice(contactBase); - - // Flat array of contacts - for (int k = 0; k < colorContactCount; ++k) - { - contacts[B2_SIMD_WIDTH * contactBase + k] = color.contactSims.data[k]; - } + color.wideConstraints = wideContactConstraints.Slice(wideBase); - // remainder int colorContactCountW = ((colorContactCount - 1) >> B2_SIMD_SHIFT) + 1; - for (int k = colorContactCount; k < B2_SIMD_WIDTH * colorContactCountW; ++k) + color.wideConstraintCount = colorContactCountW; + + // Zero remainder lanes in the tail wide slot so prepare workers don't need to + // initialize them. + if ((colorContactCount & (B2_SIMD_WIDTH - 1)) != 0) { - contacts[B2_SIMD_WIDTH * contactBase + k] = null; + color.wideConstraints[colorContactCountW - 1] = new B2ContactConstraintWide(); } - contactBase += colorContactCountW; - } - - // Flat array of joints - int colorJointCount = color.jointSims.count; - for (int k = 0; k < colorJointCount; ++k) - { - joints[jointBase + k] = color.jointSims.data[k]; + wideBase += colorContactCountW; } - jointBase += colorJointCount; + jointPrepareSpans[i].start = jointBase; + jointPrepareSpans[i].count = color.jointSims.count; + jointPrepareSpans[i].joints = color.jointSims.data; + jointBase += color.jointSims.count; } - B2_ASSERT(contactBase == wideContactCount); - B2_ASSERT(jointBase == awakeJointCount); - } - - // Define work blocks for preparing contacts and storing contact impulses - int contactBlockCount = b2ComputeBlockCount(wideContactCount, blocksPerWorker, maxBlockCount); + // Sentinel + contactPrepareSpans[activeColorCount].start = wideContactCount; + contactPrepareSpans[activeColorCount].count = 0; + contactPrepareSpans[activeColorCount].contacts = null; + B2_ASSERT(wideBase == wideContactCount); - // Define work blocks for preparing joints - int jointBlockCount = b2ComputeBlockCount(awakeJointCount, blocksPerWorker, maxBlockCount); + jointPrepareSpans[activeColorCount].start = jointCount; + jointPrepareSpans[activeColorCount].count = 0; + jointPrepareSpans[activeColorCount].joints = null; + B2_ASSERT(jointBase == jointCount); + } int stageCount = 0; @@ -1614,11 +1502,11 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) // b2_stageStoreImpulses stageCount += 1; - ArraySegment stages = b2AllocateArenaItem(world.arena, stageCount, "stages"); - ArraySegment bodyBlocks = b2AllocateArenaItem(world.arena, bodyBlockCount, "body blocks"); - ArraySegment contactBlocks = b2AllocateArenaItem(world.arena, contactBlockCount, "contact blocks"); - ArraySegment jointBlocks = b2AllocateArenaItem(world.arena, jointBlockCount, "joint blocks"); - ArraySegment graphBlocks = b2AllocateArenaItem(world.arena, graphBlockCount, "graph blocks"); + ArraySegment stages = b2StackAlloc(world.stack, stageCount, "stages"); + ArraySegment bodyBlocks = b2StackAlloc(world.stack, bodyDim.count, "body blocks"); + ArraySegment contactBlocks = b2StackAlloc(world.stack, contactPrepareDim.count, "contact blocks"); + ArraySegment jointBlocks = b2StackAlloc(world.stack, jointPrepareDim.count, "joint blocks"); + ArraySegment graphBlocks = b2StackAlloc(world.stack, graphBlockCount, "graph blocks"); // Split an awake island. This modifies: // - stack allocator @@ -1641,48 +1529,50 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } } - // Prepare body, joint, and contact work blocks - b2InitBlocks(bodyBlocks, bodyBlockCount, awakeBodyCount, 1 << 5, maxBlockCount, B2SolverBlockType.b2_bodyBlock); - b2InitBlocks(jointBlocks, jointBlockCount, awakeJointCount, blocksPerWorker, maxBlockCount, B2SolverBlockType.b2_jointBlock); - b2InitBlocks(contactBlocks, contactBlockCount, wideContactCount, blocksPerWorker, maxBlockCount, B2SolverBlockType.b2_contactBlock); + // Prepare body blocks + b2InitBlocks(bodyBlocks, bodyDim, awakeBodyCount, B2SolverBlockType.b2_bodyBlock, byte.MaxValue); - // Prepare graph work blocks. Each color gets joint blocks followed by contact blocks. - ArraySegment[] graphColorBlocks = new ArraySegment[B2_GRAPH_COLOR_COUNT]; - ArraySegment baseGraphBlock = graphBlocks; + // Prepare blocks as a single flat parallel-for over the whole constraint range. + // The task walks spans to decode flat slot indices back to per-color arrays. + b2InitBlocks(contactBlocks, contactPrepareDim, wideContactCount, B2SolverBlockType.b2_contactBlock, byte.MaxValue); + b2InitBlocks(jointBlocks, jointPrepareDim, jointCount, B2SolverBlockType.b2_jointBlock, byte.MaxValue); + // Prepare graph work blocks. Each color gets joint blocks followed by contact blocks. + ArraySegment[] graphColorBlocks = new ArraySegment[B2_GRAPH_COLOR_COUNT]; + ArraySegment baseGraphBlock = graphBlocks; + B2FixedArray24 arrayGraphBlockCounts = new B2FixedArray24(); + Span graphBlockCounts = arrayGraphBlockCounts.AsSpan(); for (int i = 0; i < activeColorCount; ++i) { graphColorBlocks[i] = baseGraphBlock; - int count; - count = b2ComputeBlockCount(colorJointCounts[i], blocksPerWorker, maxBlockCount); - b2InitBlocks(baseGraphBlock, count, colorJointCounts[i], blocksPerWorker, maxBlockCount, B2SolverBlockType.b2_graphJointBlock); - baseGraphBlock = baseGraphBlock.Slice(count); + byte colorIndex = (byte)activeColorIndices[i]; + b2InitBlocks(baseGraphBlock, graphJointDims[i], colorJointCounts[i], B2SolverBlockType.b2_graphJointBlock, colorIndex); + baseGraphBlock = baseGraphBlock.Slice(graphJointDims[i].count); + + b2InitBlocks(baseGraphBlock, graphContactDims[i], colorContactCounts[i], B2SolverBlockType.b2_graphContactBlock, colorIndex); + baseGraphBlock = baseGraphBlock.Slice(graphContactDims[i].count); - count = b2ComputeBlockCount(colorContactCounts[i], blocksPerWorker, maxBlockCount); - b2InitBlocks(baseGraphBlock, count, colorContactCounts[i], blocksPerWorker, maxBlockCount, B2SolverBlockType.b2_graphContactBlock); - baseGraphBlock = baseGraphBlock.Slice(count); + graphBlockCounts[i] = graphJointDims[i].count + graphContactDims[i].count; } B2_ASSERT((baseGraphBlock.Offset - graphBlocks.Offset) == graphBlockCount); int stageIdx = 0; - stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stagePrepareJoints, jointBlocks, jointBlockCount, -1); - stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stagePrepareContacts, contactBlocks, contactBlockCount, -1); - stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageIntegrateVelocities, bodyBlocks, bodyBlockCount, -1); + stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stagePrepareJoints, jointBlocks, jointPrepareDim.count, byte.MaxValue); + stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stagePrepareContacts, contactBlocks, contactPrepareDim.count, byte.MaxValue); + stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageIntegrateVelocities, bodyBlocks, bodyDim.count, byte.MaxValue); stageIdx = b2InitColorStages(stageIdx, stages, B2SolverStageType.b2_stageWarmStart, 1, activeColorCount, graphColorBlocks, - colorBlockCounts, activeColorIndices); + graphBlockCounts, activeColorIndices); stageIdx = b2InitColorStages(stageIdx, stages, B2SolverStageType.b2_stageSolve, ITERATIONS, activeColorCount, graphColorBlocks, - colorBlockCounts, activeColorIndices); - stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageIntegratePositions, bodyBlocks, bodyBlockCount, -1); + graphBlockCounts, activeColorIndices); + stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageIntegratePositions, bodyBlocks, bodyDim.count, byte.MaxValue); stageIdx = b2InitColorStages(stageIdx, stages, B2SolverStageType.b2_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, - colorBlockCounts, activeColorIndices); - // Note: joint blocks mixed in, could have joint limit restitution + graphBlockCounts, activeColorIndices); stageIdx = b2InitColorStages(stageIdx, stages, B2SolverStageType.b2_stageRestitution, 1, activeColorCount, graphColorBlocks, - colorBlockCounts, activeColorIndices); - stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageStoreImpulses, contactBlocks, contactBlockCount, -1); + graphBlockCounts, activeColorIndices); + stageIdx = b2InitStage(stageIdx, stages, B2SolverStageType.b2_stageStoreImpulses, contactBlocks, contactPrepareDim.count, byte.MaxValue); - //B2_ASSERT( (int)( stage - stages ) == stageCount ); B2_ASSERT((int)(stageIdx) == stageCount); B2_ASSERT(workerCount <= B2_MAX_WORKERS); @@ -1695,27 +1585,31 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } stepContext.graph = graph; - stepContext.joints = joints; - stepContext.contacts = contacts; - stepContext.wideContactConstraints = wideContactConstraints; stepContext.activeColorCount = activeColorCount; stepContext.workerCount = workerCount; stepContext.stageCount = stageCount; stepContext.stages = stages; + stepContext.wideContactConstraints = wideContactConstraints; + stepContext.contactPrepareSpans = contactPrepareSpans; + stepContext.wideContactCount = wideContactCount; + stepContext.jointPrepareSpans = jointPrepareSpans; b2AtomicStoreU32(ref stepContext.atomicSyncBits, 0); + b2AtomicStoreInt(ref stepContext.mainClaimed, 0); - world.profile.prepareStages = b2GetMillisecondsAndReset(ref prepareTicks); - b2TracyCZoneEnd(B2TracyCZone.prepare_stages); + world.profile.solverSetup = b2GetMillisecondsAndReset(ref setupTicks); + b2TracyCZoneEnd(B2TracyCZone.solver_setup); b2TracyCZoneNC(B2TracyCZone.solve_constraints, "Solve Constraints", B2HexColor.b2_colorIndigo, true); ulong constraintTicks = b2GetTicks(); - // Must use worker index because thread 0 can be assigned multiple tasks int jointIdCapacity = b2GetIdCapacity(world.jointIdPool); + int contactIdCapacity = b2GetIdCapacity(world.contactIdPool); for (int i = 0; i < workerCount; ++i) { B2TaskContext taskContext = b2Array_Get(ref world.taskContexts, i); b2SetBitCountAndClear(ref taskContext.jointStateBitSet, jointIdCapacity); + b2SetBitCountAndClear(ref taskContext.hitEventBitSet, contactIdCapacity); + taskContext.hasHitEvents = false; workerContext[i].context = stepContext; workerContext[i].workerIndex = i; @@ -1733,14 +1627,16 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } } - // Finish island split - if (splitIslandTask != null) - { - world.finishTaskFcn(splitIslandTask, world.userTaskContext); - world.activeTaskCount -= 1; - } - - world.splitIslandId = B2_NULL_INDEX; + // The calling thread of b2World_Step also enters b2SolverTask as worker 0 and races for the + // orchestrator slot via the CAS inside. This guarantees progress even when the user's task + // system can't run the queued worker 0 promptly: it might schedule out of order, have fewer + // threads than workerCount, or invert priority by parking the calling thread in finishTaskFcn. + // Whoever wins the CAS becomes the orchestrator; the loser returns and lets the spinner-only + // path handle workers >0. + B2WorkerContext callerContext = new B2WorkerContext(); + callerContext.context = stepContext; + callerContext.workerIndex = 0; + b2SolverTask(callerContext); // Finish constraint solve for (int i = 0; i < workerCount; ++i) @@ -1752,7 +1648,16 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } } - world.profile.solveConstraints = b2GetMillisecondsAndReset(ref constraintTicks); + // Finish island split + if (splitIslandTask != null) + { + world.finishTaskFcn(splitIslandTask, world.userTaskContext); + world.activeTaskCount -= 1; + } + + world.splitIslandId = B2_NULL_INDEX; + + world.profile.constraints = b2GetMillisecondsAndReset(ref constraintTicks); b2TracyCZoneEnd(B2TracyCZone.solve_constraints); b2TracyCZoneNC(B2TracyCZone.update_transforms, "Update Transforms", B2HexColor.b2_colorMediumSeaGreen, true); @@ -1773,15 +1678,13 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) // Finalize bodies. Must happen after the constraint solver and after island splitting. b2ParallelFor(world, b2FinalizeBodiesTask, awakeBodyCount, 64, stepContext); - b2FreeArenaItem(world.arena, graphBlocks); - b2FreeArenaItem(world.arena, jointBlocks); - b2FreeArenaItem(world.arena, contactBlocks); - b2FreeArenaItem(world.arena, bodyBlocks); - b2FreeArenaItem(world.arena, stages); - b2FreeArenaItem(world.arena, overflowContactConstraints); - b2FreeArenaItem(world.arena, wideContactConstraints); - b2FreeArenaItem(world.arena, joints); - b2FreeArenaItem(world.arena, contacts); + b2StackFree(world.stack, graphBlocks); + b2StackFree(world.stack, jointBlocks); + b2StackFree(world.stack, contactBlocks); + b2StackFree(world.stack, bodyBlocks); + b2StackFree(world.stack, stages); + b2StackFree(world.stack, overflowContacts); + b2StackFree(world.stack, wideContactConstraints); world.profile.transforms = b2GetMilliseconds(transformTicks); b2TracyCZoneEnd(B2TracyCZone.update_transforms); @@ -1837,68 +1740,105 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } // Report hit events - // todo_erin perhaps optimize this with a bitset - // todo_erin perhaps do this in parallel with other work below { b2TracyCZoneNC(B2TracyCZone.hit_events, "Hit Events", B2HexColor.b2_colorRosyBrown, true); ulong hitTicks = b2GetTicks(); B2_ASSERT(world.contactHitEvents.count == 0); - float threshold = world.hitEventThreshold; - B2GraphColor[] colors = world.constraintGraph.colors; - for (int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i) + // Fast path: if no worker flagged any hit-event candidates during b2StoreImpulsesTask, skip entirely. + bool anyHitEvents = false; + for (int i = 0; i < world.workerCount; ++i) + { + if (world.taskContexts.data[i].hasHitEvents) + { + anyHitEvents = true; + break; + } + } + + if (anyHitEvents) { - ref B2GraphColor color = ref colors[i]; - int contactCount = color.contactSims.count; - B2ContactSim[] contactSims = color.contactSims.data; - for (int j = 0; j < contactCount; ++j) + // Union per-worker bits into worker 0's bit set. + ref B2BitSet hitEventBitSet = ref world.taskContexts.data[0].hitEventBitSet; + for (int i = 1; i < world.workerCount; ++i) { - B2ContactSim contactSim = contactSims[j]; - if ((contactSim.simFlags & (uint)B2ContactSimFlags.b2_simEnableHitEvent) == 0) + if (world.taskContexts.data[i].hasHitEvents) { - continue; + b2InPlaceUnion(ref hitEventBitSet, ref world.taskContexts.data[i].hitEventBitSet); } + } - B2ContactHitEvent @event = new B2ContactHitEvent(); - @event.approachSpeed = threshold; + float threshold = world.hitEventThreshold; + B2GraphColor[] colors = world.constraintGraph.colors; + B2Contact[] contactArray = world.contacts.data; + B2Shape[] shapeArray = world.shapes.data; + ushort worldId = world.worldId; - bool hit = false; - int pointCount = contactSim.manifold.pointCount; - for (int k = 0; k < pointCount; ++k) + uint wordCount = (uint)hitEventBitSet.blockCount; + ulong[] bits = hitEventBitSet.bits; + for (uint k = 0; k < wordCount; ++k) + { + ulong word = bits[k]; + while (word != 0) { - ref B2ManifoldPoint mp = ref contactSim.manifold.points[k]; - float approachSpeed = -mp.normalVelocity; + uint ctz = b2CTZ64(word); + int contactId = (int)(64 * k + ctz); + + B2_ASSERT(contactId < world.contacts.capacity); - // Need to check total impulse because the point may be speculative and not colliding - if (approachSpeed > @event.approachSpeed && mp.totalNormalImpulse > 0.0f) + B2Contact contact = contactArray[contactId]; + + B2_ASSERT(contact.setIndex == (int)B2SolverSetType.b2_awakeSet); + B2_ASSERT(contact.colorIndex != B2_NULL_INDEX); + B2_ASSERT(contact.localIndex != B2_NULL_INDEX); + + B2ContactSim contactSim = colors[contact.colorIndex].contactSims.data[contact.localIndex]; + + B2ContactHitEvent @event = new B2ContactHitEvent(); + @event.approachSpeed = threshold; + + bool hit = false; + int pointCount = contactSim.manifold.pointCount; + for (int j = 0; j < pointCount; ++j) { - @event.approachSpeed = approachSpeed; - @event.point = mp.clipPoint; - hit = true; + ref B2ManifoldPoint mp = ref contactSim.manifold.points[j]; + float approachSpeed = -mp.normalVelocity; + + // Need to check total impulse because the point may be speculative and not colliding + if (approachSpeed > @event.approachSpeed && mp.totalNormalImpulse > 0.0f) + { + @event.approachSpeed = approachSpeed; + // Using the clip point here is somewhat questionable + @event.point = mp.clipPoint; + hit = true; + } } - } - if (hit == true) - { - @event.normal = contactSim.manifold.normal; + B2_VALIDATE(hit); - B2Shape shapeA = b2Array_Get(ref world.shapes, contactSim.shapeIdA); - B2Shape shapeB = b2Array_Get(ref world.shapes, contactSim.shapeIdB); + if (hit == true) + { + @event.normal = contactSim.manifold.normal; - @event.shapeIdA = new B2ShapeId(shapeA.id + 1, world.worldId, shapeA.generation); - @event.shapeIdB = new B2ShapeId(shapeB.id + 1, world.worldId, shapeB.generation); + B2Shape shapeA = shapeArray[contactSim.shapeIdA]; + B2Shape shapeB = shapeArray[contactSim.shapeIdB]; - B2Contact contact = b2Array_Get(ref world.contacts, contactSim.contactId); + @event.shapeIdA = new B2ShapeId(shapeA.id + 1, worldId, shapeA.generation); + @event.shapeIdB = new B2ShapeId(shapeB.id + 1, worldId, shapeB.generation); - @event.contactId = new B2ContactId( - index1: contact.contactId + 1, - world0: world.worldId, - padding: 0, - generation: contact.generation - ); + @event.contactId = new B2ContactId( + index1: contact.contactId + 1, + world0: worldId, + padding: 0, + generation: contact.generation + ); - b2Array_Push(ref world.contactHitEvents, @event); + b2Array_Push(ref world.contactHitEvents, @event); + } + + // Clear the smallest set bit + word = word & (word - 1); } } } @@ -2072,7 +2012,7 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } // Need to free this even if no bullets got processed. - b2FreeArenaItem(world.arena, stepContext.bulletBodies); + b2StackFree(world.stack, stepContext.bulletBodies); stepContext.bulletBodies = null; b2AtomicStoreInt(ref stepContext.bulletBodyCount, 0); @@ -2169,4 +2109,4 @@ internal static void b2Solve(B2World world, B2StepContext stepContext) } } } -} \ No newline at end of file +} diff --git a/src/Box2D.NET/B2ArenaAllocatorTyped.cs b/src/Box2D.NET/B2Stack.cs similarity index 87% rename from src/Box2D.NET/B2ArenaAllocatorTyped.cs rename to src/Box2D.NET/B2Stack.cs index a896e420..2f4e1a16 100644 --- a/src/Box2D.NET/B2ArenaAllocatorTyped.cs +++ b/src/Box2D.NET/B2Stack.cs @@ -10,11 +10,11 @@ namespace Box2D.NET { // This is a stack-like arena allocator used for fast per step allocations. - // You must nest allocate/free pairs. The code will Debug.Assert + // You must nest allocate/free pairs. The code will B2_ASSERT // if you try to interleave multiple allocate/free pairs. // This allocator uses the heap if space is insufficient. // I could remove the need to free entries individually. - public class B2ArenaAllocatorTyped : IB2ArenaAllocatable where T : new() + public class B2Stack : IB2ArenaAllocatable where T : new() { public ArraySegment data; public int capacity { get; set; } @@ -22,7 +22,7 @@ namespace Box2D.NET public int allocation { get; set; } public int maxAllocation { get; set; } - public B2Array> entries; + public B2Array> entries; public int Grow() { diff --git a/src/Box2D.NET/B2ArenaAllocator.cs b/src/Box2D.NET/B2StackAllocator.cs similarity index 90% rename from src/Box2D.NET/B2ArenaAllocator.cs rename to src/Box2D.NET/B2StackAllocator.cs index 446fa042..1431a4a6 100644 --- a/src/Box2D.NET/B2ArenaAllocator.cs +++ b/src/Box2D.NET/B2StackAllocator.cs @@ -11,7 +11,7 @@ namespace Box2D.NET // if you try to interleave multiple allocate/free pairs. // This allocator uses the heap if space is insufficient. // I could remove the need to free entries individually. - public class B2ArenaAllocator + public class B2StackAllocator { private readonly object _lock; @@ -21,7 +21,7 @@ public class B2ArenaAllocator public int Count => _allocators.Length; - public B2ArenaAllocator(int capacity) + public B2StackAllocator(int capacity) { _lock = new object(); _capacity = capacity; @@ -29,7 +29,7 @@ public B2ArenaAllocator(int capacity) _allocators = Array.Empty(); } - public B2ArenaAllocatorTyped GetOrCreateFor() where T : new() + public B2Stack GetOrCreateFor() where T : new() { var index = B2ArenaAllocatorIndexer.Index(); if (_lookup.Length <= index || null == _lookup[index]) @@ -45,7 +45,7 @@ public B2ArenaAllocator(int capacity) // new if (null == _lookup[index]) { - var newAllocator = B2ArenaAllocators.b2CreateArenaAllocator(_capacity); + var newAllocator = B2ArenaAllocators.b2CreateStack(_capacity); _lookup[index] = newAllocator; // @@ -56,7 +56,7 @@ public B2ArenaAllocator(int capacity) } } - return _lookup[index] as B2ArenaAllocatorTyped; + return _lookup[index] as B2Stack; } private static IB2ArenaAllocatable[] Resize(IB2ArenaAllocatable[] source, int count) diff --git a/src/Box2D.NET/B2ArenaEntry.cs b/src/Box2D.NET/B2StackEntry.cs similarity index 90% rename from src/Box2D.NET/B2ArenaEntry.cs rename to src/Box2D.NET/B2StackEntry.cs index bec2bafa..751ac82d 100644 --- a/src/Box2D.NET/B2ArenaEntry.cs +++ b/src/Box2D.NET/B2StackEntry.cs @@ -6,7 +6,7 @@ namespace Box2D.NET { - public struct B2ArenaEntry + public struct B2StackEntry { public ArraySegment data; public string name; diff --git a/src/Box2D.NET/B2StepContext.cs b/src/Box2D.NET/B2StepContext.cs index 6aacd979..8901936d 100644 --- a/src/Box2D.NET/B2StepContext.cs +++ b/src/Box2D.NET/B2StepContext.cs @@ -44,17 +44,21 @@ public class B2StepContext // TODO: @ikpil, check struct or class public ArraySegment bulletBodies; public B2AtomicInt bulletBodyCount; - // joint pointers for simplified parallel-for access. - public ArraySegment joints; - // contact pointers for simplified parallel-for access. - // - parallel-for collide with no gaps - // - parallel-for prepare and store contacts with NULL gaps for SIMD remainders - // despite being an array of pointers, these are contiguous sub-arrays corresponding - // to constraint graph colors - public ArraySegment contacts; + // - parallel-for collide with no gaps, includes touching and non-touching + public ArraySegment contactSims; + // Flat view of the wide contact constraint array used by prepare and store. + // prepareSpans has activeColorCount + 1 entries, the last being a sentinel + // at wideContactCount. wideContactConstraints is the contiguous base + // pointer; per-color slices live at colors[i].wideConstraints. public ArraySegment wideContactConstraints; + public B2ContactPrepareSpan[] contactPrepareSpans; + public int wideContactCount; + + public B2JointPrepareSpan[] jointPrepareSpans; + public int jointCount; + public int activeColorCount; public int workerCount; @@ -62,12 +66,27 @@ public class B2StepContext // TODO: @ikpil, check struct or class public int stageCount; public bool enableWarmStarting; - // todo padding to prevent false sharing - public B2FixedArray64 dummy1; + // padding to prevent false sharing + public B2FixedArray64 padding1; + // This atomic is central to multi-threaded solver task synchronization. + // It prevents ABA problems by monotonically growing as the solver advances. + // This means a delayed worker thread will catch up without repeating already completed + // work (causing a race condition). // sync index (16-bits) | stage type (16-bits) public B2AtomicU32 atomicSyncBits; - public B2FixedArray64 dummy2; + // padding to prevent false sharing + public B2FixedArray64 padding2; + + // Race flag claimed by whichever runner reaches b2SolverTask with workerIndex 0 first. + // The calling thread of b2World_Step also races for this slot so the orchestrator can + // always make progress, regardless of how the user's task system schedules tasks (out + // of order, fewer threads than workers, or synchronously inside enqueueTaskFcn). The + // loser of the race no-ops as workerIndex 0. + public B2AtomicInt mainClaimed; + + // padding to prevent false sharing + public B2FixedArray64 padding3; } } diff --git a/src/Box2D.NET/B2SyncBlock.cs b/src/Box2D.NET/B2SyncBlock.cs new file mode 100644 index 00000000..614355b7 --- /dev/null +++ b/src/Box2D.NET/B2SyncBlock.cs @@ -0,0 +1,16 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +namespace Box2D.NET +{ + // A unit of multithreaded work along with atomic synchronization. The syncIndex grows + // monotonically allowing the solver block to be re-used across sub-steps. + // TODO: @ikpil, this is a struct in C. It is a class here so the atomic syncIndex + // can be passed by reference out of an array. + public class B2SyncBlock + { + public B2SolverBlock block; + public B2AtomicInt syncIndex; + } +} diff --git a/src/Box2D.NET/B2TaskContext.cs b/src/Box2D.NET/B2TaskContext.cs index 8efb069a..53c4a899 100644 --- a/src/Box2D.NET/B2TaskContext.cs +++ b/src/Box2D.NET/B2TaskContext.cs @@ -13,6 +13,12 @@ public class B2TaskContext // These bits align with the contact id capacity and signal a change in contact status public B2BitSet contactStateBitSet; + // These bits align with the contact id capacity and signal a hit event. + public B2BitSet hitEventBitSet; + + // Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step. + public bool hasHitEvents; + // These bits align with the joint id capacity and signal a change in contact status public B2BitSet jointStateBitSet; @@ -26,5 +32,8 @@ public class B2TaskContext // Per worker split island candidate public float splitSleepTime; public int splitIslandId; + + // Number of contacts recycled this step (collide pass). + public int recycledContactCount; } } diff --git a/src/Box2D.NET/B2TracyCZone.cs b/src/Box2D.NET/B2TracyCZone.cs index 2c4b91bd..43e5797f 100644 --- a/src/Box2D.NET/B2TracyCZone.cs +++ b/src/Box2D.NET/B2TracyCZone.cs @@ -31,7 +31,7 @@ public enum B2TracyCZone finalize_transforms, bullet_body_task, merge, - prepare_stages, + solver_setup, solve_constraints, update_transforms, joint_events, diff --git a/src/Box2D.NET/B2Types.cs b/src/Box2D.NET/B2Types.cs index ce0c5055..45440646 100644 --- a/src/Box2D.NET/B2Types.cs +++ b/src/Box2D.NET/B2Types.cs @@ -162,7 +162,7 @@ public static B2DebugDraw b2DefaultDebugDraw() draw.DrawCircleFcn = b2EmptyDrawCircle; draw.DrawSolidCircleFcn = b2EmptyDrawSolidCircle; draw.DrawSolidCapsuleFcn = b2EmptyDrawSolidCapsule; - draw.drawLineFcn = b2EmptyDrawSegment; + draw.DrawLineFcn = b2EmptyDrawSegment; draw.DrawTransformFcn = b2EmptyDrawTransform; draw.DrawPointFcn = b2EmptyDrawPoint; draw.DrawStringFcn = b2EmptyDrawString; diff --git a/src/Box2D.NET/B2WeldJoints.cs b/src/Box2D.NET/B2WeldJoints.cs index f99a95ca..60419d4a 100644 --- a/src/Box2D.NET/B2WeldJoints.cs +++ b/src/Box2D.NET/B2WeldJoints.cs @@ -157,13 +157,13 @@ internal static float b2GetWeldJointTorque(B2World world, B2JointSim @base) // J = [0 0 -1 0 0 1] // K = invI1 + invI2 - // Point-to-point constraint - // C = p2 - p1 - // Cdot = v2 - v1 - // = v2 + cross(w2, r2) - v1 - cross(w1, r1) - // J = [-E -r1_skew E r2_skew ] - // Identity used: - // w k % (rx i + ry j) = w * (-ry i + rx j) + // 3x3 Block + // K = [J1] * invM * [J1T J2T] + // [J2] + // = [J1] * [invM * J1T invM * J2T] + // [J2] + // = [J1 * invM * J1T J1 * invM * J2T] + // [J2 * invM * J1T J2 * invM * J2T] internal static void b2PrepareWeldJoint(B2JointSim @base, B2StepContext context) { B2_ASSERT(@base.type == B2JointType.b2_weldJoint); diff --git a/src/Box2D.NET/B2WheelJoints.cs b/src/Box2D.NET/B2WheelJoints.cs index 2c01f0d1..64033ada 100644 --- a/src/Box2D.NET/B2WheelJoints.cs +++ b/src/Box2D.NET/B2WheelJoints.cs @@ -182,12 +182,9 @@ internal static float b2GetWheelJointTorque(B2World world, B2JointSim @base) // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] - // Linear constraint (point-to-line) - // d = pB - pA = xB + rB - xA - rA - // C = dot(ay, d) - // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) - // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) - // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + // Motor rotational constraint + // Cdot = wB - wA + // J = [0 0 -1 0 0 1] internal static void b2PrepareWheelJoint(B2JointSim @base, B2StepContext context) { B2_ASSERT(@base.type == B2JointType.b2_wheelJoint); @@ -555,20 +552,20 @@ internal static void b2DrawWheelJoint(B2DebugDraw draw, B2JointSim @base, in B2T B2HexColor c4 = B2HexColor.b2_colorDimGray; B2HexColor c5 = B2HexColor.b2_colorBlue; - draw.drawLineFcn(frameA.p, frameB.p, c5, draw.context); + draw.DrawLineFcn(frameA.p, frameB.p, c5, draw.context); if (joint.enableLimit) { B2Vec2 lower = b2MulAdd(frameA.p, joint.lowerTranslation, axisA); B2Vec2 upper = b2MulAdd(frameA.p, joint.upperTranslation, axisA); B2Vec2 perp = b2LeftPerp(axisA); - draw.drawLineFcn(lower, upper, c1, draw.context); - draw.drawLineFcn(b2MulSub(lower, 0.1f * drawScale, perp), b2MulAdd(lower, 0.1f * drawScale, perp), c2, draw.context); - draw.drawLineFcn(b2MulSub(upper, 0.1f * drawScale, perp), b2MulAdd(upper, 0.1f * drawScale, perp), c3, draw.context); + draw.DrawLineFcn(lower, upper, c1, draw.context); + draw.DrawLineFcn(b2MulSub(lower, 0.1f * drawScale, perp), b2MulAdd(lower, 0.1f * drawScale, perp), c2, draw.context); + draw.DrawLineFcn(b2MulSub(upper, 0.1f * drawScale, perp), b2MulAdd(upper, 0.1f * drawScale, perp), c3, draw.context); } else { - draw.drawLineFcn(b2MulSub(frameA.p, 1.0f, axisA), b2MulAdd(frameA.p, 1.0f, axisA), c1, draw.context); + draw.DrawLineFcn(b2MulSub(frameA.p, 1.0f, axisA), b2MulAdd(frameA.p, 1.0f, axisA), c1, draw.context); } draw.DrawPointFcn(frameA.p, 5.0f, c1, draw.context); diff --git a/src/Box2D.NET/B2World.cs b/src/Box2D.NET/B2World.cs index a75027e7..38bbe219 100644 --- a/src/Box2D.NET/B2World.cs +++ b/src/Box2D.NET/B2World.cs @@ -11,7 +11,7 @@ namespace Box2D.NET // The world also contains efficient memory management facilities. public class B2World { - public B2ArenaAllocator arena; + public B2StackAllocator stack; public B2BroadPhase broadPhase; public B2ConstraintGraph constraintGraph; @@ -88,14 +88,7 @@ public class B2World //b2BitSet bodyWakeSet; //b2ImpulseArray deferredImpulses; - // 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; public B2BitSet debugContactSet; @@ -129,6 +122,8 @@ public class B2World public B2Profile profile; + public B2Capacity maxCapacity; + public b2PreSolveFcn preSolveFcn; public object preSolveContext; @@ -170,7 +165,7 @@ public class B2World public void Clear() { - arena = null; + stack = null; broadPhase = null; bodyIdPool = null; @@ -241,6 +236,8 @@ public void Clear() profile = new B2Profile(); + maxCapacity = new B2Capacity(); + preSolveFcn = null; preSolveContext = null; diff --git a/src/Box2D.NET/B2WorldDef.cs b/src/Box2D.NET/B2WorldDef.cs index d9264304..16eaabc6 100644 --- a/src/Box2D.NET/B2WorldDef.cs +++ b/src/Box2D.NET/B2WorldDef.cs @@ -70,6 +70,9 @@ public struct B2WorldDef /// User data public B2UserData userData; + /// Optional initial capacities + public B2Capacity capacity; + /// Used internally to detect a valid definition. DO NOT SET. public int internalValue; } diff --git a/src/Box2D.NET/B2Worlds.cs b/src/Box2D.NET/B2Worlds.cs index 3a186b74..e5b1ce59 100644 --- a/src/Box2D.NET/B2Worlds.cs +++ b/src/Box2D.NET/B2Worlds.cs @@ -52,7 +52,7 @@ private static B2World[] b2AllocWorlds(int maxWorld) return worlds; } - public static B2World b3GetUnlockedWorldFromId(B2WorldId id) + public static B2World b2GetUnlockedWorldFromId(B2WorldId id) { B2_ASSERT(1 <= id.index1 && id.index1 <= B2_MAX_WORLDS); B2World world = b2_worlds[(id.index1 - 1)]; @@ -136,6 +136,8 @@ private static void b2CreateWorkerContexts(B2World world) { world.taskContexts.data[i].sensorHits = b2Array_Create(8); world.taskContexts.data[i].contactStateBitSet = b2CreateBitSet(1024); + world.taskContexts.data[i].hitEventBitSet = b2CreateBitSet(1024); + world.taskContexts.data[i].hasHitEvents = false; world.taskContexts.data[i].jointStateBitSet = b2CreateBitSet(1024); world.taskContexts.data[i].enlargedSimBitSet = b2CreateBitSet(256); world.taskContexts.data[i].awakeIslandBitSet = b2CreateBitSet(256); @@ -151,6 +153,7 @@ private static void b2DestroyWorkerContexts(B2World world) { b2Array_Destroy(ref world.taskContexts.data[i].sensorHits); b2DestroyBitSet(ref world.taskContexts.data[i].contactStateBitSet); + b2DestroyBitSet(ref world.taskContexts.data[i].hitEventBitSet); b2DestroyBitSet(ref world.taskContexts.data[i].jointStateBitSet); b2DestroyBitSet(ref world.taskContexts.data[i].enlargedSimBitSet); b2DestroyBitSet(ref world.taskContexts.data[i].awakeIslandBitSet); @@ -227,13 +230,14 @@ public static B2WorldId b2CreateWorld(in B2WorldDef def) world.generation = generation; world.inUse = true; - world.arena = b2CreateArenaAllocator(256); - b2CreateBroadPhase(ref world.broadPhase); - b2CreateGraph(ref world.constraintGraph, 16); + world.stack = b2CreateStackAllocator(2048); + b2CreateBroadPhase(ref world.broadPhase, def.capacity); + b2CreateGraph(ref world.constraintGraph, def.capacity); // pools world.bodyIdPool = b2CreateIdPool(); - world.bodies = b2Array_Create(16); + int bodyCapacity = b2MaxInt(16, def.capacity.staticBodyCount + def.capacity.dynamicBodyCount); + world.bodies = b2Array_Create(bodyCapacity); world.solverSets = b2Array_Create(8); // add empty static, active, and disabled body sets @@ -244,6 +248,7 @@ public static B2WorldId b2CreateWorld(in B2WorldDef def) set = b2CreateSolverSet(world); set.setIndex = b2AllocId(world.solverSetIdPool); b2Array_Push(ref world.solverSets, set); + b2Array_Reserve(ref world.solverSets.data[(int)B2SolverSetType.b2_staticSet].bodySims, b2MaxInt(16, def.capacity.staticBodyCount)); B2_ASSERT(world.solverSets.data[(int)B2SolverSetType.b2_staticSet].setIndex == (int)B2SolverSetType.b2_staticSet); // disabled set @@ -256,22 +261,26 @@ public static B2WorldId b2CreateWorld(in B2WorldDef def) set = b2CreateSolverSet(world); set.setIndex = b2AllocId(world.solverSetIdPool); b2Array_Push(ref world.solverSets, set); + b2Array_Reserve(ref world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].bodySims, b2MaxInt(16, def.capacity.dynamicBodyCount)); + b2Array_Reserve(ref world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].bodyStates, b2MaxInt(16, def.capacity.dynamicBodyCount)); + b2Array_Reserve(ref world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].contactSims, b2MaxInt(16, def.capacity.contactCount)); B2_ASSERT(world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].setIndex == (int)B2SolverSetType.b2_awakeSet); world.shapeIdPool = b2CreateIdPool(); - world.shapes = b2Array_Create(16); + int shapeCapacity = b2MaxInt(16, def.capacity.staticShapeCount + def.capacity.dynamicShapeCount); + world.shapes = b2Array_Create(shapeCapacity); world.chainIdPool = b2CreateIdPool(); world.chainShapes = b2Array_Create(4); world.contactIdPool = b2CreateIdPool(); - world.contacts = b2Array_Create(16); + world.contacts = b2Array_Create(b2MaxInt(16, def.capacity.contactCount)); world.jointIdPool = b2CreateIdPool(); world.joints = b2Array_Create(16); world.islandIdPool = b2CreateIdPool(); - world.islands = b2Array_Create(8); + world.islands = b2Array_Create(b2MaxInt(16, def.capacity.dynamicBodyCount)); world.sensors = b2Array_Create(4); @@ -460,7 +469,7 @@ public static void b2DestroyWorld(B2WorldId worldId) b2DestroyIdPool(ref world.islandIdPool); b2DestroyIdPool(ref world.solverSetIdPool); - b2DestroyArenaAllocator(world.arena); + b2DestroyStackAllocator(world.stack); // Wipe world but preserve generation ushort generation = world.generation; @@ -469,14 +478,14 @@ public static void b2DestroyWorld(B2WorldId worldId) world.generation = (ushort)(generation + 1); } - internal static void b2CollideTask(int startIndex, int endIndex, int threadIndex, object context) + internal static void b2CollideTask(int startIndex, int endIndex, int workerIndex, object context) { b2TracyCZoneNC(B2TracyCZone.collide_task, "Collide", B2HexColor.b2_colorDodgerBlue, true); B2StepContext stepContext = context as B2StepContext; B2World world = stepContext.world; - B2TaskContext taskContext = world.taskContexts.data[threadIndex]; - ArraySegment contactSims = stepContext.contacts; + B2TaskContext taskContext = world.taskContexts.data[workerIndex]; + ArraySegment contactSims = stepContext.contactSims; B2Shape[] shapes = world.shapes.data; B2Body[] bodies = world.bodies.data; @@ -561,12 +570,17 @@ internal static void b2CollideTask(int startIndex, int endIndex, int threadIndex mp.persisted = true; } + taskContext.recycledContactCount += 1; + // Contact is recycled. This also skips updating other aspects of the contact // such as material parameters. continue; } } + // Caching for contact recycling. + contactSim.cachedTransformA = transformA; + contactSim.cachedTransformB = transformB; contactSim.simFlags |= (uint)B2ContactSimFlags.b2_simRelativeTransformValid; B2Vec2 centerOffsetA = b2RotateVector(transformA.q, bodySimA.localCenter); @@ -588,9 +602,6 @@ internal static void b2CollideTask(int startIndex, int endIndex, int threadIndex b2SetBit(ref taskContext.contactStateBitSet, contactId); } - // Caching for contact recycling. Requires 40 bytes. - contactSim.cachedTransformA = transformA; - contactSim.cachedTransformB = transformB; for (int i = 0; i < contactSim.manifold.pointCount; ++i) { ref B2ManifoldPoint mp = ref contactSim.manifold.points[i]; @@ -622,7 +633,7 @@ internal static void b2AddNonTouchingContact(B2World world, B2Contact contact, B contact.colorIndex = B2_NULL_INDEX; contact.localIndex = set.contactSims.count; - ref B2ContactSim newContactSim = ref b2Array_Add(ref set.contactSims); + ref B2ContactSim newContactSim = ref b2Array_Emplace(ref set.contactSims); //memcpy( newContactSim, contactSim, sizeof( b2ContactSim ) ); newContactSim.CopyFrom(contactSim); } @@ -668,7 +679,7 @@ internal static void b2Collide(B2StepContext context) return; } - ArraySegment contactSims = b2AllocateArenaItem(world.arena, contactCount, "contacts"); + ArraySegment contactSims = b2StackAlloc(world.stack, contactCount, "contacts"); int contactIndex = 0; for (int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i) @@ -694,21 +705,22 @@ internal static void b2Collide(B2StepContext context) B2_ASSERT(contactIndex == contactCount); - context.contacts = contactSims; + context.contactSims = contactSims; // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. int contactIdCapacity = b2GetIdCapacity(world.contactIdPool); for (int i = 0; i < world.workerCount; ++i) { b2SetBitCountAndClear(ref world.taskContexts.data[i].contactStateBitSet, contactIdCapacity); + world.taskContexts.data[i].recycledContactCount = 0; } // Task should take at least 40us on a 4GHz CPU (10K cycles) int minRange = 64; b2ParallelFor(world, b2CollideTask, contactCount, minRange, context); - b2FreeArenaItem(world.arena, contactSims); - context.contacts = null; + b2StackFree(world.stack, contactSims); + context.contactSims = null; contactSims = null; // Serially update contact state @@ -876,18 +888,6 @@ public static void b2World_Step(B2WorldId worldId, float timeStep, int subStepCo // world.profile = ( b2Profile ){ 0 }; world.profile = new B2Profile(); - if (timeStep == 0.0f) - { - // Swap end event array buffers - world.endEventArrayIndex = 1 - world.endEventArrayIndex; - b2Array_Clear(ref world.sensorEndEvents[world.endEventArrayIndex]); - b2Array_Clear(ref world.contactEndEvents[world.endEventArrayIndex]); - - // todo_erin would be useful to still process collision while paused - //b2TracyCFrame - return; - } - b2TracyCZoneNC(B2TracyCZone.world_step, "Step", B2HexColor.b2_colorBox2DGreen, true); world.locked = true; @@ -901,6 +901,22 @@ public static void b2World_Step(B2WorldId worldId, float timeStep, int subStepCo ulong stepTicks = b2GetTicks(); + { + ref B2Capacity c = ref world.maxCapacity; + c.staticShapeCount = b2MaxInt(c.staticShapeCount, world.broadPhase.trees[(int)B2BodyType.b2_staticBody].proxyCount); + c.dynamicShapeCount = b2MaxInt(c.dynamicShapeCount, world.broadPhase.trees[(int)B2BodyType.b2_dynamicBody].proxyCount); + + int staticBodyCount = world.solverSets.data[(int)B2SolverSetType.b2_staticSet].bodySims.count; + c.staticBodyCount = b2MaxInt(c.staticBodyCount, staticBodyCount); + + // this includes kinematic bodies + int totalBodyCount = b2GetIdCount(world.bodyIdPool); + c.dynamicBodyCount = b2MaxInt(c.dynamicBodyCount, totalBodyCount - staticBodyCount); + + int totalContactCount = b2GetIdCount(world.contactIdPool); + c.contactCount = b2MaxInt(c.contactCount, totalContactCount); + } + // Update collision pairs and create contacts { ulong pairTicks = b2GetTicks(); @@ -938,7 +954,7 @@ public static void b2World_Step(B2WorldId worldId, float timeStep, int subStepCo context.maxLinearVelocity = world.maxLinearSpeed; context.enableWarmStarting = world.enableWarmStarting; - // Update contacts + // Narrow phase : update contacts { ulong collideTicks = b2GetTicks(); b2Collide(context); @@ -953,6 +969,14 @@ public static void b2World_Step(B2WorldId worldId, float timeStep, int subStepCo world.profile.solve = b2GetMilliseconds(solveTicks); } + // Finish the tree task in case b2Solve didn't finish it + if (world.userTreeTask != null) + { + world.finishTaskFcn(world.userTreeTask, world.userTaskContext); + world.userTreeTask = null; + world.activeTaskCount -= 1; + } + // Update sensors { ulong sensorTicks = b2GetTicks(); @@ -962,10 +986,10 @@ public static void b2World_Step(B2WorldId worldId, float timeStep, int subStepCo world.profile.step = b2GetMilliseconds(stepTicks); - B2_ASSERT(b2GetArenaAllocation(world.arena) == 0); + B2_ASSERT(b2GetStackAllocation(world.stack) == 0); // Ensure stack is large enough - b2GrowArena(world.arena); + b2GrowStack(world.stack); // Make sure all tasks that were started were also finished B2_ASSERT(world.activeTaskCount == 0); @@ -1013,7 +1037,7 @@ internal static void b2DrawShape(B2DebugDraw draw, B2Shape shape, B2Transform xf ref readonly B2Segment segment = ref shape.us.segment; B2Vec2 p1 = b2TransformPoint(xf, segment.point1); B2Vec2 p2 = b2TransformPoint(xf, segment.point2); - draw.drawLineFcn(p1, p2, color, draw.context); + draw.DrawLineFcn(p1, p2, color, draw.context); } break; @@ -1022,9 +1046,9 @@ internal static void b2DrawShape(B2DebugDraw draw, B2Shape shape, B2Transform xf ref readonly B2Segment segment = ref shape.us.chainSegment.segment; B2Vec2 p1 = b2TransformPoint(xf, segment.point1); B2Vec2 p2 = b2TransformPoint(xf, segment.point2); - draw.drawLineFcn(p1, p2, color, draw.context); + draw.DrawLineFcn(p1, p2, color, draw.context); draw.DrawPointFcn(p2, 4.0f, color, draw.context); - draw.drawLineFcn(p1, b2Lerp(p1, p2, 0.1f), B2HexColor.b2_colorPaleGreen, draw.context); + draw.DrawLineFcn(p1, b2Lerp(p1, p2, 0.1f), B2HexColor.b2_colorPaleGreen, draw.context); } break; @@ -1198,7 +1222,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) B2BodySim bodySim = b2GetBodySim(world, body); B2Transform transform = new B2Transform(bodySim.center, bodySim.transform.q); - draw.drawLineFcn(bodySim.center0, bodySim.center, B2HexColor.b2_colorWhiteSmoke, draw.context); + draw.DrawLineFcn(bodySim.center0, bodySim.center, B2HexColor.b2_colorWhiteSmoke, draw.context); draw.DrawTransformFcn(transform, draw.context); B2Vec2 p = b2TransformPoint(transform, offset); @@ -1275,7 +1299,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) { // graph color float pointSize = contact.colorIndex == B2_OVERFLOW_INDEX ? 7.5f : 5.0f; - draw.DrawPointFcn(p, pointSize, b2_graphColors[contact.colorIndex], draw.context); + draw.DrawPointFcn(p, pointSize, b2GetGraphColor(contact.colorIndex), draw.context); // B2.g_draw.DrawString(point.position, "%d", point.color); } else if (mp.separation > linearSlop) @@ -1298,7 +1322,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) { B2Vec2 p1 = p; B2Vec2 p2 = b2MulAdd(p1, k_axisScale, normal); - draw.drawLineFcn(p1, p2, normalColor, draw.context); + draw.DrawLineFcn(p1, p2, normalColor, draw.context); buffer = $" {mp.separation:F2}"; draw.DrawStringFcn(p1, buffer, B2HexColor.b2_colorWhite, draw.context); @@ -1310,7 +1334,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) float force = 0.5f * mp.totalNormalImpulse * world.inv_dt; B2Vec2 p1 = p; B2Vec2 p2 = b2MulAdd(p1, draw.forceScale * force, normal); - draw.drawLineFcn(p1, p2, impulseColor, draw.context); + draw.DrawLineFcn(p1, p2, impulseColor, draw.context); buffer = $"{force:F1}"; draw.DrawStringFcn(p1, buffer, B2HexColor.b2_colorWhite, draw.context); } @@ -1327,7 +1351,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) B2Vec2 tangent = b2RightPerp(normal); B2Vec2 p1 = p; B2Vec2 p2 = b2MulAdd(p1, draw.forceScale * force, tangent); - draw.drawLineFcn(p1, p2, frictionColor, draw.context); + draw.DrawLineFcn(p1, p2, frictionColor, draw.context); buffer = $"{force:F1}"; draw.DrawStringFcn(p1, buffer, B2HexColor.b2_colorWhite, draw.context); } @@ -1829,7 +1853,7 @@ public static B2Profile b2World_GetProfile(B2WorldId worldId) /// Set the worker count. Must be between in the range [1, B2_MAX_WORKERS] public static void b2World_SetWorkerCount(B2WorldId worldId, int count) { - B2World world = b3GetUnlockedWorldFromId(worldId); + B2World world = b2GetUnlockedWorldFromId(worldId); if (world == null) { return; @@ -1848,7 +1872,7 @@ public static void b2World_SetWorkerCount(B2WorldId worldId, int count) /// Get the worker count. public static int b2World_GetWorkerCount(B2WorldId worldId) { - B2World world = b3GetUnlockedWorldFromId(worldId); + B2World world = b2GetUnlockedWorldFromId(worldId); if (world == null) { return 0; @@ -1874,17 +1898,36 @@ public static B2Counters b2World_GetCounters(B2WorldId worldId) B2DynamicTree kinematicTree = world.broadPhase.trees[(int)B2BodyType.b2_kinematicBody]; s.treeHeight = b2MaxInt(b2DynamicTree_GetHeight(dynamicTree), b2DynamicTree_GetHeight(kinematicTree)); - s.stackUsed = b2GetMaxArenaAllocation(world.arena); + s.stackUsed = b2GetMaxStackAllocation(world.stack); s.byteCount = b2GetByteCount(); s.taskCount = world.taskCount; + s.recycledContactCount = 0; + for (int i = 0; i < world.workerCount; ++i) + { + s.recycledContactCount += world.taskContexts.data[i].recycledContactCount; + } + + s.awakeContactCount = 0; for (int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i) { - s.colorCounts[i] = world.constraintGraph.colors[i].contactSims.count + world.constraintGraph.colors[i].jointSims.count; + ref B2GraphColor color = ref world.constraintGraph.colors[i]; + s.colorCounts[i] = color.contactSims.count + color.jointSims.count; + s.awakeContactCount += color.contactSims.count; } + s.awakeContactCount += world.solverSets.data[(int)B2SolverSetType.b2_awakeSet].contactSims.count; + return s; } + + /// Get max capacity. This can be used with b2WorldDef to avoid run-time allocations and copies + public static B2Capacity b2World_GetMaxCapacity(B2WorldId worldId) + { + B2World world = b2GetWorldFromId(worldId); + return world.maxCapacity; + } + /// Set the user data pointer. public static void b2World_SetUserData(B2WorldId worldId, B2UserData userData) { @@ -2024,7 +2067,7 @@ public static void b2World_DumpMemoryStats(B2WorldId worldId) writer.Write("\n"); // stack allocator - writer.Write("stack allocator: {0}\n\n", b2GetArenaCapacity(world.arena)); + writer.Write("stack allocator: {0}\n\n", b2GetStackCapacity(world.stack)); // chain shapes // todo diff --git a/test/Box2D.NET.Test/B2ArenaAllocatorTests.cs b/test/Box2D.NET.Test/B2ArenaAllocatorTests.cs index 9d6bd27b..4e774d58 100644 --- a/test/Box2D.NET.Test/B2ArenaAllocatorTests.cs +++ b/test/Box2D.NET.Test/B2ArenaAllocatorTests.cs @@ -15,21 +15,21 @@ public class B2ArenaAllocatorTests public void Test_GetOrCreateFor_CreatesAllocatorForType() { // Arrange - var arena = b2CreateArenaAllocator(10); + var arena = b2CreateStackAllocator(10); // Act var typedAllocator = arena.GetOrCreateFor(); // Assert Assert.That(typedAllocator, Is.Not.Null, "Allocator should be created for the specified type."); - Assert.That(typedAllocator, Is.AssignableFrom>(), "Returned allocator should be of the correct type."); + Assert.That(typedAllocator, Is.AssignableFrom>(), "Returned allocator should be of the correct type."); } [Test] public void Test_GetOrCreateFor_ReturnsSameAllocatorForSameType() { // Arrange - var arena = b2CreateArenaAllocator(10); + var arena = b2CreateStackAllocator(10); // Act var typedAllocator1 = arena.GetOrCreateFor(); @@ -43,7 +43,7 @@ public void Test_GetOrCreateFor_ReturnsSameAllocatorForSameType() public void Test_GetOrCreateFor_AllocatesForDifferentTypes() { // Arrange - var arena = b2CreateArenaAllocator(10); + var arena = b2CreateStackAllocator(10); // Act IB2ArenaAllocatable typedAllocator1 = arena.GetOrCreateFor(); @@ -60,7 +60,7 @@ public void Test_GetOrCreateFor_AllocatesForDifferentTypes() public void Test_AsSpan_ReturnsAllocatorsSpan() { // Arrange - var arena = b2CreateArenaAllocator(10); + var arena = b2CreateStackAllocator(10); IB2ArenaAllocatable first = arena.GetOrCreateFor(); IB2ArenaAllocatable second = arena.GetOrCreateFor(); IB2ArenaAllocatable third = arena.GetOrCreateFor(); @@ -79,7 +79,7 @@ public void Test_AsSpan_ReturnsAllocatorsSpan() public void Test_GetOrCreateFor_ShouldBeThreadSafe_WhenCalledConcurrently() { // Arrange - var arena = b2CreateArenaAllocator(10); + var arena = b2CreateStackAllocator(10); var tasks = new Task[100]; var typedAllocators = new IB2ArenaAllocatable[100]; var ce = new CountdownEvent(1); @@ -125,13 +125,13 @@ public void Test_GetOrCreateFor_ShouldBeThreadSafe_WhenCalledConcurrently() [Test] public void Test_AllocateArenaItem_WithinCapacity_UsesArenaMemory() { - var arena = b2CreateArenaAllocator(100); + var arena = b2CreateStackAllocator(100); var alloc = arena.GetOrCreateFor(); // 0, 1, 2 for (int i = 0; i < 100 / 32; ++i) { - var result = b2AllocateArenaItem(arena, 1, "test"); + var result = b2StackAlloc(arena, 1, "test"); Assert.That(result.Count, Is.EqualTo(32), $"index({i})"); Assert.That(result.Offset, Is.EqualTo(i * 32), $"index({i})"); @@ -146,14 +146,14 @@ public void Test_AllocateArenaItem_WithinCapacity_UsesArenaMemory() public void Test_AllocateArenaItem_ExceedsCapacity_UsesHeapFallback() { // Create an arena with very small capacity to force fallback - var arena = b2CreateArenaAllocator(32); + var arena = b2CreateStackAllocator(32); var alloc = arena.GetOrCreateFor(); for (int i = 0; i < 3; ++i) { // Request size large enough to exceed arena capacity every time int requestSize = 33; - var result = b2AllocateArenaItem(arena, requestSize, $"heap_test_{i}"); + var result = b2StackAlloc(arena, requestSize, $"heap_test_{i}"); int expectedSize32 = ((requestSize - 1) | 0x1F) + 1; @@ -174,26 +174,26 @@ public void Test_AllocateArenaItem_ExceedsCapacity_UsesHeapFallback() [Test] public void Test_FreeArenaItem_ArenaAndHeap() { - var arena = b2CreateArenaAllocator(64); + var arena = b2CreateStackAllocator(64); var alloc = arena.GetOrCreateFor(); // Arena allocation (fits into arena buffer) - var arenaMem = b2AllocateArenaItem(arena, 1, "arena_alloc"); // size32 = 32 + var arenaMem = b2StackAlloc(arena, 1, "arena_alloc"); // size32 = 32 // Heap fallback allocation (exceeds arena capacity) - var heapMem = b2AllocateArenaItem(arena, 65, "heap_alloc"); // size32 = 96 → forces fallback + var heapMem = b2StackAlloc(arena, 65, "heap_alloc"); // size32 = 96 → forces fallback Assert.That(alloc.entries.count, Is.EqualTo(2)); // --- Free heap allocation --- - b2FreeArenaItem(arena, heapMem); + b2StackFree(arena, heapMem); Assert.That(alloc.entries.count, Is.EqualTo(1)); Assert.That(alloc.index, Is.EqualTo(32), "Heap free should not affect arena index"); Assert.That(alloc.allocation, Is.EqualTo(32)); // --- Free arena allocation --- - b2FreeArenaItem(arena, arenaMem); + b2StackFree(arena, arenaMem); Assert.That(alloc.entries.count, Is.EqualTo(0)); Assert.That(alloc.index, Is.EqualTo(0)); @@ -203,9 +203,9 @@ public void Test_FreeArenaItem_ArenaAndHeap() [Test] public void Test_GrowArena() { - var arena = b2CreateArenaAllocator(10); - var intSegment = b2AllocateArenaItem(arena, 32, "int * 32"); - var byteSegment = b2AllocateArenaItem(arena, 32, "byte * 32"); + var arena = b2CreateStackAllocator(10); + var intSegment = b2StackAlloc(arena, 32, "int * 32"); + var byteSegment = b2StackAlloc(arena, 32, "byte * 32"); // before var allocSpan = arena.AsSpan(); @@ -217,9 +217,9 @@ public void Test_GrowArena() Assert.That(allocSpan[i].maxAllocation, Is.EqualTo(32)); } - b2FreeArenaItem(arena, intSegment); - b2FreeArenaItem(arena, byteSegment); - b2GrowArena(arena); + b2StackFree(arena, intSegment); + b2StackFree(arena, byteSegment); + b2GrowStack(arena); for (int i = 0; i < allocSpan.Length; ++i) { diff --git a/test/Box2D.NET.Test/B2ArenaAllocatorTypedTests.cs b/test/Box2D.NET.Test/B2ArenaAllocatorTypedTests.cs index de58ec51..89751ebc 100644 --- a/test/Box2D.NET.Test/B2ArenaAllocatorTypedTests.cs +++ b/test/Box2D.NET.Test/B2ArenaAllocatorTypedTests.cs @@ -12,7 +12,7 @@ public class B2ArenaAllocatorTypedTests [Test] public void Constructor_InitializesCorrectly() { - B2ArenaAllocatorTyped alloc = b2CreateArenaAllocator(10); + B2Stack alloc = b2CreateStack(10); Assert.That(alloc.capacity, Is.EqualTo(10)); Assert.That(alloc.data.Array, Is.Not.Null); Assert.That(alloc.data.Count, Is.EqualTo(10)); @@ -27,7 +27,7 @@ public void Constructor_InitializesCorrectly() [Test] public void Grow_IncreasesCapacityWhenMaxAllocationExceedsCurrent() { - B2ArenaAllocatorTyped alloc = b2CreateArenaAllocator(10); + B2Stack alloc = b2CreateStack(10); alloc.maxAllocation = 15; int oldCapacity = alloc.capacity; @@ -42,7 +42,7 @@ public void Grow_IncreasesCapacityWhenMaxAllocationExceedsCurrent() [Test] public void Grow_DoesNotIncreaseCapacityWhenMaxAllocationIsWithinCurrent() { - B2ArenaAllocatorTyped alloc = b2CreateArenaAllocator(10); + B2Stack alloc = b2CreateStack(10); alloc.maxAllocation = 5; int oldCapacity = alloc.capacity; @@ -56,7 +56,7 @@ public void Grow_DoesNotIncreaseCapacityWhenMaxAllocationIsWithinCurrent() [Test] public void Destroy_ReleasesResourcesAndResetsProperties() { - B2ArenaAllocatorTyped alloc = b2CreateArenaAllocator(10); + B2Stack alloc = b2CreateStack(10); alloc.Destroy(); diff --git a/test/Box2D.NET.Test/B2ArrayTests.cs b/test/Box2D.NET.Test/B2ArrayTests.cs index 16e9a05b..2e9ee8dc 100644 --- a/test/Box2D.NET.Test/B2ArrayTests.cs +++ b/test/Box2D.NET.Test/B2ArrayTests.cs @@ -122,7 +122,7 @@ public void TestArrayEmplace() for (int i = 0; i < 100; ++i) { - ref ulong j = ref b2Array_Add(ref a); + ref ulong j = ref b2Array_Emplace(ref a); j = (ulong)i; } @@ -398,7 +398,7 @@ public void TestArrayEmplaceStruct() for (int i = 0; i < 50; ++i) { - ref Foo f = ref b2Array_Add(ref a); + ref Foo f = ref b2Array_Emplace(ref a); f.a = i; f.b = (float)i * 2.0f; } @@ -445,7 +445,7 @@ public void TestArraySingleElement() { B2Array a = new B2Array(); - ref Foo f = ref b2Array_Add(ref a); + ref Foo f = ref b2Array_Emplace(ref a); f.a = 7; f.b = 3.14f; diff --git a/test/Box2D.NET.Test/B2DeterminismTest.cs b/test/Box2D.NET.Test/B2DeterminismTest.cs index 1098a7d3..c1a1eefa 100644 --- a/test/Box2D.NET.Test/B2DeterminismTest.cs +++ b/test/Box2D.NET.Test/B2DeterminismTest.cs @@ -14,8 +14,8 @@ namespace Box2D.NET.Test; public class B2DeterminismTest { - private const int EXPECTED_SLEEP_STEP = 293; - private const uint EXPECTED_HASH = 0x2FF98AC6; + private const int EXPECTED_SLEEP_STEP = 262; + private const uint EXPECTED_HASH = 0x3841BB81; // todo_erin move this to shared public static int SingleMultithreadingTest(int workerCount) @@ -28,7 +28,7 @@ public static int SingleMultithreadingTest(int workerCount) FallingHingeData data = CreateFallingHinges(worldId); float timeStep = 1.0f / 60.0f; - int stepLimit = 1000; + int stepLimit = 500; for ( int i = 0; i < stepLimit; ++i ) { int subStepCount = 4; diff --git a/test/Box2D.NET.Test/B2DynamicTreeTest.cs b/test/Box2D.NET.Test/B2DynamicTreeTest.cs index e34458d4..d541c642 100644 --- a/test/Box2D.NET.Test/B2DynamicTreeTest.cs +++ b/test/Box2D.NET.Test/B2DynamicTreeTest.cs @@ -22,7 +22,7 @@ public void TreeCreateDestroy() upperBound: new B2Vec2(2.0f, 2.0f) ); - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); b2DynamicTree_CreateProxy(tree, a, 1, 0); Assert.That(tree.nodeCount > 0); @@ -49,7 +49,7 @@ public void TreeRayCastTest() { // Test AABB centered at origin with bounds [-1, -1] to [1, 1] B2AABB a = new B2AABB(lowerBound: new B2Vec2(-1.0f, -1.0f), upperBound: new B2Vec2(1.0f, 1.0f)); - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); int proxyId = b2DynamicTree_CreateProxy(tree, a, 1, 0); B2RayCastInput input = new B2RayCastInput(); @@ -249,7 +249,7 @@ private static bool QueryCollectListCallback(int proxyId, ulong userData, ref in [Test] public void TreeMultipleProxiesTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); B2AABB a1 = new B2AABB(lowerBound: new B2Vec2(-5.0f, -1.0f), upperBound: new B2Vec2(-3.0f, 1.0f)); B2AABB a2 = new B2AABB(lowerBound: new B2Vec2(-1.0f, -1.0f), upperBound: new B2Vec2(1.0f, 1.0f)); @@ -275,7 +275,7 @@ public void TreeMultipleProxiesTest() [Test] public void TreeQueryTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); B2AABB a1 = new B2AABB(lowerBound: new B2Vec2(-5.0f, -1.0f), upperBound: new B2Vec2(-3.0f, 1.0f)); B2AABB a2 = new B2AABB(lowerBound: new B2Vec2(-1.0f, -1.0f), upperBound: new B2Vec2(1.0f, 1.0f)); @@ -307,7 +307,7 @@ public void TreeQueryTest() [Test] public void TreeMoveAndEnlargeTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); B2AABB a = new B2AABB(lowerBound: new B2Vec2(0.0f, 0.0f), upperBound: new B2Vec2(1.0f, 1.0f)); int id = b2DynamicTree_CreateProxy(tree, a, 0x1ul, 100); @@ -336,7 +336,7 @@ public void TreeMoveAndEnlargeTest() [Test] public void TreeRebuildAndValidateTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); // Create a number of proxies to make rebuild meaningful for (int i = 0; i < 12; ++i) @@ -358,7 +358,7 @@ public void TreeRebuildAndValidateTest() [Test] public void TreeRowHeightTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); int columnCount = 200; for (int i = 0; i < columnCount; ++i) @@ -378,7 +378,7 @@ public void TreeRowHeightTest() [Test] public void TreeGridHeightTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); int columnCount = 20; int rowCount = 20; @@ -403,7 +403,7 @@ public void TreeGridHeightTest() [Test] public void TreeGridMovementTest() { - B2DynamicTree tree = b2DynamicTree_Create(); + B2DynamicTree tree = b2DynamicTree_Create(16); int[] proxyIds = new int[GRID_COUNT * GRID_COUNT]; int index = 0; diff --git a/test/Box2D.NET.Test/B2WorldTest.cs b/test/Box2D.NET.Test/B2WorldTest.cs index fe5a4450..8ea65997 100644 --- a/test/Box2D.NET.Test/B2WorldTest.cs +++ b/test/Box2D.NET.Test/B2WorldTest.cs @@ -125,6 +125,42 @@ public void EmptyWorld() Assert.That(b2World_IsValid(worldId), Is.EqualTo(false)); } + [Test] + public void MaxCapacityTracksWorldUsage() + { + B2WorldDef worldDef = b2DefaultWorldDef(); + worldDef.capacity.staticBodyCount = 4; + worldDef.capacity.dynamicBodyCount = 8; + worldDef.capacity.staticShapeCount = 4; + worldDef.capacity.dynamicShapeCount = 8; + worldDef.capacity.contactCount = 16; + + B2WorldId worldId = b2CreateWorld(worldDef); + Assert.That(b2World_IsValid(worldId), Is.EqualTo(true)); + + B2BodyDef groundBodyDef = b2DefaultBodyDef(); + B2BodyId groundId = b2CreateBody(worldId, groundBodyDef); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(groundId, shapeDef, b2MakeBox(10.0f, 1.0f)); + + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(0.0f, 4.0f); + + B2BodyId bodyId = b2CreateBody(worldId, bodyDef); + b2CreatePolygonShape(bodyId, shapeDef, b2MakeBox(0.5f, 0.5f)); + + b2World_Step(worldId, 1.0f / 60.0f, 4); + + B2Capacity capacity = b2World_GetMaxCapacity(worldId); + Assert.That(capacity.staticBodyCount, Is.GreaterThanOrEqualTo(1)); + Assert.That(capacity.dynamicBodyCount, Is.GreaterThanOrEqualTo(1)); + Assert.That(capacity.staticShapeCount, Is.GreaterThanOrEqualTo(1)); + Assert.That(capacity.dynamicShapeCount, Is.GreaterThanOrEqualTo(1)); + + b2DestroyWorld(worldId); + } + public const int BODY_COUNT = 10; [Test]