Pull model task API (#228)

Port the pull model task API from Box2D.

https://github.com/erincatto/box2d/pull/1133

Addresses discussion #150
This commit is contained in:
Erin Catto 2026-11-17 16:42:52 -08:00 • committed by GitHub
commit 1767150df4
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
24 changed files with 2048 additions and 406 deletions

View file

@ -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 );

View file

@ -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 );

View file

@ -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

View file

@ -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 <stddef.h>
// 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 );
}
}

View file

@ -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 );

View file

@ -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" );

View file

@ -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;

View file

@ -7,6 +7,7 @@
#include "box3d/base.h"
#include "box3d/constants.h"
#include "box3d/types.h"
#include <stdbool.h>
#include <stdio.h>
@ -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 );

View file

@ -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 );

View file

@ -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 )

View file

@ -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 );

View file

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

View file

@ -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 <stdbool.h>
#include <stdint.h>
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 )
{

View file

@ -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();