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:
parent
8f3b2e12eb
commit
1767150df4
24 changed files with 2048 additions and 406 deletions
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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" );
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
18
src/sensor.c
18
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 )
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
267
src/solver.c
267
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 );
|
||||
}
|
||||
|
||||
|
|
|
|||
102
src/solver.h
102
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 <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 )
|
||||
{
|
||||
|
|
|
|||
33
src/types.c
33
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();
|
||||
|
|
|
|||
Loading…
Reference in a new issue