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]