From 1767150df433d94ffccd96f550431351403c4e8e Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 17 Nov 2026 16:42:52 -0800 Subject: [PATCH] Pull model task API (#228) Port the pull model task API from Box2D. https://github.com/erincatto/box2d/pull/1133 Addresses discussion #150 --- docs/foundation.md | 51 ++- include/box3d/box3d.h | 7 + include/box3d/constants.h | 10 +- include/box3d/types.h | 64 ++- samples/sample.cpp | 2 +- shared/determinism.c | 128 ++++++ shared/determinism.h | 23 ++ src/broad_phase.c | 106 ++--- src/broad_phase.h | 6 +- src/contact.c | 2 +- src/parallel_for.c | 132 +++--- src/parallel_for.h | 43 +- src/physics_world.c | 653 ++++++++++++++++++++++++------ src/physics_world.h | 6 +- src/scheduler.c | 5 +- src/scheduler.h | 3 +- src/sensor.c | 18 +- src/sensor.h | 3 +- src/solver.c | 267 ++++++------ src/solver.h | 102 ++++- src/types.c | 33 ++ test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_step.c | 829 ++++++++++++++++++++++++++++++++++++++ 24 files changed, 2069 insertions(+), 427 deletions(-) create mode 100644 test/test_step.c diff --git a/docs/foundation.md b/docs/foundation.md index 4705c24..ab6457b 100644 --- a/docs/foundation.md +++ b/docs/foundation.md @@ -159,12 +159,12 @@ You can optionally connect your own task scheduler if you want more control. Mu ```c // Implement b3EnqueueTaskCallback void* MyEnqueueTask(b3TaskCallback* task, void* taskContext, - void* userContext, const char* taskName) + void* userContext, b3TaskType taskType) { MyTask* t = AllocTask(); t->task = task; t->taskContext = taskContext; - Scheduler* myScheduler = (Scheduler*)userContact; + Scheduler* myScheduler = (Scheduler*)userContext; SubmitToThreadPool(myScheduler, t); return t; } @@ -184,6 +184,53 @@ worldDef.userTaskContext = myScheduler; worldDef.workerCount = GetMyWorkerCount(); ``` +The finish callback is optional. Without it `b3World_Step()` waits on an internal semaphore that +the last task of each phase signals. This suits task systems that can launch work but cannot wait +on a single task. + +### Stepping without blocking + +`b3World_Step()` holds the calling thread until the step is done. Some task systems cannot allow +this, for example a phased or wavefront scheduler where a thread that joins a parallel job cannot +leave it. For these you can drive the step yourself with `b3World_BeginStep()` and +`b3World_ContinueStep()`. Each call runs the serial work between two phases and returns the next +batch of tasks. An empty batch means the step is complete. + +```c +b3TaskBatch batch = b3World_BeginStep( worldId, timeStep, subStepCount ); +while ( batch.count > 0 ) +{ + // Run every task in the batch on any threads, in any order, then wait for them. + // MyRunTasks is a function you implement. + MyRunTasks( batch.tasks, batch.count ); + + batch = b3World_ContinueStep( worldId ); +} +``` + +No callbacks are needed and the built-in scheduler is never created. The worker count still sets +the most tasks in a batch, `workerCount + 2`, and the size of the per-worker data. Each parallel +task claims blocks of work from a shared counter until none are left, so the tasks that run first +do most of the work and a task that starts late returns right away. Fewer threads than tasks costs +only the extra task launches, and you can adjust the worker count with `b3World_SetWorkerCount()` at +runtime. + +The rules: + +- Call each task function exactly once. Tasks in a batch may run concurrently on any threads. +- Begin and continue may be called from any thread, but never while a task of the current batch is + running. +- The world is locked until the step completes. Do not read or write it in between. +- The tasks of the constraint solver wait on each other. They work with any number of threads, + even when run one after another on a single thread, but a solver task that has started must not + be suspended indefinitely. + +One task may be marked `b3Task::deferred`. This is the rebuild of the dynamic tree, which overlaps +the narrow phase and the solver. You may leave it running across continue calls. A later batch +lists it in `b3TaskBatch::joinTasks` and it must be complete before the continue that follows that +batch. If you ignore the flag and run it with the rest of its batch, the step is still correct, it +just loses the overlap. + ### Threading model The multithreading design for Box3D is focused on [data parallelism](https://en.wikipedia.org/wiki/Data_parallelism). diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index ecc1bfc..ae107fa 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -56,6 +56,13 @@ B3_API bool b3World_IsValid( b3WorldId id ); /// accuracy. Usually 4. B3_API void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ); +/// The pull model for tasking. Instead of getting task callbacks from b3World_Step you can call +/// this to kick off a step and get an array of tasks to execute. +B3_API b3TaskBatch b3World_BeginStep( b3WorldId worldId, float timeStep, int subStepCount ); + +/// Call this after executing each task batch. The step is done when the batch is empty. +B3_API b3TaskBatch b3World_ContinueStep( b3WorldId worldId ); + /// Call this to draw shapes and other debug draw data B3_API void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ); diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 8906a12..f702d2d 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -37,11 +37,6 @@ B3_API float b3GetLengthUnitsPerMeter( void ); /// The compound barrel benchmark has minor overflow with 24 colors #define B3_GRAPH_COLOR_COUNT 24 -/// The maximum number of contact manifolds between shape pairs. Excess manifolds are dropped using heuristics until -/// this number is reached. Culling can lead to contact dither across time steps. Contact recycling mitigates the -/// dithering. -#define B3_MAX_MANIFOLDS 8 - /// A small length used as a collision and constraint tolerance. Usually it is /// chosen to be numerically significant, but visually insignificant. In meters. /// @warning modifying this can have a significant impact on stability @@ -103,6 +98,11 @@ B3_API float b3GetLengthUnitsPerMeter( void ); /// The maximum number of restitution iterations. Needed to avoid a solver overflow. #define B3_MAX_RESTITUTION_ITERATIONS 63 +/// The maximum number of contact manifolds between shape pairs. Excess manifolds are dropped using heuristics until +/// this number is reached. Culling can lead to contact dither across time steps. Contact recycling mitigates the +/// dithering. +#define B3_MAX_MANIFOLDS 8 + /// The maximum number of contact points between two touching shapes. /// The default and minimum is 4 and this case uses a fast approximate hull to reduce the /// point count. A larger value builds a 2D convex hull of the candidate points and then diff --git a/include/box3d/types.h b/include/box3d/types.h index df2f1ad..6a6a1a2 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -14,6 +14,24 @@ #define B3_DEFAULT_CATEGORY_BITS UINT64_MAX #define B3_DEFAULT_MASK_BITS UINT64_MAX +/// The task type for the pull model. +/// @ingroup world +typedef enum b3TaskType +{ + b3_taskPairs, ///< Finding pairs of potentially colliding shapes. The broad-phase. + b3_taskCollide, ///< Computing contact manifolds. The narrow-phase. + b3_taskTreeRebuild, ///< Rebuilding the bounding volume hierarchy. A long running side task. + b3_taskIslandSplit, ///< Splitting an island in order to let an island sleep. + b3_taskSolver, ///< Solving constraints. + b3_taskFinalize, ///< Updating body transforms and shape bounding boxes. + b3_taskContinuous, ///< Continuous collision detection. + b3_taskSensors, ///< Sensor overlaps and events. + b3_taskTypeCount ///< The type count. +} b3TaskType; + +/// Helper to get a string for a task type. +B3_API const char* b3GetTaskTypeName( b3TaskType type ); + /// Task interface /// This is the prototype for a Box3D task. Your task system is expected to run this callback on a /// worker thread, exactly once per enqueue, passing back the same taskContext pointer supplied to @@ -29,9 +47,9 @@ typedef void b3TaskCallback( void* taskContext ); /// @param task the Box3D task to be called by the scheduler /// @param taskContext the Box3D context object that the scheduler must pass to the task /// @param userContext the scheduler context object that is opaque to Box3D -/// @param taskName the Box3D task name that the scheduler can use for diagnostics +/// @param taskType the task type that the scheduler can use for diagnostics /// @ingroup world -typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, const char* taskName ); +typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, b3TaskType taskType ); /// Finishes a user task object that wraps a Box3D task. This must block until the task has /// completed. The step blocks here on the tasks it spawned, so b3World_Step holds its stack across @@ -40,9 +58,48 @@ typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, vo /// do not call b3World_Step from inside a job: a job that blocks on its own sub-jobs without /// yielding its thread can deadlock. The in-tree scheduler instead runs other pending tasks on the /// waiting thread. +/// This callback is optional. Without it b3World_Step waits on an internal semaphore that the last +/// task of each phase signals, which suits task systems that can launch work but cannot wait on a +/// single task. /// @ingroup world typedef void b3FinishTaskCallback( void* userTask, void* userContext ); +/// A task in the pull model. +/// @ingroup world +typedef struct b3Task +{ + /// The task function. + b3TaskCallback* fcn; + + /// The context that must be passed to the task function. + void* context; + + /// The task type. Informational. + b3TaskType type; + + /// Is this task deferred? This indicates that this task must be joined + /// at a later stage instead of immediately before the next stage. + /// @see b3TaskBatch::joinTasks + bool deferred; +} b3Task; + +/// A batch of tasks for the pull model. +/// @ingroup world +typedef struct b3TaskBatch +{ + /// The regular tasks that must be executed in this stage. + const b3Task* tasks; + + /// The task count. + int count; + + /// Earlier tasks that must now be joined before the next stage. + const b3Task* joinTasks; + + /// The join task count. + int joinCount; +} b3TaskBatch; + typedef struct b3DebugShape b3DebugShape; /// The user needs to be able to create debug draw shapes for multi-pass rendering to work @@ -209,7 +266,7 @@ typedef struct b3WorldDef /// function to spawn task b3EnqueueTaskCallback* enqueueTask; - /// function to finish a task + /// function to finish a task. Blocks and helps make progress. b3FinishTaskCallback* finishTask; /// User context that is provided to enqueueTask and finishTask @@ -615,6 +672,7 @@ typedef struct b3Counters int satCallCount; ///< int satCacheHitCount; ///< int taskCount; ///< + int enqueueCount; ///< int colorCounts[24]; ///< int manifoldCounts[B3_MAX_MANIFOLDS]; ///< diff --git a/samples/sample.cpp b/samples/sample.cpp index 0d16f9a..8d26b97 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -1071,7 +1071,7 @@ void Sample::DrawMetrics() ImGui::SetItemTooltip( "current / max" ); ImGui::Text( "joints %d", s.jointCount ); ImGui::Text( "islands %d", s.islandCount ); - ImGui::Text( "tasks %d", s.taskCount ); + ImGui::Text( "tasks %d / %d", s.enqueueCount, s.taskCount ); ImGui::TableNextColumn(); ImGui::Text( "static tree %d", s.staticTreeHeight ); diff --git a/shared/determinism.c b/shared/determinism.c index e3121bb..d3e37eb 100644 --- a/shared/determinism.c +++ b/shared/determinism.c @@ -396,3 +396,131 @@ void DestroyQuerySpawn( QuerySpawnData* data ) { (void)data; } + +FallingHingeData CreateFallingHinges( b3WorldId worldId ) +{ + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + int columnCount = 4; + int rowCount = 20; + int bodyCount = rowCount * columnCount; + + b3BodyId* bodyIds = calloc( bodyCount, sizeof( b3BodyId ) ); + + float h = 0.25f; + b3BoxHull box = b3MakeBoxHull( h, h, h ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.1f * B3_PI; + jointDef.upperAngle = 0.2f * B3_PI; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 1.0f; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 0.25f; + jointDef.base.localFrameA.p = (b3Vec3){ -h, h, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ -h, -h, 0.0f }; + jointDef.base.constraintHertz = 60.0f; + jointDef.base.constraintDampingRatio = 0.0f; + jointDef.base.drawScale = 0.5f; + + int bodyIndex = 0; + float offset = 0.4f * h; + float dx = 10.0f * h; + float xBase = -0.5f * dx * ( columnCount - 1.0f ); + + for ( int j = 0; j < columnCount; ++j ) + { + float x = xBase + j * dx; + + b3BodyId prevBodyId = b3_nullBodyId; + + for ( int i = 0; i < rowCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + bodyDef.position.x = x + offset * i; + bodyDef.position.y = h + 2.0f * h * i; + + // this tests the deterministic cosine and sine functions + float angle = ( i & 1 ) == 0 ? -0.1f : 0.1f; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, angle ); + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + if ( ( i & 1 ) == 0 ) + { + prevBodyId = bodyId; + } + else + { + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = bodyId; + b3CreateRevoluteJoint( worldId, &jointDef ); + prevBodyId = b3_nullBodyId; + } + + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + assert( bodyIndex < bodyCount ); + bodyIds[bodyIndex] = bodyId; + + bodyIndex += 1; + } + } + + assert( bodyIndex == bodyCount ); + + FallingHingeData data = { + .bodyIds = bodyIds, + .bodyCount = bodyCount, + .stepCount = 0, + .sleepStep = -1, + .hash = 0, + }; + return data; +} + +bool UpdateFallingHinges( b3WorldId worldId, FallingHingeData* data ) +{ + if ( data->hash == 0 ) + { + b3BodyEvents bodyEvents = b3World_GetBodyEvents( worldId ); + + if ( bodyEvents.moveCount == 0 ) + { + assert( b3World_GetAwakeBodyCount( worldId ) == 0 ); + + data->hash = B3_HASH_INIT; + for ( int i = 0; i < data->bodyCount; ++i ) + { + b3WorldTransform transform = b3Body_GetTransform( data->bodyIds[i] ); + data->hash = b3Hash( data->hash, (uint8_t*)( &transform ), sizeof( b3WorldTransform ) ); + } + + data->sleepStep = data->stepCount; + } + } + + data->stepCount += 1; + + return data->hash != 0; +} + +void DestroyFallingHinges( FallingHingeData* data ) +{ + free( data->bodyIds ); + data->bodyIds = NULL; +} diff --git a/shared/determinism.h b/shared/determinism.h index 78925a1..cb3f5a3 100644 --- a/shared/determinism.h +++ b/shared/determinism.h @@ -85,6 +85,29 @@ QuerySpawnData CreateQuerySpawn( b3WorldId worldId ); bool UpdateQuerySpawn( b3WorldId worldId, QuerySpawnData* data ); void DestroyQuerySpawn( QuerySpawnData* data ); +// A grid of short hinged columns that fall and settle. Light enough to step in a unit test +// and it exercises joints, sleeping islands, and island splitting. +#if defined( BOX3D_DOUBLE_PRECISION ) +#define FALLING_HINGE_SLEEP_STEP 277 +#define FALLING_HINGE_HASH 0x97A63BFF +#else +#define FALLING_HINGE_SLEEP_STEP 248 +#define FALLING_HINGE_HASH 0xF4CAB37C +#endif + +typedef struct FallingHingeData +{ + b3BodyId* bodyIds; + int bodyCount; + int stepCount; + int sleepStep; + uint32_t hash; +} FallingHingeData; + +FallingHingeData CreateFallingHinges( b3WorldId worldId ); +bool UpdateFallingHinges( b3WorldId worldId, FallingHingeData* data ); +void DestroyFallingHinges( FallingHingeData* data ); + #ifdef __cplusplus } #endif diff --git a/src/broad_phase.c b/src/broad_phase.c index 003c16a..1f5ce16 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -36,6 +36,7 @@ void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) bp->trees[b3_dynamicBody] = b3DynamicTree_Create( dynamicCapacity ); bp->movedSiblings = NULL; + bp->crossContext = NULL; } void b3DestroyBroadPhase( b3BroadPhase* bp ) @@ -50,8 +51,8 @@ void b3DestroyBroadPhase( b3BroadPhase* bp ) memset( bp, 0, sizeof( b3BroadPhase ) ); } -int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, - bool forcePairCreation ) +int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, + int shapeIndex, bool forcePairCreation ) { B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount ); @@ -232,7 +233,8 @@ static void b3EmitCompoundPairs( b3PairContext* context, int compoundShapeId, in b3DynamicTree_GetAABB( world->broadPhase.trees + B3_PROXY_TYPE( otherProxyKey ), B3_PROXY_ID( otherProxyKey ) ); // Query bounds are float world space, so the demoted transform is the matching float frame - b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, compoundShape->bodyId ), b3Pos_zero ); + b3Transform compoundTransform = + b3ToRelativeTransform( b3GetBodyTransform( world, compoundShape->bodyId ), b3Pos_zero ); b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), otherAABB ); b3CompoundPairContext compoundContext = { @@ -364,7 +366,8 @@ B3_FORCE_INLINE bool b3TestPair( const b3TreeNode* a, const b3TreeNode* b ) return b3OverlapV( &a->aabb, &b->aabb ); } -static void b3CollideProxyAndSubtree( const b3TreeNode* proxy, const b3TreeNode* nodes, int pair, b3PairContext* context ) +static void b3CollideProxyAndSubtree( const b3TreeNode* proxy, const b3TreeNode* nodes, int pair, + b3PairContext* context ) { uint32_t proxyMark = proxy->flagIndex & B3_MOVED_NODE; b3AABBV boxv = b3LoadAABBV( &proxy->aabb ); @@ -436,7 +439,8 @@ B3_FORCE_INLINE void b3VisitPair( const b3TreeNode* arrayA, const b3TreeNode* ar { if ( *stackCount < B3_TREE_STACK_SIZE ) { - stack[*stackCount] = B3_LITERAL( b3IndexPair ){ .a = b3GetLeftChild( nodeA ), .b = b3GetLeftChild( nodeB ) }; + stack[*stackCount] = + B3_LITERAL( b3IndexPair ){ .a = b3GetLeftChild( nodeA ), .b = b3GetLeftChild( nodeB ) }; *stackCount += 1; } else @@ -574,8 +578,10 @@ static int b3GatherCrossSeeds( const b3DynamicTree* treeA, const b3DynamicTree* typedef struct b3CrossContext { b3World* world; - const b3NodePair* seeds; + b3NodePair seeds[2 * B3_CROSS_SEED_COUNT]; int staticSeedCount; + int crossMoveCount; + int dynamicMoveCount; bool checkCompounds; } b3CrossContext; @@ -629,24 +635,7 @@ static void b3UpdateTreesTask( void* context ) b3TracyCZoneEnd( tree_task ); } -// Task that can be done in parallel with the narrow-phase -// - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good -static void b3EnqueueTreeUpdate( b3World* world ) -{ - if ( world->taskCount < B3_MAX_TASKS ) - { - world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" ); - world->taskCount += 1; - world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; - } - else - { - world->userTreeTask = NULL; - b3UpdateTreesTask( world ); - } -} - -void b3UpdateBroadPhasePairs( b3World* world ) +bool b3BeginPairs( b3World* world ) { b3BroadPhase* bp = &world->broadPhase; @@ -656,10 +645,10 @@ void b3UpdateBroadPhasePairs( b3World* world ) if ( needUpdate == false ) { - return; + return false; } - b3TracyCZoneNC( update_pairs, "Find Pairs", b3_colorMediumSlateBlue, true ); + b3TracyCZoneNC( update_pairs, "Find Pairs Setup", b3_colorMediumSlateBlue, true ); b3Stack* alloc = &world->stack; @@ -677,35 +666,52 @@ void b3UpdateBroadPhasePairs( b3World* world ) bp->movedSiblings = b3StackAlloc( alloc, pairCapacity * sizeof( int ), "moved pairs" ); int dynamicMoveCount = b3GatherMovedSiblings( dynamicTree, bp->movedSiblings ); + b3CrossContext* crossContext = b3StackAlloc( alloc, sizeof( b3CrossContext ), "cross context" ); + bp->crossContext = crossContext; + // Get seeds for colliding against the static and kinematic trees. - b3NodePair crossSeeds[2 * B3_CROSS_SEED_COUNT]; - int staticSeedCount = b3GatherCrossSeeds( dynamicTree, bp->trees + b3_staticBody, crossSeeds ); + int staticSeedCount = b3GatherCrossSeeds( dynamicTree, bp->trees + b3_staticBody, crossContext->seeds ); B3_ASSERT( staticSeedCount <= B3_CROSS_SEED_COUNT ); - int kinematicSeedCount = b3GatherCrossSeeds( dynamicTree, bp->trees + b3_kinematicBody, crossSeeds + staticSeedCount ); + int kinematicSeedCount = + b3GatherCrossSeeds( dynamicTree, bp->trees + b3_kinematicBody, crossContext->seeds + staticSeedCount ); B3_ASSERT( kinematicSeedCount <= B3_CROSS_SEED_COUNT ); - int crossMoveCount = staticSeedCount + kinematicSeedCount; + + crossContext->world = world; + crossContext->staticSeedCount = staticSeedCount; + crossContext->crossMoveCount = staticSeedCount + kinematicSeedCount; + crossContext->dynamicMoveCount = dynamicMoveCount; + crossContext->checkCompounds = world->compoundShapeCount > 0; // Collide the dynamic body tree against the static and kinematic trees. - b3CrossContext crossContext = { - .world = world, - .seeds = crossSeeds, - .staticSeedCount = staticSeedCount, - .checkCompounds = world->compoundShapeCount > 0, - }; - b3ParallelFor( world, &b3CrossPairsTask, crossMoveCount, 1, &crossContext, "cross pairs" ); - - // Collide the dynamic body tree against itself. - b3ParallelFor( world, &b3SelfPairsTask, dynamicMoveCount, 64, world, "self pairs" ); + b3StageParallelFor( world, b3_taskPairs, &b3CrossPairsTask, crossContext->crossMoveCount, 1, crossContext ); } - b3DynamicTree_ClearMoved( bp->trees + b3_staticBody ); - b3TracyCZoneEnd( update_pairs ); + return true; +} + +void b3StageSelfPairs( b3World* world ) +{ + // Collide the dynamic body tree against itself. + b3StageParallelFor( world, b3_taskPairs, &b3SelfPairsTask, world->broadPhase.crossContext->dynamicMoveCount, 64, + world ); +} + +void b3EndPairs( b3World* world ) +{ + b3BroadPhase* bp = &world->broadPhase; + b3Stack* alloc = &world->stack; + + b3StackFree( alloc, bp->crossContext ); + bp->crossContext = NULL; + + b3DynamicTree_ClearMoved( bp->trees + b3_staticBody ); + b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true ); // Update stale trees. - b3EnqueueTreeUpdate( world ); + b3StageDeferredTask( world->stepContext, b3_taskTreeRebuild, &b3UpdateTreesTask, world ); // Pairs arrive in deterministic order but scrambled relative to body and shape order. // Sorting them here improves solver performance and makes contact order independent of @@ -732,13 +738,13 @@ void b3UpdateBroadPhasePairs( b3World* world ) { #define LESS( i, j ) ( pairKeys[(int)( i )] < pairKeys[(int)( j )] ) -#define SWAP( i, j ) \ - do \ - { \ - uint64_t tmp_ = pairKeys[(int)( i )]; \ - pairKeys[(int)( i )] = pairKeys[(int)( j )]; \ - pairKeys[(int)( j )] = tmp_; \ - } \ +#define SWAP( i, j ) \ + do \ + { \ + uint64_t tmp_ = pairKeys[(int)( i )]; \ + pairKeys[(int)( i )] = pairKeys[(int)( j )]; \ + pairKeys[(int)( j )] = tmp_; \ + } \ while ( 0 ) QSORT( pairCount, LESS, SWAP ); diff --git a/src/broad_phase.h b/src/broad_phase.h index 4564f9f..ab981b9 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -29,6 +29,8 @@ typedef struct b3BroadPhase // The moved siblings gathered from the dynamic body tree. int* movedSiblings; + struct b3CrossContext* crossContext; + // Tracks shape pairs that have a b3Contact // todo pairSet can grow quite large on the first time step and remain large b3HashSet pairSet; @@ -45,7 +47,9 @@ void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ); -void b3UpdateBroadPhasePairs( b3World* world ); +bool b3BeginPairs( b3World* world ); +void b3StageSelfPairs( b3World* world ); +void b3EndPairs( b3World* world ); bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ); void b3ValidateBroadPhase( const b3BroadPhase* bp ); diff --git a/src/contact.c b/src/contact.c index 134e7b2..e1d94ff 100644 --- a/src/contact.c +++ b/src/contact.c @@ -19,7 +19,7 @@ // The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. // // Creation: -// - Contacts are created using results from b3UpdateBroadPhasePairs +// - Contacts are created using results from the broad-phase pair update // - These results are ordered according to the order of the broad-phase move array // - The move array is ordered according to the shape creation order using a bitset. // - The island/shape/body order is determined by creation order diff --git a/src/parallel_for.c b/src/parallel_for.c index 5a497a4..d444fa6 100644 --- a/src/parallel_for.c +++ b/src/parallel_for.c @@ -6,29 +6,85 @@ #include "core.h" #include "physics_world.h" #include "platform.h" +#include "solver.h" #include "box3d/base.h" #include "box3d/constants.h" #include -// Shared state for one b3ParallelFor invocation. Workers race on nextBlock to -// claim work, so a slow chunk can't strand the other threads. -typedef struct b3ParallelForShared +static void b3RunStepTask( void* taskContext ) { - b3AtomicInt nextBlock; - int blockCount; - int blockSize; - int itemCount; - b3ParallelForCallback* callback; - void* context; -} b3ParallelForShared; + b3StepTask* task = taskContext; + b3StepContext* stepContext = task->stepContext; -typedef struct b3ParallelForTask + task->fcn( task->context ); + + int index = (int)( task - stepContext->stepTasks ); + if ( stepContext->tasks[index].deferred ) + { + if ( b3AtomicFetchAddInt( &stepContext->treePending, -1 ) == 1 && stepContext->signalSemaphores ) + { + b3SignalSemaphore( stepContext->treeSemaphore ); + } + } + else + { + if ( b3AtomicFetchAddInt( &stepContext->pendingCount, -1 ) == 1 && stepContext->signalSemaphores ) + { + b3SignalSemaphore( stepContext->batchSemaphore ); + } + } +} + +void b3ResetTaskBatch( b3StepContext* stepContext ) { - b3ParallelForShared* shared; - int workerIndex; -} b3ParallelForTask; + stepContext->batchTaskBase = 1; + stepContext->batchTaskCount = 0; + stepContext->joinTree = false; +} + +void b3AddTask( b3StepContext* stepContext, b3TaskType type, b3TaskCallback* fcn, void* context ) +{ + int index = stepContext->batchTaskBase + stepContext->batchTaskCount; + B3_ASSERT( 1 <= index && index < B3_STEP_TASK_CAPACITY ); + + b3StepTask* stepTask = stepContext->stepTasks + index; + stepTask->fcn = fcn; + stepTask->context = context; + stepTask->stepContext = stepContext; + + stepContext->tasks[index] = (b3Task){ + .fcn = b3RunStepTask, + .context = stepTask, + .type = type, + .deferred = false, + }; + + stepContext->batchTaskCount += 1; + stepContext->taskCount += 1; +} + +void b3StageDeferredTask( b3StepContext* stepContext, b3TaskType type, b3TaskCallback* fcn, void* context ) +{ + B3_ASSERT( stepContext->treeStaged == false ); + + b3StepTask* stepTask = stepContext->stepTasks + 0; + stepTask->fcn = fcn; + stepTask->context = context; + stepTask->stepContext = stepContext; + + stepContext->tasks[0] = (b3Task){ + .fcn = b3RunStepTask, + .context = stepTask, + .type = type, + .deferred = true, + }; + + stepContext->treeStaged = true; + stepContext->taskCount += 1; + b3AtomicStoreInt( &stepContext->treePending, 1 ); +} static void b3ParallelForTrampoline( void* taskContext ) { @@ -61,8 +117,8 @@ static void b3ParallelForTrampoline( void* taskContext ) } } -void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, - const char* name ) +void b3StageParallelFor( b3World* world, b3TaskType type, b3ParallelForCallback* callback, int itemCount, int minRange, + void* context ) { if ( itemCount <= 0 ) { @@ -78,8 +134,6 @@ void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCou // block size grows once items exceed maxBlockCount * minRange // so the block count stays bounded and per-block sync overhead stays low. // Benchmarking shows 32 is optimal for the convex pile benchmark and others. - // This is a lot different than Box2D because the 3D narrow phase cost - // varies a lot. int blocksPerWorker = 32; int maxBlockCount = blocksPerWorker * workerCount; @@ -101,38 +155,20 @@ void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCou // No point enqueueing more tasks than blocks. int taskCount = workerCount < blockCount ? workerCount : blockCount; - b3ParallelForShared shared; - shared.blockCount = blockCount; - shared.blockSize = blockSize; - shared.itemCount = itemCount; - shared.callback = callback; - shared.context = context; - b3AtomicStoreInt( &shared.nextBlock, 0 ); - - b3ParallelForTask tasks[B3_MAX_WORKERS]; - void* handles[B3_MAX_WORKERS]; - for ( int i = 0; i < taskCount; ++i ) - { - tasks[i].shared = &shared; - tasks[i].workerIndex = i; - - if (world->taskCount < B3_MAX_TASKS) - { - handles[i] = world->enqueueTaskFcn( &b3ParallelForTrampoline, tasks + i, world->userTaskContext, name ); - world->taskCount += 1; - } - else - { - handles[i] = NULL; - b3ParallelForTrampoline( tasks + i ); - } - } + b3StepContext* stepContext = world->stepContext; + b3ParallelForShared* shared = &stepContext->parallelFor; + shared->blockCount = blockCount; + shared->blockSize = blockSize; + shared->itemCount = itemCount; + shared->callback = callback; + shared->context = context; + b3AtomicStoreInt( &shared->nextBlock, 0 ); for ( int i = 0; i < taskCount; ++i ) { - if ( handles[i] != NULL ) - { - world->finishTaskFcn( handles[i], world->userTaskContext ); - } + b3ParallelForTask* task = stepContext->parallelForTasks + i; + task->shared = shared; + task->workerIndex = i; + b3AddTask( stepContext, type, b3ParallelForTrampoline, task ); } } diff --git a/src/parallel_for.h b/src/parallel_for.h index 4a6d285..59f8218 100644 --- a/src/parallel_for.h +++ b/src/parallel_for.h @@ -3,9 +3,16 @@ #pragma once +#include "core.h" + +#include "box3d/types.h" + +// todo rename files to task.c and task.h + +typedef struct b3StepContext b3StepContext; typedef struct b3World b3World; -// Callback invoked by b3ParallelFor to process a range of items. May be called +// Callback invoked by a staged parallel-for to process a range of items. May be called // multiple times per worker: work is divided into blocks that workers claim // atomically, so a worker that finishes early picks up the next unclaimed // block instead of sitting idle. workerIndex is the worker identity and is @@ -13,11 +20,29 @@ typedef struct b3World b3World; // an index into per-worker state (e.g. world->taskContexts.data + workerIndex). typedef void b3ParallelForCallback( int startIndex, int endIndex, int workerIndex, void* context ); -// Divide [0, itemCount) into blocks and process them with cooperative claiming: -// up to world->workerCount tasks are enqueued, and each task loops, atomically -// claiming the next unclaimed block until the range is drained. Blocks the -// caller until all work is complete. minRange is the minimum block size; block -// size grows once itemCount exceeds 4 * workerCount * minRange so block count -// stays bounded. -void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, - const char* name ); +typedef struct b3ParallelForShared +{ + b3AtomicInt nextBlock; + int blockCount; + int blockSize; + int itemCount; + b3ParallelForCallback* callback; + void* context; +} b3ParallelForShared; + +typedef struct b3ParallelForTask +{ + b3ParallelForShared* shared; + int workerIndex; +} b3ParallelForTask; + +void b3ResetTaskBatch( b3StepContext* stepContext ); +void b3AddTask( b3StepContext* stepContext, b3TaskType type, b3TaskCallback* fcn, void* context ); +void b3StageDeferredTask( b3StepContext* stepContext, b3TaskType type, b3TaskCallback* fcn, void* context ); + +// Divide [0, itemCount) into blocks and add up to world->workerCount tasks to the current +// batch. Each task loops, atomically claiming the next unclaimed block until the range is +// drained. minRange is the minimum block size. Block size grows once itemCount exceeds +// 32 * workerCount * minRange so block count stays bounded. +void b3StageParallelFor( b3World* world, b3TaskType type, b3ParallelForCallback* callback, int itemCount, int minRange, + void* context ); diff --git a/src/physics_world.c b/src/physics_world.c index e24ae9d..2cea1d0 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -139,9 +139,9 @@ b3World* b3GetUnlockedWorld( int index ) return world; } -static void* b3DefaultAddTaskFcn( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +static void* b3DefaultAddTaskFcn( b3TaskCallback* task, void* taskContext, void* userContext, b3TaskType taskType ) { - B3_UNUSED( userContext, name ); + B3_UNUSED( userContext, taskType ); task( taskContext ); return NULL; } @@ -324,8 +324,6 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) world->stepIndex = 0; world->splitIslandId = B3_NULL_INDEX; - world->activeTaskCount = 0; - world->taskCount = 0; world->gravity = def->gravity; world->hitEventThreshold = def->hitEventThreshold; world->restitutionThreshold = def->restitutionThreshold; @@ -361,10 +359,10 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) world->enableWarmStarting = true; world->enableContinuous = def->enableContinuous; world->enableSpeculative = true; - world->userTreeTask = NULL; + world->useScheduler = false; world->userData = def->userData; - if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + if ( def->workerCount > 0 && def->enqueueTask != NULL ) { // External task system world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); @@ -375,12 +373,13 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) } else if ( def->workerCount > 1 ) { - // Built-in scheduler + // Built-in scheduler, created on the first b3World_Step world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); - world->scheduler = b3CreateScheduler( world->workerCount ); + world->useScheduler = true; + world->scheduler = NULL; world->enqueueTaskFcn = b3SchedulerEnqueueTask; world->finishTaskFcn = b3SchedulerFinishTask; - world->userTaskContext = world->scheduler; + world->userTaskContext = NULL; } else { @@ -394,6 +393,14 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) b3CreateWorkerContexts( world ); + world->stepContext = b3AllocZero( sizeof( b3StepContext ) ); + + if ( world->finishTaskFcn == NULL ) + { + world->stepContext->batchSemaphore = b3CreateSemaphore( 0 ); + world->stepContext->treeSemaphore = b3CreateSemaphore( 0 ); + } + world->debugBodySet = b3CreateBitSet( 256 ); world->debugJointSet = b3CreateBitSet( 256 ); world->debugContactSet = b3CreateBitSet( 256 ); @@ -427,6 +434,15 @@ void b3DestroyWorld( b3WorldId worldId ) world->scheduler = NULL; } + if ( world->stepContext->batchSemaphore != NULL ) + { + b3DestroySemaphore( world->stepContext->batchSemaphore ); + b3DestroySemaphore( world->stepContext->treeSemaphore ); + } + + b3Free( world->stepContext, sizeof( b3StepContext ) ); + world->stepContext = NULL; + b3DestroyBitSet( &world->debugBodySet ); b3DestroyBitSet( &world->debugJointSet ); b3DestroyBitSet( &world->debugContactSet ); @@ -821,14 +837,12 @@ static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localI } // Narrow-phase collision -static void b3Collide( b3StepContext* context ) +static void b3BeginCollide( b3StepContext* context ) { b3World* world = context->world; B3_ASSERT( world->workerCount > 0 ); - b3TracyCZoneNC( collide, "Collide", b3_colorDarkOrchid, true ); - // Gather contacts from all the graph colors into a single array for easier parallel-for int touchingCount = 0; @@ -842,13 +856,15 @@ static void b3Collide( b3StepContext* context ) int nonTouchingCount = awakeSet->contactIndices.count; int contactCount = touchingCount + nonTouchingCount; + context->collideContactCount = contactCount; if ( contactCount == 0 ) { - b3TracyCZoneEnd( collide ); return; } + b3TracyCZoneNC( collide, "Narrow Phase Setup", b3_colorDarkOrchid, true ); + int* contactIndices = (int*)b3StackAlloc( &world->stack, contactCount * sizeof( int ), "contact indices" ); int contactIndex = 0; @@ -894,11 +910,22 @@ static void b3Collide( b3StepContext* context ) // Task should take at least 40us on a 4GHz CPU (10K cycles) int minRange = 20; - b3ParallelFor( world, b3CollideTask, contactCount, minRange, context, "collide" ); + b3StageParallelFor( world, b3_taskCollide, b3CollideTask, contactCount, minRange, context ); - b3StackFree( &world->stack, contactIndices ); + b3TracyCZoneEnd( collide ); +} + +static void b3EndCollide( b3StepContext* context ) +{ + b3World* world = context->world; + + if ( context->collideContactCount == 0 ) + { + return; + } + + b3StackFree( &world->stack, context->awakeContactIndices ); context->awakeContactIndices = NULL; - contactIndices = NULL; // Serially update contact state // todo bring this zone together with island merge @@ -1033,154 +1060,102 @@ static void b3Collide( b3StepContext* context ) b3ValidateContacts( world ); b3TracyCZoneEnd( contact_state ); - b3TracyCZoneEnd( collide ); } -void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) +static void b3PrepareStepContext( b3World* world, b3StepContext* context ) { - b3World* world = b3GetUnlockedWorldFromId( worldId ); - if ( world == NULL ) - { - return; - } - - B3_REC( world, Step, worldId, timeStep, subStepCount ); - - world->locked = true; - - b3TracyCZoneNC( world_step, "Step", b3_colorBox2DGreen, true ); - - // Clear debug buffers - for ( int i = 0; i < world->workerCount; ++i ) - { - world->taskContexts.data[i].pointCount = 0; - world->taskContexts.data[i].lineCount = 0; - } - - // Prepare to capture events - // Ensure user does not access stale data if there is an early return - b3Array_Clear( world->bodyMoveEvents ); - b3Array_Clear( world->sensorBeginEvents ); - b3Array_Clear( world->contactBeginEvents ); - b3Array_Clear( world->contactHitEvents ); - b3Array_Clear( world->jointEvents ); - - world->profile = (b3Profile){ 0 }; - - world->activeTaskCount = 0; - world->taskCount = 0; - - if ( world->scheduler != NULL ) - { - b3ResetScheduler( world->scheduler ); - } - - uint64_t stepTicks = b3GetTicks(); - - { - b3Capacity* c = &world->maxCapacity; - c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); - c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); - - int staticBodyCount = b3GetSolverSet( world, b3_staticSet )->bodySims.count; - c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); - - // this includes kinematic bodies - int totalBodyCount = b3GetIdCount( &world->bodyIdPool ); - c->dynamicBodyCount = b3MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount ); - - int totalContactCount = b3GetIdCount( &world->contactIdPool ); - c->contactCount = b3MaxInt( c->contactCount, totalContactCount ); - } - - // Update collision pairs and create contacts - { - uint64_t pairTicks = b3GetTicks(); - b3UpdateBroadPhasePairs( world ); - world->profile.pairs = b3GetMilliseconds( pairTicks ); - } - - b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); - - b3StepContext context = { 0 }; - context.world = world; - context.states = awakeSet->bodyStates.data; - context.dt = timeStep; - context.subStepCount = b3MaxInt( 1, subStepCount ); + float timeStep = context->dt; + context->subStepCount = b3MaxInt( 1, context->subStepCount ); if ( timeStep > 0.0f ) { - context.inv_dt = 1.0f / timeStep; - context.h = timeStep / context.subStepCount; - context.inv_h = context.subStepCount * context.inv_dt; + context->inv_dt = 1.0f / timeStep; + context->h = timeStep / context->subStepCount; + context->inv_h = context->subStepCount * context->inv_dt; } else { - context.inv_dt = 0.0f; - context.h = 0.0f; - context.inv_h = 0.0f; + context->inv_dt = 0.0f; + context->h = 0.0f; + context->inv_h = 0.0f; } - world->inv_h = context.inv_h; - world->inv_dt = context.inv_dt; + world->inv_h = context->inv_h; + world->inv_dt = context->inv_dt; // Hertz values get reduced for large time steps - float contactHertz = b3MinFloat( world->contactHertz, 0.125f * context.inv_h ); - context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h ); - context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h ); + float contactHertz = b3MinFloat( world->contactHertz, 0.125f * context->inv_h ); + context->contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context->h ); + context->staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context->h ); - context.maxLinearVelocity = world->maxLinearSpeed; - context.enableWarmStarting = world->enableWarmStarting; + context->maxLinearVelocity = world->maxLinearSpeed; + context->enableWarmStarting = world->enableWarmStarting; +} - // Narrow phase : update contacts +static b3TaskBatch b3MakeTaskBatch( b3StepContext* context ) +{ + int deferredCount = context->batchTaskBase == 0 && context->tasks[0].deferred ? 1 : 0; + b3AtomicStoreInt( &context->pendingCount, context->batchTaskCount - deferredCount ); + + return (b3TaskBatch){ + .tasks = context->tasks + context->batchTaskBase, + .count = context->batchTaskCount, + .joinTasks = context->joinTree ? context->tasks : NULL, + .joinCount = context->joinTree ? 1 : 0, + }; +} + +static void b3LaunchTreeTask( b3StepContext* context, bool deferred ) +{ + B3_ASSERT( context->treeStaged && context->treeLaunched == false ); + context->batchTaskBase = 0; + context->batchTaskCount += 1; + context->treeLaunched = true; + + if ( deferred == false ) { - uint64_t collideTicks = b3GetTicks(); - b3Collide( &context ); - world->profile.collide = b3GetMilliseconds( collideTicks ); + context->tasks[0].deferred = false; + b3AtomicStoreInt( &context->treePending, 0 ); } +} - // Integrate velocities, solve velocity constraints, and integrate positions. - if ( timeStep > 0.0f ) +static void b3JoinTreeTask( b3StepContext* context ) +{ + if ( context->treeStaged ) { - uint64_t solveTicks = b3GetTicks(); - b3Solve( world, &context ); - world->profile.solve = b3GetMilliseconds( solveTicks ); + B3_ASSERT( b3AtomicLoadInt( &context->treePending ) == 0 ); + context->treeStaged = false; + b3ValidateNoMoved( &context->world->broadPhase ); } +} - // Finish the tree task in case b3Solve didn't finish it - if ( world->userTreeTask ) - { - world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); - world->userTreeTask = NULL; - world->activeTaskCount -= 1; - b3ValidateNoMoved( &world->broadPhase ); - } +static void b3EnterSensors( b3World* world, b3StepContext* context ) +{ + context->phase = b3_stepSensors; + context->sensorTicks = b3GetTicks(); + b3BeginSensors( world ); +} - // Update sensors - { - uint64_t sensorTicks = b3GetTicks(); - b3OverlapSensors( world ); - world->profile.sensors = b3GetMilliseconds( sensorTicks ); - } +static void b3EndStep( b3World* world ) +{ + b3StepContext* context = world->stepContext; + B3_ASSERT( context->treeStaged == false ); - world->profile.step = b3GetMilliseconds( stepTicks ); + world->profile.step = b3GetMilliseconds( context->stepTicks ); B3_ASSERT( world->stack.allocation == 0 ); // Ensure stack is large enough b3GrowStack( &world->stack ); - // Make sure all tasks that were started were also finished - B3_ASSERT( world->activeTaskCount == 0 ); - // Swap end event array buffers world->endEventArrayIndex = 1 - world->endEventArrayIndex; b3Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] ); b3Array_Clear( world->contactEndEvents[world->endEventArrayIndex] ); - world->locked = false; if ( world->recording != NULL ) { + b3WorldId worldId = { (uint16_t)( world->worldId + 1 ), world->generation }; uint64_t hash = b3HashWorldState( world ); b3RecArgs_StateHash stateHash = { worldId, hash }; b3RecWrite_StateHash( world->recording, &stateHash ); @@ -1205,10 +1180,398 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) } } - b3TracyCZoneEnd( world_step ); + context->phase = b3_stepIdle; + world->locked = false; + b3TracyCFrame; } +static b3TaskBatch b3AdvanceStep( b3World* world ) +{ + b3StepContext* context = world->stepContext; + + for ( ;; ) + { + b3ResetTaskBatch( context ); + + switch ( context->phase ) + { + case b3_stepBegin: + context->phase = b3_stepCrossPairs; + context->pairTicks = b3GetTicks(); + context->pairsUpdated = b3BeginPairs( world ); + break; + + case b3_stepCrossPairs: + context->phase = b3_stepSelfPairs; + if ( context->pairsUpdated ) + { + b3StageSelfPairs( world ); + } + break; + + case b3_stepSelfPairs: + if ( context->pairsUpdated ) + { + b3EndPairs( world ); + } + world->profile.pairs = b3GetMilliseconds( context->pairTicks ); + + b3PrepareStepContext( world, context ); + + context->phase = b3_stepCollide; + context->collideTicks = b3GetTicks(); + context->solveSkipped = + ( context->dt > 0.0f ) == false || b3GetSolverSet( world, b3_awakeSet )->bodySims.count == 0; + + b3BeginCollide( context ); + + if ( context->treeStaged && ( context->batchTaskCount > 0 || context->solveSkipped ) ) + { + b3LaunchTreeTask( context, context->solveSkipped == false ); + } + break; + + case b3_stepCollide: + b3EndCollide( context ); + world->profile.collide = b3GetMilliseconds( context->collideTicks ); + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( context->dt > 0.0f ) + { + context->phase = b3_stepSolve; + context->solveTicks = b3GetTicks(); + if ( b3BeginSolve( world, context ) ) + { + B3_ASSERT( context->solveSkipped == false ); + if ( context->treeStaged ) + { + if ( context->treeLaunched ) + { + context->joinTree = true; + } + else + { + b3LaunchTreeTask( context, false ); + } + } + break; + } + + world->profile.solve = b3GetMilliseconds( context->solveTicks ); + } + + B3_ASSERT( context->solveSkipped ); + b3JoinTreeTask( context ); + b3EnterSensors( world, context ); + break; + + case b3_stepSolve: + b3JoinTreeTask( context ); + context->phase = b3_stepFinalize; + b3BeginFinalize( world, context ); + break; + + case b3_stepFinalize: + context->phase = b3_stepBullets; + b3BeginBullets( world, context ); + break; + + case b3_stepBullets: + b3EndBullets( world, context ); + world->profile.solve = b3GetMilliseconds( context->solveTicks ); + b3EnterSensors( world, context ); + break; + + case b3_stepSensors: + b3EndSensors( world ); + world->profile.sensors = b3GetMilliseconds( context->sensorTicks ); + b3EndStep( world ); + return (b3TaskBatch){ 0 }; + + default: + B3_ASSERT( false ); + return (b3TaskBatch){ 0 }; + } + + if ( context->batchTaskCount > 0 ) + { + return b3MakeTaskBatch( context ); + } + } +} + +static b3TaskBatch b3StartStep( b3World* world, b3WorldId worldId, float timeStep, int subStepCount, + bool signalSemaphores ) +{ + // Record step inputs before simulation runs + B3_REC( world, Step, worldId, timeStep, subStepCount ); + + // Clear debug buffers + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].pointCount = 0; + world->taskContexts.data[i].lineCount = 0; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b3Array_Clear( world->bodyMoveEvents ); + b3Array_Clear( world->sensorBeginEvents ); + b3Array_Clear( world->contactBeginEvents ); + b3Array_Clear( world->contactHitEvents ); + b3Array_Clear( world->jointEvents ); + + world->profile = (b3Profile){ 0 }; + + world->locked = true; + + b3StepContext* context = world->stepContext; + b3Semaphore* batchSemaphore = context->batchSemaphore; + b3Semaphore* treeSemaphore = context->treeSemaphore; + memset( context, 0, sizeof( b3StepContext ) ); + context->batchSemaphore = batchSemaphore; + context->treeSemaphore = treeSemaphore; + context->signalSemaphores = signalSemaphores; + B3_ASSERT( signalSemaphores == false || ( batchSemaphore != NULL && treeSemaphore != NULL ) ); + + context->world = world; + context->dt = timeStep; + context->subStepCount = subStepCount; + context->phase = b3_stepBegin; + context->stepTicks = b3GetTicks(); + + { + b3Capacity* c = &world->maxCapacity; + c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); + c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); + + int staticBodyCount = b3GetSolverSet( world, b3_staticSet )->bodySims.count; + c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); + + // this includes kinematic bodies + int totalBodyCount = b3GetIdCount( &world->bodyIdPool ); + c->dynamicBodyCount = b3MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount ); + + int totalContactCount = b3GetIdCount( &world->contactIdPool ); + c->contactCount = b3MaxInt( c->contactCount, totalContactCount ); + } + + return b3AdvanceStep( world ); +} + +static b3TaskBatch b3ContinueStepInternal( b3World* world ) +{ + b3StepContext* context = world->stepContext; + B3_UNUSED( context ); + B3_ASSERT( b3AtomicLoadInt( &context->pendingCount ) == 0 ); + B3_ASSERT( context->joinTree == false || b3AtomicLoadInt( &context->treePending ) == 0 ); + return b3AdvanceStep( world ); +} + +typedef struct b3StepDriver +{ + b3World* world; + void* treeTask; + int enqueueCount; + bool useFinish; +} b3StepDriver; + +static void b3RunTaskBatch( b3StepDriver* driver, b3TaskBatch batch ) +{ + b3World* world = driver->world; + void* userContext = world->userTaskContext; + + void* handles[B3_STEP_TASK_CAPACITY]; + bool waitBatch = false; + int lastIndex = batch.count - 1; + B3_ASSERT( 0 <= lastIndex && lastIndex < B3_STEP_TASK_CAPACITY ); + + for ( int i = 0; i < lastIndex; ++i ) + { + const b3Task* task = batch.tasks + i; + void* handle = NULL; + if ( driver->enqueueCount < B3_MAX_TASKS ) + { + handle = world->enqueueTaskFcn( task->fcn, task->context, userContext, task->type ); + driver->enqueueCount += 1; + } + else + { + task->fcn( task->context ); + } + + if ( task->deferred ) + { + B3_ASSERT( driver->treeTask == NULL ); + driver->treeTask = handle; + handles[i] = NULL; + } + else + { + handles[i] = handle; + waitBatch = true; + } + } + + const b3Task* lastTask = batch.tasks + lastIndex; + waitBatch = waitBatch || lastTask->deferred == false; + lastTask->fcn( lastTask->context ); + + if ( driver->useFinish ) + { + for ( int i = 0; i < lastIndex; ++i ) + { + if ( handles[i] != NULL ) + { + world->finishTaskFcn( handles[i], userContext ); + } + } + } + else if ( waitBatch ) + { + b3WaitSemaphore( world->stepContext->batchSemaphore ); + } + + if ( batch.joinCount > 0 ) + { + if ( driver->useFinish ) + { + if ( driver->treeTask != NULL ) + { + world->finishTaskFcn( driver->treeTask, userContext ); + } + } + else + { + b3WaitSemaphore( world->stepContext->treeSemaphore ); + } + + driver->treeTask = NULL; + } +} + +void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3TracyCZoneNC( world_step, "Step", b3_colorBox2DGreen, true ); + + if ( world->useScheduler ) + { + if ( world->scheduler == NULL ) + { + world->scheduler = b3CreateScheduler( world->workerCount ); + world->userTaskContext = world->scheduler; + } + + b3ResetScheduler( world->scheduler ); + } + + b3StepDriver driver = { + .world = world, + .treeTask = NULL, + .enqueueCount = 0, + .useFinish = world->finishTaskFcn != NULL, + }; + + b3TaskBatch batch = b3StartStep( world, worldId, timeStep, subStepCount, driver.useFinish == false ); + while ( batch.count > 0 ) + { + switch ( world->stepContext->phase ) + { + case b3_stepCrossPairs: + case b3_stepSelfPairs: + { + b3TracyCZoneNC( find_pairs, "Find Pairs", b3_colorMediumSlateBlue, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( find_pairs ); + } + break; + + case b3_stepCollide: + { + b3TracyCZoneNC( collide, "Narrow Phase", b3_colorDarkOrchid, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( collide ); + } + break; + + case b3_stepSolve: + { + b3TracyCZoneNC( solve_constraints, "Solve Constraints", b3_colorIndigo, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( solve_constraints ); + } + break; + + case b3_stepFinalize: + { + b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( update_transforms ); + } + break; + + case b3_stepBullets: + { + b3TracyCZoneNC( bullets, "Bullets", b3_colorDarkGoldenRod, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( bullets ); + } + break; + + case b3_stepSensors: + { + b3TracyCZoneNC( sensors, "Sensors", b3_colorMediumPurple, true ); + b3RunTaskBatch( &driver, batch ); + b3TracyCZoneEnd( sensors ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + batch = b3ContinueStepInternal( world ); + } + + B3_ASSERT( driver.treeTask == NULL ); + world->stepContext->enqueueCount = driver.enqueueCount; + + b3TracyCZoneEnd( world_step ); +} + +b3TaskBatch b3World_BeginStep( b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( world->locked == false ); + if ( world->locked ) + { + return (b3TaskBatch){ 0 }; + } + + return b3StartStep( world, worldId, timeStep, subStepCount, false ); +} + +b3TaskBatch b3World_ContinueStep( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + b3StepContext* context = world->stepContext; + B3_ASSERT( world->locked && context->phase != b3_stepIdle ); + if ( context->phase == b3_stepIdle ) + { + return (b3TaskBatch){ 0 }; + } + + return b3ContinueStepInternal( world ); +} + typedef struct DrawContext { b3World* world; @@ -2300,14 +2663,31 @@ void b3World_SetWorkerCount( b3WorldId worldId, int count ) return; } - if ( count == world->workerCount ) + int clampedCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + + if ( clampedCount == world->workerCount ) { return; } + if ( world->enqueueTaskFcn == b3DefaultAddTaskFcn && clampedCount > 1 ) + { + world->useScheduler = true; + world->enqueueTaskFcn = b3SchedulerEnqueueTask; + world->finishTaskFcn = b3SchedulerFinishTask; + } + b3DestroyWorkerContexts( world ); - world->workerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + world->workerCount = clampedCount; b3CreateWorkerContexts( world ); + + if ( world->scheduler != NULL ) + { + // The scheduler will be recreated on the next world step. + b3DestroyScheduler( world->scheduler ); + world->scheduler = NULL; + world->userTaskContext = NULL; + } } int b3World_GetWorkerCount( b3WorldId worldId ) @@ -2457,6 +2837,14 @@ static uint64_t b3GetWorldByteCount( b3World* world, bool log ) b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); } + int stepContextBytes = (int)sizeof( b3StepContext ); + total += (uint64_t)stepContextBytes; + + if ( log ) + { + b3Log( "step context: %d", stepContextBytes ); + } + // Manifold block allocators, one per manifold point count int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators ); int manifoldBlockBytes = 0; @@ -2644,7 +3032,8 @@ b3Counters b3World_GetCounters( b3WorldId worldId ) memcpy( s.manifoldCounts, world->manifoldCounts, B3_MAX_MANIFOLDS * sizeof( int ) ); s.stackUsed = world->stack.maxAllocation; s.byteCount = (int64_t)b3GetWorldByteCount( world, false ); - s.taskCount = world->taskCount; + s.taskCount = world->stepContext->taskCount; + s.enqueueCount = world->stepContext->enqueueCount; _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); diff --git a/src/physics_world.h b/src/physics_world.h index 64d363e..a7ecd28 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -269,9 +269,10 @@ typedef struct b3World b3EnqueueTaskCallback* enqueueTaskFcn; b3FinishTaskCallback* finishTaskFcn; void* userTaskContext; - void* userTreeTask; + struct b3StepContext* stepContext; struct b3Scheduler* scheduler; + bool useScheduler; void* userData; @@ -285,9 +286,6 @@ typedef struct b3World // latest inverse full-step float inv_dt; - int activeTaskCount; - int taskCount; - uint16_t worldId; bool enableSleep; diff --git a/src/scheduler.c b/src/scheduler.c index ed942f2..9a2cdc9 100644 --- a/src/scheduler.c +++ b/src/scheduler.c @@ -7,6 +7,7 @@ #include "box3d/base.h" #include "box3d/constants.h" +#include "box3d/types.h" #include #include @@ -154,9 +155,9 @@ void b3ResetScheduler( b3Scheduler* scheduler ) b3AtomicStoreInt( &scheduler->nextSlot, 0 ); } -void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, b3TaskType taskType ) { - B3_UNUSED( name ); + B3_UNUSED( taskType ); b3Scheduler* scheduler = userContext; int slot = b3AtomicFetchAddInt( &scheduler->nextSlot, 1 ); diff --git a/src/scheduler.h b/src/scheduler.h index f9aeff7..b5963c5 100644 --- a/src/scheduler.h +++ b/src/scheduler.h @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #pragma once +#include "box3d/types.h" typedef void b3TaskCallback( void* taskContext ); typedef struct b3Scheduler b3Scheduler; @@ -11,5 +12,5 @@ void b3DestroyScheduler( b3Scheduler* scheduler ); void b3ResetScheduler( b3Scheduler* scheduler ); // See b3EnqueueTaskCallback and b3FinishTaskCallback -void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ); +void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, b3TaskType taskType ); void b3SchedulerFinishTask( void* userTask, void* userContext ); diff --git a/src/sensor.c b/src/sensor.c index 056708a..8dc1e26 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -278,7 +278,7 @@ static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* c b3TracyCZoneEnd( sensor_task ); } -void b3OverlapSensors( b3World* world ) +void b3BeginSensors( b3World* world ) { int sensorCount = world->sensors.count; if ( sensorCount == 0 ) @@ -288,8 +288,6 @@ void b3OverlapSensors( b3World* world ) B3_ASSERT( world->workerCount > 0 ); - b3TracyCZoneNC( overlap_sensors, "Sensors", b3_colorMediumPurple, true ); - for ( int i = 0; i < world->workerCount; ++i ) { b3SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount ); @@ -297,9 +295,18 @@ void b3OverlapSensors( b3World* world ) // Parallel-for sensors overlaps int minRange = 16; - b3ParallelFor( world, b3SensorTask, sensorCount, minRange, world, "sensors" ); + b3StageParallelFor( world, b3_taskSensors, b3SensorTask, sensorCount, minRange, world ); +} - b3TracyCZoneNC( sensor_state, "Events", b3_colorLightSlateGray, true ); +void b3EndSensors( b3World* world ) +{ + int sensorCount = world->sensors.count; + if ( sensorCount == 0 ) + { + return; + } + + b3TracyCZoneNC( sensor_state, "Sensor Events", b3_colorLightSlateGray, true ); b3BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits; for ( int i = 1; i < world->workerCount; ++i ) @@ -408,7 +415,6 @@ void b3OverlapSensors( b3World* world ) } b3TracyCZoneEnd( sensor_state ); - b3TracyCZoneEnd( overlap_sensors ); } void b3DestroySensor( b3World* world, b3Shape* sensorShape ) diff --git a/src/sensor.h b/src/sensor.h index 1a17366..3928719 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -37,5 +37,6 @@ typedef struct b3SensorTaskContext b3BitSet eventBits; } b3SensorTaskContext; -void b3OverlapSensors( b3World* world ); +void b3BeginSensors( b3World* world ); +void b3EndSensors( b3World* world ); void b3DestroySensor( b3World* world, b3Shape* sensorShape ); diff --git a/src/solver.c b/src/solver.c index 5a08eb0..46212a4 100644 --- a/src/solver.c +++ b/src/solver.c @@ -55,13 +55,6 @@ static void b3Pause( void ) } #endif -typedef struct b3WorkerContext -{ - b3StepContext* context; - int workerIndex; - void* userTask; -} b3WorkerContext; - // Integrate velocities, apply damping, and gyroscopic torque static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* context ) { @@ -1103,7 +1096,12 @@ static void b3ExecuteStage( b3SolverStage* stage, b3StepContext* context, int pr int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); if ( startIndex == B3_NULL_INDEX ) { - return; + if ( workerIndex != context->orchestratorIndex ) + { + return; + } + + startIndex = 0; } B3_ASSERT( 0 <= startIndex && startIndex < blockCount ); @@ -1131,7 +1129,7 @@ static void b3ExecuteStage( b3SolverStage* stage, b3StepContext* context, int pr (void)b3AtomicFetchAddInt( &stage->completionCount, completedCount ); } -// Execute a stage on worker 0 (main thread). +// Execute a stage on the orchestrator, whichever solver task claimed that role. static void b3ExecuteMainStage( b3SolverStage* stage, b3StepContext* context, uint32_t syncBits ) { int blockCount = stage->blockCount; @@ -1140,7 +1138,7 @@ static void b3ExecuteMainStage( b3SolverStage* stage, b3StepContext* context, ui return; } - const int workerIndex = 0; + int workerIndex = context->orchestratorIndex; if ( blockCount == 1 ) { @@ -1176,19 +1174,11 @@ static void b3SolverTask( void* taskContext ) b3SolverStage* stages = context->stages; b3Profile* profile = &context->world->profile; - if ( workerIndex == 0 ) + if ( b3AtomicCompareExchangeInt( &context->orchestratorClaimed, 0, 1 ) ) { - // The orchestrator slot is a race. The calling thread of b3World_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 ( b3AtomicCompareExchangeInt( &context->mainClaimed, 0, 1 ) == false ) - { - return; - } + context->orchestratorIndex = workerIndex; - // Main thread synchronizes the workers and does work itself. + // The orchestrator synchronizes the workers and does work itself. // // This needs to be a task for the main thread because the user's task system may execute // the tasks serially and this is the first task. This single task is able to fully @@ -1450,7 +1440,7 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi } // Solve with graph coloring -void b3Solve( b3World* world, b3StepContext* stepContext ) +bool b3BeginSolve( b3World* world, b3StepContext* stepContext ) { int simdShift = world->simdWidth == 8 ? 3 : 2; @@ -1461,9 +1451,11 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) int awakeBodyCount = awakeSet->bodySims.count; if ( awakeBodyCount == 0 ) { - return; + return false; } + stepContext->awakeBodyCount = awakeBodyCount; + // Solve constraints using graph coloring { b3TracyCZoneNC( solver_setup, "Solver Setup", b3_colorDarkOrange, true ); @@ -1671,9 +1663,9 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) // 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. - b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; - b3MeshPrepareSpan meshPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; - b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3WidePrepareSpan* widePrepareSpans = stepContext->widePrepareSpans; + b3MeshPrepareSpan* meshPrepareSpans = stepContext->meshPrepareSpans; + b3JointPrepareSpan* jointPrepareSpans = stepContext->jointPrepareSpans; // Distribute transient constraints to each graph color and prepare spans { @@ -1754,7 +1746,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) } //// Special span for overflow to allow for function re-use - b3ContactPrepareSpan overflowSpans[2] = { 0 }; + b3ContactPrepareSpan* overflowSpans = stepContext->overflowSpans; overflowSpans[0].start = 0; overflowSpans[0].count = overflow->contacts.count; overflowSpans[0].contacts = overflow->contacts.data; @@ -1795,27 +1787,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) b3SyncBlock* jointBlocks = b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" ); b3SyncBlock* graphBlocks = b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" ); - // Split an awake island. This modifies: - // - stack allocator - // - world island array and solver set - // - island indices on bodies, contacts, and joints - // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. - // Note: cannot split islands in parallel with FinalizeBodies - void* splitIslandTask = NULL; - if ( world->splitIslandId != B3_NULL_INDEX ) - { - if ( world->taskCount < B3_MAX_TASKS ) - { - splitIslandTask = world->enqueueTaskFcn( &b3SplitIslandTask, world, world->userTaskContext, "split" ); - world->taskCount += 1; - world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; - } - else - { - b3SplitIslandTask( world ); - } - } - // Prepare body blocks b3InitBlocks( bodyBlocks, bodyDim, awakeBodyCount, b3_bodyBlock, UINT8_MAX ); @@ -1869,29 +1840,30 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) B3_ASSERT( (int)( stage - stages ) == stageCount ); B3_ASSERT( workerCount <= B3_MAX_WORKERS ); - b3WorkerContext workerContext[B3_MAX_WORKERS]; stepContext->graph = graph; stepContext->activeColorCount = activeColorCount; stepContext->workerCount = workerCount; stepContext->stageCount = stageCount; stepContext->stages = stages; + stepContext->bodyBlocks = bodyBlocks; + stepContext->convexBlocks = convexBlocks; + stepContext->meshBlocks = meshBlocks; + stepContext->jointBlocks = jointBlocks; + stepContext->graphBlocks = graphBlocks; stepContext->wideConstraints = wideConstraints; - stepContext->widePrepareSpans = widePrepareSpans; stepContext->wideContactCount = wideContactCount; - stepContext->overflowSpans = overflowSpans; stepContext->wideMeshConstraints = wideMeshConstraints; stepContext->wideMeshManifoldStarts = meshManifoldStarts; - stepContext->meshPrepareSpans = meshPrepareSpans; - stepContext->jointPrepareSpans = jointPrepareSpans; + stepContext->meshLaneOrder = meshLaneOrder; + stepContext->orchestratorIndex = B3_NULL_INDEX; b3AtomicStoreU32( &stepContext->atomicSyncBits, 0 ); - b3AtomicStoreInt( &stepContext->mainClaimed, 0 ); + b3AtomicStoreInt( &stepContext->orchestratorClaimed, 0 ); world->profile.solverSetup = b3GetMillisecondsAndReset( &setupTicks ); b3TracyCZoneEnd( solver_setup ); - b3TracyCZoneNC( solve_constraints, "Solve Constraints", b3_colorIndigo, true ); - uint64_t constraintTicks = b3GetTicks(); + stepContext->constraintTicks = b3GetTicks(); int jointIdCapacity = b3GetIdCapacity( &world->jointIdPool ); int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); @@ -1901,102 +1873,91 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) b3SetBitCountAndClear( &taskContext->jointStateBitSet, jointIdCapacity ); b3SetBitCountAndClear( &taskContext->hitEventBitSet, contactIdCapacity ); taskContext->hasHitEvents = false; - - workerContext[i].context = stepContext; - workerContext[i].workerIndex = i; - - if ( world->taskCount < B3_MAX_TASKS ) - { - char buffer[16]; - snprintf( buffer, sizeof( buffer ), "solve[%d]", i ); - workerContext[i].userTask = - world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); - world->taskCount += 1; - world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; - } - else - { - workerContext[i].userTask = NULL; - b3SolverTask( workerContext + i ); - } } - // The calling thread of b3World_Step also enters b3SolverTask 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. - b3WorkerContext callerContext = { stepContext, 0, NULL }; - b3SolverTask( &callerContext ); + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + if ( world->splitIslandId != B3_NULL_INDEX ) + { + b3AddTask( stepContext, b3_taskIslandSplit, &b3SplitIslandTask, world ); + } - // Finish constraint solve for ( int i = 0; i < workerCount; ++i ) { - if ( workerContext[i].userTask != NULL ) - { - world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); - world->activeTaskCount -= 1; - } + b3WorkerContext* workerContext = stepContext->workerContexts + i; + workerContext->context = stepContext; + workerContext->workerIndex = i; + + b3AddTask( stepContext, b3_taskSolver, &b3SolverTask, workerContext ); } - - // Finish island split - if ( splitIslandTask != NULL ) - { - world->finishTaskFcn( splitIslandTask, world->userTaskContext ); - world->activeTaskCount -= 1; - } - world->splitIslandId = B3_NULL_INDEX; - - world->profile.constraints = b3GetMillisecondsAndReset( &constraintTicks ); - b3TracyCZoneEnd( solve_constraints ); - - b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); - uint64_t transformTicks = b3GetTicks(); - - // Prepare the island bit set used in body finalization. - int awakeIslandCount = awakeSet->islandSims.count; - for ( int i = 0; i < world->workerCount; ++i ) - { - b3TaskContext* taskContext = world->taskContexts.data + i; - b3Array_Clear( taskContext->sensorHits ); - b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); - taskContext->splitIslandId = B3_NULL_INDEX; - taskContext->splitSleepTime = 0.0f; - } - - // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the - // broad-phase. - if ( world->userTreeTask != NULL ) - { - world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); - world->userTreeTask = NULL; - world->activeTaskCount -= 1; - } - - b3ValidateNoMoved( &world->broadPhase ); - - // Finalize bodies. Must happen after the constraint solver and after island splitting. - b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 8, stepContext, "ccd" ); - - // Free in reverse order - b3StackFree( &world->stack, graphBlocks ); - b3StackFree( &world->stack, jointBlocks ); - b3StackFree( &world->stack, meshBlocks ); - b3StackFree( &world->stack, convexBlocks ); - b3StackFree( &world->stack, bodyBlocks ); - b3StackFree( &world->stack, stages ); - b3StackFree( &world->stack, overflow->manifoldConstraints ); - b3StackFree( &world->stack, overflow->contactConstraints ); - b3StackFree( &world->stack, wideMeshConstraints ); - b3StackFree( &world->stack, meshManifoldStarts ); - b3StackFree( &world->stack, meshLaneOrder ); - b3StackFree( &world->stack, wideConstraints ); - - world->profile.transforms = b3GetMilliseconds( transformTicks ); - b3TracyCZoneEnd( update_transforms ); } + return true; +} + +void b3BeginFinalize( b3World* world, b3StepContext* stepContext ) +{ + world->splitIslandId = B3_NULL_INDEX; + world->profile.constraints = b3GetMilliseconds( stepContext->constraintTicks ); + + stepContext->transformTicks = b3GetTicks(); + + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); + int awakeBodyCount = stepContext->awakeBodyCount; + B3_ASSERT( awakeBodyCount == awakeSet->bodySims.count ); + + // Prepare the island bit set used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3Array_Clear( taskContext->sensorHits ); + b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B3_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + b3ValidateNoMoved( &world->broadPhase ); + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + b3StageParallelFor( world, b3_taskFinalize, &b3FinalizeBodiesTask, awakeBodyCount, 8, stepContext ); +} + +void b3BeginBullets( b3World* world, b3StepContext* stepContext ) +{ + // Free in reverse order + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + + b3StackFree( &world->stack, stepContext->graphBlocks ); + b3StackFree( &world->stack, stepContext->jointBlocks ); + b3StackFree( &world->stack, stepContext->meshBlocks ); + b3StackFree( &world->stack, stepContext->convexBlocks ); + b3StackFree( &world->stack, stepContext->bodyBlocks ); + b3StackFree( &world->stack, stepContext->stages ); + b3StackFree( &world->stack, overflow->manifoldConstraints ); + b3StackFree( &world->stack, overflow->contactConstraints ); + b3StackFree( &world->stack, stepContext->wideMeshConstraints ); + b3StackFree( &world->stack, stepContext->wideMeshManifoldStarts ); + b3StackFree( &world->stack, stepContext->meshLaneOrder ); + b3StackFree( &world->stack, stepContext->wideConstraints ); + + stepContext->graphBlocks = NULL; + stepContext->jointBlocks = NULL; + stepContext->meshBlocks = NULL; + stepContext->convexBlocks = NULL; + stepContext->bodyBlocks = NULL; + stepContext->stages = NULL; + stepContext->wideMeshConstraints = NULL; + stepContext->wideMeshManifoldStarts = NULL; + stepContext->meshLaneOrder = NULL; + stepContext->wideConstraints = NULL; + + world->profile.transforms = b3GetMilliseconds( stepContext->transformTicks ); + // Report joint events { b3TracyCZoneNC( joint_events, "Joint Events", b3_colorPeru, true ); @@ -2180,13 +2141,23 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) int bulletBodyCount = b3AtomicLoadInt( &stepContext->bulletBodyCount ); if ( bulletBodyCount > 0 ) { - b3TracyCZoneNC( bullets, "Bullets", b3_colorDarkGoldenRod, true ); - uint64_t bulletTicks = b3GetTicks(); + stepContext->bulletTicks = b3GetTicks(); // Fast bullet bodies // Note: a bullet body may be moving slow int minRange = 8; - b3ParallelFor( world, &b3BulletBodyTask, bulletBodyCount, minRange, stepContext, "bullets" ); + b3StageParallelFor( world, b3_taskContinuous, &b3BulletBodyTask, bulletBodyCount, minRange, stepContext ); + } +} + +void b3EndBullets( b3World* world, b3StepContext* stepContext ) +{ + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); + + int bulletBodyCount = b3AtomicLoadInt( &stepContext->bulletBodyCount ); + if ( bulletBodyCount > 0 ) + { + b3TracyCZoneNC( bullets, "Bullet Proxies", b3_colorDarkGoldenRod, true ); // Serially enlarge broad-phase proxies for bullet shapes b3BroadPhase* broadPhase = &world->broadPhase; @@ -2237,7 +2208,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) } } - world->profile.bullets = b3GetMilliseconds( bulletTicks ); + world->profile.bullets = b3GetMilliseconds( stepContext->bulletTicks ); b3TracyCZoneEnd( bullets ); } diff --git a/src/solver.h b/src/solver.h index 297213d..f7ae1ab 100644 --- a/src/solver.h +++ b/src/solver.h @@ -50,9 +50,13 @@ #include "container.h" #include "core.h" +#include "parallel_for.h" +#include "box3d/constants.h" #include "box3d/math_functions.h" +#include "box3d/types.h" +#include #include typedef struct b3BodySim b3BodySim; @@ -64,6 +68,34 @@ typedef struct b3Manifold b3Manifold; typedef struct b3ManifoldConstraint b3ManifoldConstraint; typedef struct b3World b3World; +typedef enum b3StepPhase +{ + b3_stepIdle, + b3_stepBegin, + b3_stepCrossPairs, + b3_stepSelfPairs, + b3_stepCollide, + b3_stepSolve, + b3_stepFinalize, + b3_stepBullets, + b3_stepSensors, +} b3StepPhase; + +typedef struct b3StepTask +{ + b3TaskCallback* fcn; + void* context; + b3StepContext* stepContext; +} b3StepTask; + +typedef struct b3WorkerContext +{ + b3StepContext* context; + int workerIndex; +} b3WorkerContext; + +#define B3_STEP_TASK_CAPACITY ( B3_MAX_WORKERS + 2 ) + // Solver stages typedef enum b3SolverStageType { @@ -181,6 +213,48 @@ typedef struct b3JointPrepareSpan // Context for a time step. Recreated each time step. typedef struct b3StepContext { + b3StepPhase phase; + bool pairsUpdated; + bool treeStaged; + bool treeLaunched; + bool solveSkipped; + bool joinTree; + bool signalSemaphores; + + int batchTaskBase; + int batchTaskCount; + int taskCount; + int enqueueCount; + + b3Task tasks[B3_STEP_TASK_CAPACITY]; + b3StepTask stepTasks[B3_STEP_TASK_CAPACITY]; + + b3ParallelForShared parallelFor; + b3ParallelForTask parallelForTasks[B3_MAX_WORKERS]; + b3WorkerContext workerContexts[B3_MAX_WORKERS]; + + b3Semaphore* batchSemaphore; + b3Semaphore* treeSemaphore; + + uint64_t stepTicks; + uint64_t pairTicks; + uint64_t collideTicks; + uint64_t solveTicks; + uint64_t constraintTicks; + uint64_t transformTicks; + uint64_t bulletTicks; + uint64_t sensorTicks; + + int awakeBodyCount; + int collideContactCount; + + b3SyncBlock* bodyBlocks; + b3SyncBlock* convexBlocks; + b3SyncBlock* meshBlocks; + b3SyncBlock* jointBlocks; + b3SyncBlock* graphBlocks; + int* meshLaneOrder; + // time step float dt; @@ -222,25 +296,26 @@ typedef struct b3StepContext int* awakeContactIndices; // 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 + // widePrepareSpans has activeColorCount + 1 entries, the last being a sentinel + // at wideContactCount. wideConstraints is the contiguous base // pointer. Per-color slices live at colors[i].wideConstraints. void* wideConstraints; - b3WidePrepareSpan* widePrepareSpans; + b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; int wideContactCount; // Similar for overflow contact constraints. - b3ContactPrepareSpan* overflowSpans; + b3ContactPrepareSpan overflowSpans[2]; void* wideMeshConstraints; int* wideMeshManifoldStarts; - b3MeshPrepareSpan* meshPrepareSpans; - b3JointPrepareSpan* jointPrepareSpans; + b3MeshPrepareSpan meshPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; int activeColorCount; int workerCount; b3SolverStage* stages; int stageCount; + int orchestratorIndex; bool enableWarmStarting; // padding to prevent false sharing @@ -256,18 +331,19 @@ typedef struct b3StepContext // padding to prevent false sharing char padding2[64]; - // Race flag claimed by whichever runner reaches b3SolverTask with workerIndex 0 first. - // The calling thread of b3World_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. - b3AtomicInt mainClaimed; + b3AtomicInt orchestratorClaimed; // padding to prevent false sharing char padding3[64]; + + b3AtomicInt pendingCount; + b3AtomicInt treePending; } b3StepContext; -void b3Solve( b3World* world, b3StepContext* stepContext ); +bool b3BeginSolve( b3World* world, b3StepContext* stepContext ); +void b3BeginFinalize( b3World* world, b3StepContext* stepContext ); +void b3BeginBullets( b3World* world, b3StepContext* stepContext ); +void b3EndBullets( b3World* world, b3StepContext* stepContext ); static inline b3Softness b3MakeSoft( float hertz, float zeta, float h ) { diff --git a/src/types.c b/src/types.c index f5e1ad3..284e6e1 100644 --- a/src/types.c +++ b/src/types.c @@ -7,6 +7,39 @@ #include "box3d/constants.h" +const char* b3GetTaskTypeName( b3TaskType type ) +{ + switch ( type ) + { + case b3_taskPairs: + return "pairs"; + + case b3_taskCollide: + return "collide"; + + case b3_taskTreeRebuild: + return "tree"; + + case b3_taskIslandSplit: + return "split"; + + case b3_taskSolver: + return "solve"; + + case b3_taskFinalize: + return "finalize"; + + case b3_taskContinuous: + return "continuous"; + + case b3_taskSensors: + return "sensors"; + + default: + return ""; + } +} + b3WorldDef b3DefaultWorldDef( void ) { float lengthUnits = b3GetLengthUnitsPerMeter(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ee6fceb..b73e089 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -29,6 +29,7 @@ set(BOX3D_TEST_FILES test_restitution.c test_sat.c test_shape.c + test_step.c test_table.c test_world.c ) diff --git a/test/main.c b/test/main.c index df993cf..37980e7 100644 --- a/test/main.c +++ b/test/main.c @@ -51,6 +51,7 @@ extern int RecordingTest( void ); extern int RestitutionTest( void ); extern int SeparatingAxisTest( void ); extern int ShapeTest( void ); +extern int StepTest( void ); extern int TableTest( void ); extern int WorldTest( void ); @@ -129,6 +130,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( RestitutionTest ); MAYBE_RUN_TEST( SeparatingAxisTest ); MAYBE_RUN_TEST( ShapeTest ); + MAYBE_RUN_TEST( StepTest ); MAYBE_RUN_TEST( TableTest ); MAYBE_RUN_TEST( WorldTest ); diff --git a/test/test_step.c b/test/test_step.c new file mode 100644 index 0000000..97ff13c --- /dev/null +++ b/test/test_step.c @@ -0,0 +1,829 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" +#include "determinism.h" +#include "physics_world.h" +#include "platform.h" +#include "recording.h" +#include "test_macros.h" + +#include "box3d/box3d.h" +#include "box3d/types.h" + +#include +#include + +#define MAX_BATCH_TASKS ( B3_MAX_WORKERS + 2 ) + +typedef enum TaskOrder +{ + e_forward, + e_reverse, +} TaskOrder; + +// Runs every batch on the calling thread. Honoring the deferred flag holds the tree rebuild back until +// right before the Continue that joins it, which catches a join list placed on the wrong batch. +typedef struct SerialHost +{ + TaskOrder order; + bool honorDeferred; + int batchCount; + int deferredBatch; + int joinBatch; + int deferredOnlyBatchCount; + int maxBatchSize; + int taskCount; + const b3Task* heldTask; +} SerialHost; + +static void RunSerialBatch( SerialHost* host, b3TaskBatch batch ) +{ + host->maxBatchSize = batch.count > host->maxBatchSize ? batch.count : host->maxBatchSize; + + int deferredCount = 0; + for ( int k = 0; k < batch.count; ++k ) + { + int i = host->order == e_forward ? k : batch.count - 1 - k; + const b3Task* task = batch.tasks + i; + if ( task->deferred ) + { + deferredCount += 1; + host->deferredBatch = host->batchCount; + if ( host->honorDeferred ) + { + host->heldTask = task; + continue; + } + } + + task->fcn( task->context ); + } + + if ( deferredCount == batch.count ) + { + host->deferredOnlyBatchCount += 1; + } + + if ( batch.joinCount > 0 ) + { + host->joinBatch = host->batchCount; + if ( host->heldTask != NULL ) + { + host->heldTask->fcn( host->heldTask->context ); + host->heldTask = NULL; + } + } + + host->batchCount += 1; +} + +static int SerialPhasedStep( SerialHost* host, b3WorldId worldId, float timeStep, int subStepCount ) +{ + host->batchCount = 0; + host->taskCount = 0; + host->deferredBatch = -1; + host->joinBatch = -1; + host->heldTask = NULL; + + b3TaskBatch batch = b3World_BeginStep( worldId, timeStep, subStepCount ); + while ( batch.count > 0 ) + { + ENSURE( batch.count <= MAX_BATCH_TASKS ); + ENSURE( batch.joinCount == 0 || batch.joinCount == 1 ); + for ( int i = 0; i < batch.count; ++i ) + { + b3TaskType type = batch.tasks[i].type; + ENSURE( 0 <= type && type < b3_taskTypeCount ); + ENSURE( batch.tasks[i].deferred == false || type == b3_taskTreeRebuild ); + } + host->taskCount += batch.count; + RunSerialBatch( host, batch ); + batch = b3World_ContinueStep( worldId ); + } + + ENSURE( host->heldTask == NULL ); + ENSURE( b3World_GetCounters( worldId ).taskCount == host->taskCount ); + ENSURE( b3World_GetCounters( worldId ).enqueueCount == 0 ); + ENSURE( ( host->deferredBatch == -1 ) == ( host->joinBatch == -1 ) ); + ENSURE( host->deferredBatch < host->joinBatch || host->deferredBatch == -1 ); + return 0; +} + +// A pool of threads draining a queue that only grows. Slots are never reused, so producers and +// consumers need no reset handshake. +#define POOL_SLOT_CAPACITY ( 1 << 18 ) +#define POOL_MAX_THREADS 8 + +typedef struct PoolSlot +{ + b3TaskCallback* fcn; + void* context; + b3AtomicInt* counter; +} PoolSlot; + +typedef struct Pool +{ + PoolSlot* slots; + b3AtomicInt published; + b3AtomicInt claimed; + b3AtomicInt quit; + b3Thread* threads[POOL_MAX_THREADS]; + int threadCount; +} Pool; + +static bool PoolRunOne( Pool* pool ) +{ + int claimed = b3AtomicLoadInt( &pool->claimed ); + int published = b3AtomicLoadInt( &pool->published ); + if ( claimed >= published ) + { + return false; + } + + if ( b3AtomicCompareExchangeInt( &pool->claimed, claimed, claimed + 1 ) == false ) + { + return true; + } + + PoolSlot* slot = pool->slots + claimed; + slot->fcn( slot->context ); + if ( slot->counter != NULL ) + { + b3AtomicFetchAddInt( slot->counter, -1 ); + } + + return true; +} + +static void PoolThreadMain( void* context ) +{ + Pool* pool = context; + while ( b3AtomicLoadInt( &pool->quit ) == 0 ) + { + if ( PoolRunOne( pool ) == false ) + { + b3Yield(); + } + } +} + +static void PoolPush( Pool* pool, b3TaskCallback* fcn, void* context, b3AtomicInt* counter ) +{ + int index = b3AtomicLoadInt( &pool->published ); + B3_ASSERT( index < POOL_SLOT_CAPACITY ); + pool->slots[index] = (PoolSlot){ fcn, context, counter }; + b3AtomicStoreInt( &pool->published, index + 1 ); +} + +static void PoolHelpUntilZero( Pool* pool, b3AtomicInt* counter ) +{ + while ( b3AtomicLoadInt( counter ) > 0 ) + { + if ( PoolRunOne( pool ) == false ) + { + b3Yield(); + } + } +} + +static void CreatePool( Pool* pool, int threadCount ) +{ + memset( pool, 0, sizeof( Pool ) ); + pool->slots = b3Alloc( POOL_SLOT_CAPACITY * sizeof( PoolSlot ) ); + pool->threadCount = threadCount; + for ( int i = 0; i < threadCount; ++i ) + { + pool->threads[i] = b3CreateThread( PoolThreadMain, pool, "step_test" ); + } +} + +static void DestroyPool( Pool* pool ) +{ + b3AtomicStoreInt( &pool->quit, 1 ); + for ( int i = 0; i < pool->threadCount; ++i ) + { + b3JoinThread( pool->threads[i] ); + } + b3Free( pool->slots, POOL_SLOT_CAPACITY * sizeof( PoolSlot ) ); +} + +// Pull host on the pool. The rebuild stays in flight across Continue calls until a batch joins it. +static void PooledPhasedStep( Pool* pool, b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3AtomicInt batchCounter; + b3AtomicInt treeCounter; + b3AtomicStoreInt( &treeCounter, 0 ); + + b3TaskBatch batch = b3World_BeginStep( worldId, timeStep, subStepCount ); + while ( batch.count > 0 ) + { + int nonDeferred = 0; + for ( int i = 0; i < batch.count; ++i ) + { + nonDeferred += batch.tasks[i].deferred ? 0 : 1; + } + + b3AtomicStoreInt( &batchCounter, nonDeferred ); + + for ( int i = 0; i < batch.count; ++i ) + { + const b3Task* task = batch.tasks + i; + if ( task->deferred ) + { + b3AtomicStoreInt( &treeCounter, 1 ); + PoolPush( pool, task->fcn, task->context, &treeCounter ); + } + else + { + PoolPush( pool, task->fcn, task->context, &batchCounter ); + } + } + + PoolHelpUntilZero( pool, &batchCounter ); + + if ( batch.joinCount > 0 ) + { + PoolHelpUntilZero( pool, &treeCounter ); + } + + batch = b3World_ContinueStep( worldId ); + } + + B3_ASSERT( b3AtomicLoadInt( &treeCounter ) == 0 ); +} + +static int enqueueOnlyCount; + +static void* EnqueueOnlyTask( b3TaskCallback* task, void* taskContext, void* userContext, b3TaskType taskType ) +{ + B3_UNUSED( taskType ); + enqueueOnlyCount += 1; + Pool* pool = userContext; + PoolPush( pool, task, taskContext, NULL ); + return pool; +} + +typedef enum HostKind +{ + e_syncHost, + e_serialHost, + e_pooledHost, +} HostKind; + +typedef struct HostConfig +{ + HostKind kind; + TaskOrder order; + bool honorDeferred; + int workerCount; + int threadCount; +} HostConfig; + +static int RunFallingHinges( HostConfig config ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = config.workerCount; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + Pool pool; + if ( config.kind == e_pooledHost ) + { + CreatePool( &pool, config.threadCount - 1 ); + } + + SerialHost serialHost = { .order = config.order, .honorDeferred = config.honorDeferred }; + + FallingHingeData data = CreateFallingHinges( worldId ); + + float timeStep = 1.0f / 60.0f; + int stepLimit = 1000; + for ( int i = 0; i < stepLimit; ++i ) + { + int subStepCount = 4; + if ( config.kind == e_serialHost ) + { + int result = SerialPhasedStep( &serialHost, worldId, timeStep, subStepCount ); + ENSURE( result == 0 ); + } + else if ( config.kind == e_pooledHost ) + { + PooledPhasedStep( &pool, worldId, timeStep, subStepCount ); + } + else + { + b3World_Step( worldId, timeStep, subStepCount ); + } + + if ( UpdateFallingHinges( worldId, &data ) ) + { + break; + } + } + + if ( config.kind != e_syncHost ) + { + ENSURE( b3GetWorldFromId( worldId )->scheduler == NULL ); + } + + ENSURE( serialHost.maxBatchSize <= config.workerCount + 2 ); + + b3DestroyWorld( worldId ); + + if ( config.kind == e_pooledHost ) + { + DestroyPool( &pool ); + } + + if ( data.sleepStep != FALLING_HINGE_SLEEP_STEP || data.hash != FALLING_HINGE_HASH ) + { + printf( " host=%d order=%d deferred=%d workers=%d threads=%d sleepStep=%d hash=0x%08X\n", config.kind, config.order, + config.honorDeferred ? 1 : 0, config.workerCount, config.threadCount, data.sleepStep, data.hash ); + } + + ENSURE( data.sleepStep == FALLING_HINGE_SLEEP_STEP ); + ENSURE( data.hash == FALLING_HINGE_HASH ); + + DestroyFallingHinges( &data ); + + return 0; +} + +static int SerialPullTest( void ) +{ + int workerCounts[] = { 1, 4, 8 }; + for ( int i = 0; i < 3; ++i ) + { + HostConfig config = { .kind = e_serialHost, .order = e_forward, .workerCount = workerCounts[i] }; + ENSURE( RunFallingHinges( config ) == 0 ); + } + + return 0; +} + +// The first solver task to run must orchestrate whatever its slot, or a serial host strands a spinner +static int ReverseOrderTest( void ) +{ + int workerCounts[] = { 4, 8 }; + for ( int i = 0; i < 2; ++i ) + { + HostConfig config = { .kind = e_serialHost, .order = e_reverse, .workerCount = workerCounts[i] }; + ENSURE( RunFallingHinges( config ) == 0 ); + } + + return 0; +} + +static int DeferredJoinTest( void ) +{ + int workerCounts[] = { 1, 4 }; + for ( int i = 0; i < 2; ++i ) + { + HostConfig config = { + .kind = e_serialHost, .order = e_forward, .honorDeferred = true, .workerCount = workerCounts[i] }; + ENSURE( RunFallingHinges( config ) == 0 ); + + config.order = e_reverse; + ENSURE( RunFallingHinges( config ) == 0 ); + } + + return 0; +} + +static int PooledPullTest( void ) +{ + int pairs[][2] = { { 1, 1 }, { 2, 2 }, { 4, 4 }, { 8, 8 }, { 8, 2 }, { 2, 8 } }; + for ( int i = 0; i < 6; ++i ) + { + HostConfig config = { .kind = e_pooledHost, .workerCount = pairs[i][0], .threadCount = pairs[i][1] }; + ENSURE( RunFallingHinges( config ) == 0 ); + } + + return 0; +} + +static int EnqueueOnlyTest( void ) +{ + int workerCounts[] = { 2, 4 }; + for ( int i = 0; i < 2; ++i ) + { + Pool pool; + CreatePool( &pool, 3 ); + + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCounts[i]; + worldDef.enqueueTask = EnqueueOnlyTask; + worldDef.finishTask = NULL; + worldDef.userTaskContext = &pool; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + FallingHingeData data = CreateFallingHinges( worldId ); + + float timeStep = 1.0f / 60.0f; + for ( int step = 0; step < 1000; ++step ) + { + enqueueOnlyCount = 0; + b3World_Step( worldId, timeStep, 4 ); + b3Counters counters = b3World_GetCounters( worldId ); + ENSURE( counters.enqueueCount == enqueueOnlyCount ); + ENSURE( counters.enqueueCount <= counters.taskCount ); + + if ( UpdateFallingHinges( worldId, &data ) ) + { + break; + } + } + + ENSURE( b3GetWorldFromId( worldId )->scheduler == NULL ); + + b3DestroyWorld( worldId ); + DestroyPool( &pool ); + + ENSURE( data.sleepStep == FALLING_HINGE_SLEEP_STEP ); + ENSURE( data.hash == FALLING_HINGE_HASH ); + + DestroyFallingHinges( &data ); + } + + return 0; +} + +static int AlternateTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 4; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + FallingHingeData data = CreateFallingHinges( worldId ); + SerialHost host = { .order = e_reverse, .honorDeferred = true }; + + float timeStep = 1.0f / 60.0f; + for ( int i = 0; i < 1000; ++i ) + { + if ( ( i & 1 ) == 0 ) + { + b3World_Step( worldId, timeStep, 4 ); + } + else + { + ENSURE( SerialPhasedStep( &host, worldId, timeStep, 4 ) == 0 ); + } + + if ( UpdateFallingHinges( worldId, &data ) ) + { + break; + } + } + + b3DestroyWorld( worldId ); + + ENSURE( data.sleepStep == FALLING_HINGE_SLEEP_STEP ); + ENSURE( data.hash == FALLING_HINGE_HASH ); + + DestroyFallingHinges( &data ); + + return 0; +} + +static int EmptyWorldTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 4; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + SerialHost host = { .order = e_forward }; + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + ENSURE( host.batchCount == 0 ); + ENSURE( host.deferredBatch == -1 ); + + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( SerialPhasedStep( &host, worldId, 0.0f, 4 ) == 0 ); + ENSURE( host.batchCount == 0 ); + + b3DestroyWorld( worldId ); + return 0; +} + +static b3BodyId CreateBall( b3WorldId worldId, b3Pos position ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + return bodyId; +} + +// The rebuild must join on the solve batch normally and on the collide batch when solve is skipped +static int JoinPlacementTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 4; + + // A lone ball, no contacts. A zero step skips solve, so the collide batch is the rebuild alone and + // nothing later can join it. It runs as a regular task. + { + b3WorldId worldId = b3CreateWorld( &worldDef ); + b3BodyId bodyId = CreateBall( worldId, (b3Pos){ 0.0f, 4.0f, 0.0f } ); + + SerialHost host = { .order = e_forward, .honorDeferred = true }; + ENSURE( SerialPhasedStep( &host, worldId, 0.0f, 4 ) == 0 ); + ENSURE( host.batchCount > 0 ); + ENSURE( host.deferredBatch == -1 ); + ENSURE( host.deferredOnlyBatchCount == 0 ); + + b3Pos p = b3Body_GetPosition( bodyId ); + ENSURE( p.x == 0.0f && p.y == 4.0f ); + + b3DestroyWorld( worldId ); + } + + // A lone moving ball, no contacts. The collide batch is empty, so the rebuild rides with the solver + // tasks as a regular task instead of running alone. + { + b3WorldId worldId = b3CreateWorld( &worldDef ); + b3BodyId bodyId = CreateBall( worldId, (b3Pos){ 0.0f, 4.0f, 0.0f } ); + b3Body_SetLinearVelocity( bodyId, (b3Vec3){ 10.0f, 0.0f, 0.0f } ); + + SerialHost host = { .order = e_forward, .honorDeferred = true }; + for ( int i = 0; i < 4; ++i ) + { + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + ENSURE( host.deferredBatch == -1 ); + } + + ENSURE( host.deferredOnlyBatchCount == 0 ); + + b3DestroyWorld( worldId ); + } + + // Touching balls on the ground with a zero step: the collide batch has contacts and the join + { + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef groundDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &groundDef ); + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + for ( int i = 0; i < 8; ++i ) + { + CreateBall( worldId, (b3Pos){ -4.0f + 1.0f * i, 0.5f, 0.0f } ); + } + + SerialHost host = { .order = e_forward, .honorDeferred = true }; + ENSURE( SerialPhasedStep( &host, worldId, 0.0f, 4 ) == 0 ); + ENSURE( SerialPhasedStep( &host, worldId, 0.0f, 4 ) == 0 ); + + // Moving step: the join lands on a later batch than the rebuild + for ( int i = 0; i < 4; ++i ) + { + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + if ( host.deferredBatch >= 0 ) + { + ENSURE( host.joinBatch > host.deferredBatch ); + } + } + + b3DestroyWorld( worldId ); + } + + return 0; +} + +// No contacts, a moving ball that keeps the rebuild staged, and a resting pair whose joint is destroyed. +// The pair can only sleep after its island splits, so the solve batch carries the rebuild, the split and +// the solver tasks together. +static int SplitBatchTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 4; + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyId bodyA = CreateBall( worldId, (b3Pos){ 0.0f, 0.0f, 0.0f } ); + b3BodyId bodyB = CreateBall( worldId, (b3Pos){ 2.0f, 0.0f, 0.0f } ); + b3BodyId ballId = CreateBall( worldId, (b3Pos){ 0.0f, 20.0f, 0.0f } ); + b3Body_SetLinearVelocity( ballId, (b3Vec3){ 10.0f, 0.0f, 0.0f } ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = bodyA; + jointDef.base.bodyIdB = bodyB; + jointDef.base.localFrameA.p = (b3Vec3){ 1.0f, 0.0f, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ -1.0f, 0.0f, 0.0f }; + b3JointId jointId = b3CreateRevoluteJoint( worldId, &jointDef ); + + SerialHost host = { .order = e_forward, .honorDeferred = true }; + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + + b3DestroyJoint( jointId, true ); + + int stepCount = 0; + while ( b3Body_IsAwake( bodyA ) && stepCount < 120 ) + { + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + stepCount += 1; + } + + ENSURE( host.maxBatchSize == worldDef.workerCount + 2 ); + ENSURE( b3Body_IsAwake( bodyA ) == false ); + ENSURE( b3Body_IsAwake( ballId ) ); + + b3DestroyWorld( worldId ); + return 0; +} + +// A scene with bullets, a sensor, joints and sleeping islands, checked against the synchronous step +// every step by state hash. +static b3WorldId CreateMixedScene( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef groundDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &groundDef ); + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3BoxHull groundBox = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + b3ShapeDef sensorDef = b3DefaultShapeDef(); + sensorDef.isSensor = true; + sensorDef.enableSensorEvents = true; + b3BoxHull sensorBox = b3MakeOffsetBoxHull( 4.0f, 2.0f, 4.0f, (b3Vec3){ 10.0f, 2.0f, 0.0f } ); + b3CreateHullShape( groundId, &sensorDef, &sensorBox.base ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef boxDef = b3DefaultShapeDef(); + boxDef.enableSensorEvents = true; + boxDef.enableHitEvents = true; + + for ( int row = 0; row < 10; ++row ) + { + for ( int col = 0; col < 10 - row; ++col ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ -5.0f + col * 1.05f + row * 0.525f, 0.5f + row * 1.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &boxDef, &box.base ); + } + } + + b3BodyId prevId = groundId; + for ( int i = 0; i < 6; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 12.0f + i * 1.0f, 8.0f, 0.0f }; + b3BodyId linkId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull link = b3MakeBoxHull( 0.5f, 0.125f, 0.125f ); + b3CreateHullShape( linkId, &boxDef, &link.base ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = prevId; + jointDef.base.bodyIdB = linkId; + jointDef.base.localFrameA.p = + prevId.index1 == groundId.index1 ? (b3Vec3){ 11.5f, 8.0f, 0.0f } : (b3Vec3){ 0.5f, 0.0f, 0.0f }; + jointDef.base.localFrameB.p = (b3Vec3){ -0.5f, 0.0f, 0.0f }; + b3CreateRevoluteJoint( worldId, &jointDef ); + prevId = linkId; + } + + for ( int i = 0; i < 4; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.isBullet = true; + bodyDef.position = (b3Pos){ -30.0f, 1.0f + 2.0f * i, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 200.0f, 0.0f, 0.0f }; + b3BodyId bulletId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef bulletDef = b3DefaultShapeDef(); + bulletDef.enableSensorEvents = true; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f }; + b3CreateSphereShape( bulletId, &bulletDef, &sphere ); + } + + return worldId; +} + +static uint64_t StateHash( b3WorldId worldId ) +{ + return b3HashWorldState( b3GetWorldFromId( worldId ) ); +} + +static int MixedSceneTest( void ) +{ + int workerCounts[] = { 1, 4, 8 }; + for ( int w = 0; w < 3; ++w ) + { + int workerCount = workerCounts[w]; + b3WorldId syncId = CreateMixedScene( workerCount ); + b3WorldId serialId = CreateMixedScene( workerCount ); + b3WorldId pooledId = CreateMixedScene( workerCount ); + + Pool pool; + CreatePool( &pool, 3 ); + SerialHost host = { .order = e_reverse, .honorDeferred = true }; + + int sensorBeginTotal = 0; + int hitTotal = 0; + int maxBatchCount = 0; + float timeStep = 1.0f / 60.0f; + for ( int i = 0; i < 400; ++i ) + { + b3World_Step( syncId, timeStep, 4 ); + ENSURE( SerialPhasedStep( &host, serialId, timeStep, 4 ) == 0 ); + PooledPhasedStep( &pool, pooledId, timeStep, 4 ); + + uint64_t syncHash = StateHash( syncId ); + ENSURE( StateHash( serialId ) == syncHash ); + ENSURE( StateHash( pooledId ) == syncHash ); + + b3SensorEvents syncSensors = b3World_GetSensorEvents( syncId ); + b3SensorEvents serialSensors = b3World_GetSensorEvents( serialId ); + ENSURE( syncSensors.beginCount == serialSensors.beginCount ); + ENSURE( syncSensors.endCount == serialSensors.endCount ); + + b3ContactEvents syncContacts = b3World_GetContactEvents( syncId ); + b3ContactEvents serialContacts = b3World_GetContactEvents( serialId ); + ENSURE( syncContacts.beginCount == serialContacts.beginCount ); + ENSURE( syncContacts.hitCount == serialContacts.hitCount ); + + b3BodyEvents syncBodies = b3World_GetBodyEvents( syncId ); + b3BodyEvents serialBodies = b3World_GetBodyEvents( serialId ); + ENSURE( syncBodies.moveCount == serialBodies.moveCount ); + + sensorBeginTotal += serialSensors.beginCount; + hitTotal += serialContacts.hitCount; + maxBatchCount = host.batchCount > maxBatchCount ? host.batchCount : maxBatchCount; + } + + ENSURE( sensorBeginTotal > 0 ); + ENSURE( hitTotal > 0 ); + ENSURE( maxBatchCount == 7 ); + ENSURE( b3World_GetAwakeBodyCount( serialId ) < 55 + 6 + 4 ); + + ENSURE( b3World_GetAwakeBodyCount( syncId ) == b3World_GetAwakeBodyCount( serialId ) ); + ENSURE( b3World_GetCounters( serialId ).taskCount > 0 ); + + DestroyPool( &pool ); + b3DestroyWorld( syncId ); + b3DestroyWorld( serialId ); + b3DestroyWorld( pooledId ); + } + + return 0; +} + +static int PhasedRecordingTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 1; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3Recording* recording = b3CreateRecording( 0 ); + b3World_StartRecording( worldId, recording ); + + FallingHingeData data = CreateFallingHinges( worldId ); + + SerialHost host = { .order = e_reverse, .honorDeferred = true }; + for ( int i = 0; i < 120; ++i ) + { + ENSURE( SerialPhasedStep( &host, worldId, 1.0f / 60.0f, 4 ) == 0 ); + UpdateFallingHinges( worldId, &data ); + } + + b3World_StopRecording( worldId ); + b3DestroyWorld( worldId ); + DestroyFallingHinges( &data ); + + const uint8_t* bytes = b3Recording_GetData( recording ); + int size = b3Recording_GetSize( recording ); + ENSURE( b3ValidateReplay( bytes, size, 1 ) ); + ENSURE( b3ValidateReplay( bytes, size, 4 ) ); + + b3DestroyRecording( recording ); + return 0; +} + +int StepTest( void ) +{ + RUN_SUBTEST( SerialPullTest ); + RUN_SUBTEST( ReverseOrderTest ); + RUN_SUBTEST( DeferredJoinTest ); + RUN_SUBTEST( PooledPullTest ); + RUN_SUBTEST( EnqueueOnlyTest ); + RUN_SUBTEST( AlternateTest ); + RUN_SUBTEST( EmptyWorldTest ); + RUN_SUBTEST( JoinPlacementTest ); + RUN_SUBTEST( SplitBatchTest ); + RUN_SUBTEST( MixedSceneTest ); + RUN_SUBTEST( PhasedRecordingTest ); + + return 0; +}