diff --git a/benchmark/main.c b/benchmark/main.c index d7216af..81ee8c4 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -86,6 +86,7 @@ int main( int argc, char** argv ) { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, { "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, + { "static_floor", GetStaticFloorCapacity, CreateStaticFloor, NULL, StepStaticFloor, 500 }, //{ "smash", CreateSmash, NULL, 300 }, //{ "spinner", CreateSpinner, StepSpinner, 1400 }, //{ "tumbler", CreateTumbler, NULL, 750 }, diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 1fc9cb7..6e19044 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -1095,6 +1095,39 @@ public: static int benchmarkWasher = SampleManager::Register( "Benchmark", "Washer", BenchmarkWasher::Create ); +class BenchmarkStaticFloor : public Sample +{ +public: + explicit BenchmarkStaticFloor( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 60.0f, 80.0f, b3Vec3_zero ); + EnableGrid( m_scene, false ); + } + + b3Capacity capacity = {}; + GetStaticFloorCapacity( &capacity ); + CreateWorld( &capacity ); + + CreateStaticFloor( m_worldId ); + } + + void Step() override + { + StepStaticFloor( m_worldId, m_stepCount ); + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkStaticFloor( context ); + } +}; + +static int sampleStaticFloor = SampleManager::Register( "Benchmark", "Static Floor", BenchmarkStaticFloor::Create ); + class BenchmarkHull : public Sample { public: diff --git a/shared/benchmarks.c b/shared/benchmarks.c index 2b9871b..e750ad5 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -402,6 +402,115 @@ void StepRain( b3WorldId worldId, int stepCount ) } } +// Static Floor: a huge grid of static box bodies with b3ShapeDef::invokeContactCreation = true, +// plus a small number of dynamic spheres that drop onto it staggered over time. The point of this +// benchmark is to exercise the b3BroadPhase move buffer at steady state when it was once populated +// with ~staticShapeCount entries and now sees only a handful of dynamic moves per step. +#define STATIC_FLOOR_CELL_SIZE 10.0f +#if BENCHMARK_DEBUG +#define STATIC_FLOOR_GRID 32 +#define STATIC_FLOOR_SPHERES 16 +#define STATIC_FLOOR_DROP_INTERVAL 8 +#else +#define STATIC_FLOOR_GRID 1000 +#define STATIC_FLOOR_SPHERES 100 +#define STATIC_FLOOR_DROP_INTERVAL 5 +#endif + +typedef struct StaticFloorData +{ + int spheresDropped; +} StaticFloorData; + +static StaticFloorData g_staticFloorData; + +void GetStaticFloorCapacity( b3Capacity* capacity ) +{ + int floorCount = STATIC_FLOOR_GRID * STATIC_FLOOR_GRID; + capacity->staticShapeCount = floorCount; + capacity->staticBodyCount = floorCount; + capacity->dynamicShapeCount = STATIC_FLOOR_SPHERES; + capacity->dynamicBodyCount = STATIC_FLOOR_SPHERES; + capacity->contactCount = b3MaxInt( 1024, 8 * STATIC_FLOOR_SPHERES ); +} + +void CreateStaticFloor( b3WorldId worldId ) +{ + memset( &g_staticFloorData, 0, sizeof( g_staticFloorData ) ); + + float cell = STATIC_FLOOR_CELL_SIZE; + int gridCount = STATIC_FLOOR_GRID; + float halfSpan = 0.5f * cell * gridCount; + + b3BoxHull box = b3MakeBoxHull( 0.5f * cell, 0.25f, 0.5f * cell ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + // The trigger: every static shape gets buffered into the move set on creation. + shapeDef.invokeContactCreation = true; + + for ( int i = 0; i < gridCount; ++i ) + { + float x = -halfSpan + ( i + 0.5f ) * cell; + for ( int j = 0; j < gridCount; ++j ) + { + float z = -halfSpan + ( j + 0.5f ) * cell; + bodyDef.position = (b3Vec3){ x, 0.0f, z }; + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + } +} + +void StepStaticFloor( b3WorldId worldId, int stepCount ) +{ + if ( g_staticFloorData.spheresDropped >= STATIC_FLOOR_SPHERES ) + { + return; + } + + if ( stepCount == 0 ) + { + return; + } + + if ( ( stepCount % STATIC_FLOOR_DROP_INTERVAL ) != 0 ) + { + return; + } + + // Spread spheres in a coarse grid across the floor so they don't all pile on one box. + int side = 1; + while ( side * side < STATIC_FLOOR_SPHERES ) + { + side += 1; + } + + int idx = g_staticFloorData.spheresDropped; + int gi = idx % side; + int gj = idx / side; + + float halfSpan = 0.5f * STATIC_FLOOR_CELL_SIZE * STATIC_FLOOR_GRID; + // Confine drops to the inner 80% of the floor so spheres can't roll off the edge. + float inset = 0.1f * 2.0f * halfSpan; + float usable = 2.0f * halfSpan - 2.0f * inset; + float x = -halfSpan + inset + ( gi + 0.5f ) * ( usable / side ); + float z = -halfSpan + inset + ( gj + 0.5f ) * ( usable / side ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Vec3){ x, 1.5f, z }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + b3CreateSphereShape( body, &shapeDef, &sphere ); + + g_staticFloorData.spheresDropped += 1; +} + void GetWasherCapacity( b3Capacity* capacity ) { capacity->staticShapeCount = 16; diff --git a/shared/benchmarks.h b/shared/benchmarks.h index 561e683..f25f91a 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -34,6 +34,9 @@ void GetRainCapacity( b3Capacity* capacity ); void CreateRain( b3WorldId worldId ); void StepRain( b3WorldId worldId, int stepCount ); void DestroyRain( void ); +void GetStaticFloorCapacity( b3Capacity* capacity ); +void CreateStaticFloor( b3WorldId worldId ); +void StepStaticFloor( b3WorldId worldId, int stepCount ); void GetWasherCapacity( b3Capacity* capacity ); void CreateWasher( b3WorldId worldId ); void CreateWeldedHulls( b3WorldId worldId ); diff --git a/src/broad_phase.c b/src/broad_phase.c index bd7b855..1797472 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -19,7 +19,9 @@ void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) { _Static_assert( b3_bodyTypeCount == 3, "must be three body types" ); - bp->moveSet = b3CreateSet( 2 * capacity->dynamicShapeCount ); + bp->movedProxies[b3_staticBody] = b3CreateBitSet( b3MaxInt( 16, capacity->staticShapeCount ) ); + bp->movedProxies[b3_kinematicBody] = b3CreateBitSet( 16 ); + bp->movedProxies[b3_dynamicBody] = b3CreateBitSet( b3MaxInt( 16, capacity->dynamicShapeCount ) ); b3Array_Reserve( bp->moveArray, capacity->dynamicShapeCount ); bp->moveResults = NULL; bp->movePairs = NULL; @@ -44,7 +46,10 @@ void b3DestroyBroadPhase( b3BroadPhase* bp ) b3DynamicTree_Destroy( bp->trees + i ); } - b3DestroySet( &bp->moveSet ); + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DestroyBitSet( &bp->movedProxies[i] ); + } b3Array_Destroy( bp->moveArray ); b3DestroySet( &bp->pairSet ); @@ -55,10 +60,14 @@ void b3DestroyBroadPhase( b3BroadPhase* bp ) static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey ) { - bool found = b3RemoveKey( &bp->moveSet, proxyKey + 1 ); + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + b3BitSet* set = &bp->movedProxies[proxyType]; - if ( found ) + if ( b3GetBit( set, proxyId ) ) { + b3ClearBit( set, proxyId ); + // Purge from move buffer. Linear search. // todo if I can iterate the move set then I don't need the moveArray int count = bp->moveArray.count; @@ -88,7 +97,6 @@ int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aab void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ) { - B3_ASSERT( bp->moveArray.count == (int)bp->moveSet.count ); b3UnBufferMove( bp, proxyKey ); b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); @@ -203,22 +211,16 @@ static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) // De-duplication // It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates - // early and in the worker. Most of the time the moveSet contains dynamic and kinematic proxies, but - // sometimes it has static proxies. - - // I had an optimization here to skip checking the move set if this is a query into - // the static tree. The assumption is that the static proxies are never in the move set - // so there is no risk of duplication. However, this is not true with - // b3ShapeDef::invokeContactCreation or when a static shape is modified. - // There can easily be scenarios where the static proxy is in the moveSet but the dynamic proxy is not. - // I could have some flag to indicate that there are any static bodies in the moveSet. + // early and in the worker. Most of the time the movedProxies bit sets contain dynamic and kinematic + // proxies, but sometimes static proxies are in there too (b3ShapeDef::invokeContactCreation or a + // modified static shape), so we always have to check. // Is this proxy also moving? if ( queryProxyType == b3_dynamicBody ) { if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey ) { - bool moved = b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); if ( moved ) { // Both proxies are moving. Avoid duplicate pairs. @@ -229,7 +231,7 @@ static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) else { B3_ASSERT( treeType == b3_dynamicBody ); - bool moved = b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); if ( moved ) { // Both proxies are moving. Avoid duplicate pairs. @@ -340,12 +342,6 @@ static void b3FindPairsTask( int startIndex, int endIndex, int workerIndex, void queryContext.moveResult->pairList = NULL; int proxyKey = bp->moveArray.data[i]; - if ( proxyKey == B3_NULL_INDEX ) - { - // proxy was destroyed after it moved - continue; - } - b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); int proxyId = B3_PROXY_ID( proxyKey ); @@ -403,7 +399,6 @@ void b3UpdateBroadPhasePairs( b3World* world ) b3BroadPhase* bp = &world->broadPhase; int moveCount = bp->moveArray.count; - B3_ASSERT( moveCount == (int)bp->moveSet.count ); if ( moveCount == 0 ) { @@ -476,9 +471,14 @@ void b3UpdateBroadPhasePairs( b3World* world ) } } - // Reset move buffer + // Reset move buffer: clear only the bits that were set this step. + // Invariant: bit set in movedProxies[type] iff proxyKey is present in moveArray. + for ( int i = 0; i < bp->moveArray.count; ++i ) + { + int proxyKey = bp->moveArray.data[i]; + b3ClearBit( &bp->movedProxies[B3_PROXY_TYPE( proxyKey )], B3_PROXY_ID( proxyKey ) ); + } b3Array_Clear( bp->moveArray ); - b3ClearSet( &bp->moveSet ); b3StackFree( alloc, bp->movePairs ); bp->movePairs = NULL; diff --git a/src/broad_phase.h b/src/broad_phase.h index dcd14ad..d233af6 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -3,6 +3,7 @@ #pragma once +#include "bitset.h" #include "container.h" #include "table.h" @@ -27,12 +28,9 @@ typedef struct b3BroadPhase { b3DynamicTree trees[b3_bodyTypeCount]; - // The move set and array are used to track shapes that have moved significantly - // and need a pair query for new contacts. The array has a deterministic order. - // todo perhaps just a move set? - // todo implement a 32bit hash set for faster lookup - // todo moveSet can grow quite large on the first time step and remain large - b3HashSet moveSet; + // Per body-type bit sets indexed by proxyId, marking proxies moved this step. + // Paired with moveArray which preserves deterministic insertion order for pair queries. + b3BitSet movedProxies[b3_bodyTypeCount]; b3ArrayC( int ) moveArray; // These are the results from the pair query and are used to create new contacts @@ -70,10 +68,12 @@ void b3ValidateNoEnlarged( const b3BroadPhase* bp ); // Warning: this must be called in deterministic order static inline void b3BufferMove( b3BroadPhase* bp, int queryProxy ) { - // Adding 1 because 0 is the sentinel - bool alreadyAdded = b3AddKey( &bp->moveSet, queryProxy + 1 ); - if ( alreadyAdded == false ) + b3BodyType proxyType = B3_PROXY_TYPE( queryProxy ); + int proxyId = B3_PROXY_ID( queryProxy ); + b3BitSet* set = &bp->movedProxies[proxyType]; + if ( b3GetBit( set, proxyId ) == false ) { + b3SetBitGrow( set, proxyId ); b3Array_Push( bp->moveArray, queryProxy ); } } diff --git a/src/physics_world.c b/src/physics_world.c index aac4757..fcec31b 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -2115,8 +2115,12 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) fprintf( file, "static tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ) ); fprintf( file, "kinematic tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody ) ); fprintf( file, "dynamic tree: %d\n", b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody ) ); - b3HashSet* moveSet = &world->broadPhase.moveSet; - fprintf( file, "moveSet: %d (%d, %d)\n", b3GetHashSetBytes( moveSet ), moveSet->count, moveSet->capacity ); + int movedBytes = 0; + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + movedBytes += b3GetBitSetBytes( &world->broadPhase.movedProxies[i] ); + } + fprintf( file, "movedProxies: %d\n", movedBytes ); fprintf( file, "moveArray: %d\n", b3Array_ByteCount( world->broadPhase.moveArray ) ); b3HashSet* pairSet = &world->broadPhase.pairSet; fprintf( file, "pairSet: %d (%d, %d)\n", b3GetHashSetBytes( pairSet ), pairSet->count, pairSet->capacity ); diff --git a/src/solver.c b/src/solver.c index 85c2848..7effc4c 100644 --- a/src/solver.c +++ b/src/solver.c @@ -2181,7 +2181,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) B3_ASSERT( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); // all fast bullet shapes should already be in the move buffer - B3_ASSERT( b3ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); + B3_ASSERT( b3GetBit( &broadPhase->movedProxies[b3_dynamicBody], proxyId ) ); b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB );