diff --git a/benchmark/main.c b/benchmark/main.c index 00747f8..d8ec5ce 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -239,6 +239,9 @@ int main( int argc, char** argv ) { "trees100", NULL, CreateTrees100, DestroyTrees, NULL, 500 }, { "trees50", NULL, CreateTrees50, DestroyTrees, NULL, 500 }, { "trees25", NULL, CreateTrees25, DestroyTrees, NULL, 500 }, + { "trees100hf", NULL, CreateTrees100HeightField, DestroyTrees, NULL, 500 }, + { "trees50hf", NULL, CreateTrees50HeightField, DestroyTrees, NULL, 500 }, + { "trees25hf", NULL, CreateTrees25HeightField, DestroyTrees, NULL, 500 }, { "village", GetVillageCapacity, CreateVillage, DestroyVillage, StepVillage, VILLAGE_STEP_COUNT }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, //{ "smash", CreateSmash, NULL, 300 }, diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 301e41d..fcfcf54 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -83,12 +83,6 @@ B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ); -/// Overlap test for all shapes that *potentially* overlap the provided AABB. This only considers static bodies, -/// so it may be invoked concurrently with b3World_Step. This is not thread-safe with shape creation or destruction. -/// This is a workaround for The Legend of California and may be removed later if the workaround is no longer needed. -B3_API b3TreeStats b3World_OverlapAABB_Static( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, - b3OverlapResultFcn* fcn, void* context ); - /// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative /// to the world origin, which lets the query stay precise far from the world origin. B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, @@ -121,6 +115,26 @@ B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Ve B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); +/// Overlap test for all shapes that *potentially* overlap the provided AABB. This only considers static bodies, +/// so it may be invoked concurrently with b3World_Step. This is not thread-safe with creationg and destruction +/// operations. This is a workaround for The Legend of California and may be removed later if the workaround is no +/// longer needed. This is not part of a strategy to make concurrent queries thread-safe. +B3_API b3TreeStats b3World_OverlapAABB_Static( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ); + +/// Similar for shape overlap. +B3_API b3TreeStats b3World_OverlapShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ); + +/// Similar for cast ray. +B3_API b3TreeStats b3World_CastRay_Static( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); + +/// Similar for cast shape. +B3_API b3TreeStats b3World_CastShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, + void* context ); + /// Cast a capsule mover through the world. This is a special shape cast that handles sliding along /// other shapes while reducing clipping. This is not a good source of information about what the /// mover is touching. Instead use the planes returned by b3World_CollideMover. Ignores back-side diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index d70c28e..5482088 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -693,20 +693,25 @@ public: if ( m_gridSize == 100 ) { - CreateTrees100( m_worldId ); + m_useHeightField ? CreateTrees100HeightField( m_worldId ) : CreateTrees100( m_worldId ); } else if ( m_gridSize == 50 ) { - CreateTrees50( m_worldId ); + m_useHeightField ? CreateTrees50HeightField( m_worldId ) : CreateTrees50( m_worldId ); } else { - CreateTrees25( m_worldId ); + m_useHeightField ? CreateTrees25HeightField( m_worldId ) : CreateTrees25( m_worldId ); } } bool DrawControls() override { + if ( ImGui::Checkbox( "Height Field", &m_useHeightField ) ) + { + Generate(); + } + if ( ImGui::RadioButton( "100cm", &m_gridSize, 100 ) ) { Generate(); @@ -731,6 +736,7 @@ public: } int m_gridSize; + bool m_useHeightField = false; }; static int sampleFallingTrees = RegisterSample( "Benchmark", "Falling Trees", BenchmarkFallingTrees::Create ); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index 402b14a..bed4079 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -676,19 +676,15 @@ void CreateWasher( b3WorldId worldId ) struct { b3MeshData* meshData; + b3HeightFieldData* heightField; } g_treeData; -static void CreateTrees( b3WorldId worldId, int scale ) +static void CreateTrees( b3WorldId worldId, int scale, bool useHeightField ) { memset( &g_treeData, 0, sizeof( g_treeData ) ); // float tilt = 0.15f * B3_PI; float tilt = 0.0f * B3_PI; - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = (b3Pos){ 0.0f, 0.0f, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); - b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); - int xCount = scale * 150; // int zCount = BENCHMARK_DEBUG ? 100 : 400; int zCount = scale * 200; @@ -698,9 +694,59 @@ static void CreateTrees( b3WorldId worldId, int scale ) float rowHz = 0.05f; float columnHz = 0.1f; - g_treeData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Pos){ 0.0f, 0.0f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); + if ( useHeightField ) + { + b3Vec3 corner = { -0.5f * cellWidth * xCount, 0.0f, -0.5f * cellWidth * zCount }; + corner = b3RotateVector( bodyDef.rotation, corner ); + bodyDef.position = (b3Pos){ corner.x, corner.y, corner.z }; + } + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateMeshShape( groundId, &shapeDef, g_treeData.meshData, b3Vec3_one ); + + if ( useHeightField ) + { + int countX = xCount + 1; + int countZ = zCount + 1; + float* heights = malloc( countX * countZ * sizeof( float ) ); + uint8_t* materialIndices = calloc( xCount * zCount, sizeof( uint8_t ) ); + + float omegaX = 2.0f * B3_PI * columnHz * cellWidth; + float omegaZ = 2.0f * B3_PI * rowHz * cellWidth; + for ( int iz = 0; iz < countZ; ++iz ) + { + float sz = b3ComputeCosSin( omegaZ * iz ).sine; + for ( int ix = 0; ix < countX; ++ix ) + { + float sx = b3ComputeCosSin( omegaX * ix ).sine; + heights[iz * countX + ix] = sx * sz; + } + } + + b3HeightFieldDef hfDef = { 0 }; + hfDef.heights = heights; + hfDef.materialIndices = materialIndices; + hfDef.scale = (b3Vec3){ cellWidth, amplitude, cellWidth }; + hfDef.countX = countX; + hfDef.countZ = countZ; + hfDef.globalMinimumHeight = -1.0f; + hfDef.globalMaximumHeight = 1.0f; + hfDef.clockwiseWinding = false; + + g_treeData.heightField = b3CreateHeightField( &hfDef ); + free( heights ); + free( materialIndices ); + + b3CreateHeightFieldShape( groundId, &shapeDef, g_treeData.heightField ); + } + else + { + g_treeData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + b3CreateMeshShape( groundId, &shapeDef, g_treeData.meshData, b3Vec3_one ); + } bodyDef.type = b3_dynamicBody; bodyDef.sleepThreshold = 0.2f; @@ -767,22 +813,46 @@ static void CreateTrees( b3WorldId worldId, int scale ) void CreateTrees25( b3WorldId worldId ) { - CreateTrees( worldId, 4 ); + CreateTrees( worldId, 4, false ); } void CreateTrees50( b3WorldId worldId ) { - CreateTrees( worldId, 2 ); + CreateTrees( worldId, 2, false ); } void CreateTrees100( b3WorldId worldId ) { - CreateTrees( worldId, 1 ); + CreateTrees( worldId, 1, false ); +} + +void CreateTrees25HeightField( b3WorldId worldId ) +{ + CreateTrees( worldId, 4, true ); +} + +void CreateTrees50HeightField( b3WorldId worldId ) +{ + CreateTrees( worldId, 2, true ); +} + +void CreateTrees100HeightField( b3WorldId worldId ) +{ + CreateTrees( worldId, 1, true ); } void DestroyTrees( void ) { - b3DestroyMesh( g_treeData.meshData ); + if ( g_treeData.meshData != NULL ) + { + b3DestroyMesh( g_treeData.meshData ); + } + + if ( g_treeData.heightField != NULL ) + { + b3DestroyHeightField( g_treeData.heightField ); + } + memset( &g_treeData, 0, sizeof( g_treeData ) ); } diff --git a/shared/benchmarks.h b/shared/benchmarks.h index 9218aac..29f1530 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -24,6 +24,9 @@ void ResetGroundShapeId( void ); void CreateTrees25( b3WorldId worldId ); void CreateTrees50( b3WorldId worldId ); void CreateTrees100( b3WorldId worldId ); +void CreateTrees25HeightField( b3WorldId worldId ); +void CreateTrees50HeightField( b3WorldId worldId ); +void CreateTrees100HeightField( b3WorldId worldId ); void DestroyTrees( void ); void CreateMeshDropBenchmark( b3WorldId worldId ); void DestroyMeshDropBenchmark( void ); diff --git a/src/body.c b/src/body.c index 70bbc28..b6049da 100644 --- a/src/body.c +++ b/src/body.c @@ -33,7 +33,7 @@ b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ) b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body ) { - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); return bodySim->transform; } @@ -53,7 +53,7 @@ b3BodyId b3MakeBodyId( b3World* world, int bodyId ) b3BodySim* b3GetBodySim( b3World* world, b3Body* body ) { - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); return bodySim; } @@ -62,7 +62,7 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) { if ( body->setIndex == b3_awakeSet ) { - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); return b3Array_Get( set->bodyStates, body->localIndex ); } @@ -217,20 +217,20 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) { // new set for a sleeping body in its own island setId = b3AllocId( &world->solverSetIdPool ); - if ( setId == world->solverSets.count ) + if ( setId == b3GetSolverSetCount( world ) ) { // Create a zero initialized solver set. All sub-arrays are also zero initialized. - b3Array_Push( world->solverSets, (b3SolverSet){ 0 } ); + b3Array_Push( world->sleepingSets, (b3SolverSet){ 0 } ); } else { - B3_ASSERT( world->solverSets.data[setId].setIndex == B3_NULL_INDEX ); + B3_ASSERT( b3GetSolverSet( world, setId )->setIndex == B3_NULL_INDEX ); } - world->solverSets.data[setId].setIndex = setId; + b3GetSolverSet( world, setId )->setIndex = setId; } - B3_ASSERT( 0 <= setId && setId < world->solverSets.count ); + B3_ASSERT( 0 <= setId && setId < b3GetSolverSetCount( world ) ); int bodyId = b3AllocId( &world->bodyIdPool ); @@ -242,7 +242,7 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) lockFlags |= def->motionLocks.angularY ? b3_lockAngularY : 0; lockFlags |= def->motionLocks.angularZ ? b3_lockAngularZ : 0; - b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + b3SolverSet* set = b3GetSolverSet( world, setId ); b3BodySim* bodySim = b3Array_Emplace( set->bodySims ); *bodySim = (b3BodySim){ 0 }; bodySim->transform.p = def->position; @@ -423,7 +423,7 @@ void b3DestroyBody( b3BodyId bodyId ) b3RemoveBodyFromIsland( world, body ); // Remove body sim from solver set that owns it - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); int movedIndex = b3Array_RemoveSwap( set->bodySims, body->localIndex ); if ( movedIndex != B3_NULL_INDEX ) { @@ -1364,7 +1364,7 @@ b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ) return b3Vec3_zero; } - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); b3Vec3 r = b3RotateVector( bodySim->transform.q, b3Sub( localPoint, bodySim->localCenter ) ); @@ -1382,7 +1382,7 @@ b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint ) return b3Vec3_zero; } - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); b3Vec3 r = b3SubPos( worldPoint, bodySim->center ); @@ -1476,7 +1476,7 @@ void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bo if ( body->setIndex == b3_awakeSet ) { int localIndex = body->localIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); @@ -1511,7 +1511,7 @@ void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wa if ( body->setIndex == b3_awakeSet ) { int localIndex = body->localIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); @@ -1546,7 +1546,7 @@ void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ) if ( body->setIndex == b3_awakeSet ) { int localIndex = body->localIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); @@ -1659,7 +1659,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) b3WakeBody( world, body ); // Stage 4: move joints to temporary storage - b3SolverSet* staticSet = b3Array_Get( world->solverSets, b3_staticSet ); + b3SolverSet* staticSet = b3GetSolverSet( world, b3_staticSet ); int jointKey = body->headJointKey; while ( jointKey != B3_NULL_INDEX ) @@ -1689,7 +1689,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) // It is necessary to transfer all joints to the static set // so they can be added to the constraint graph below and acquire consistent colors. - b3SolverSet* jointSourceSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3SolverSet* jointSourceSet = b3GetSolverSet( world, joint->setIndex ); b3TransferJoint( world, staticSet, jointSourceSet, joint ); } @@ -1705,8 +1705,8 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) body->flags &= ~b3_dynamicFlag; } - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); - b3SolverSet* sourceSet = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); + b3SolverSet* sourceSet = b3GetSolverSet( world, body->setIndex ); b3SolverSet* targetSet = type == b3_staticBody ? staticSet : awakeSet; // Transfer body @@ -2232,10 +2232,10 @@ void b3Body_Disable( b3BodyId bodyId ) b3DestroyBodyContacts( world, body, wakeBodies ); // The current solver set of the body - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); // Disabled bodies and connected joints are moved to the disabled set - b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* disabledSet = b3GetSolverSet( world, b3_disabledSet ); // Unlink joints and transfer them to the disabled set int jointKey = body->headJointKey; @@ -2259,7 +2259,7 @@ void b3Body_Disable( b3BodyId bodyId ) b3UnlinkJoint( world, joint ); // Transfer joint to disabled set - b3SolverSet* jointSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3SolverSet* jointSet = b3GetSolverSet( world, joint->setIndex ); b3TransferJoint( world, disabledSet, jointSet, joint ); } @@ -2300,9 +2300,9 @@ void b3Body_Enable( b3BodyId bodyId ) return; } - b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* disabledSet = b3GetSolverSet( world, b3_disabledSet ); int setId = body->type == b3_staticBody ? b3_staticSet : b3_awakeSet; - b3SolverSet* targetSet = b3Array_Get( world->solverSets, setId ); + b3SolverSet* targetSet = b3GetSolverSet( world, setId ); b3TransferBody( world, targetSet, disabledSet, body ); @@ -2363,7 +2363,7 @@ void b3Body_Enable( b3BodyId bodyId ) jointSetId = bodyA->setIndex; } - b3SolverSet* jointSet = b3Array_Get( world->solverSets, jointSetId ); + b3SolverSet* jointSet = b3GetSolverSet( world, jointSetId ); b3TransferJoint( world, jointSet, disabledSet, joint ); // Now that the joint is in the correct set, I can link the joint in the island. diff --git a/src/contact.c b/src/contact.c index e5c9a60..134e7b2 100644 --- a/src/contact.c +++ b/src/contact.c @@ -183,7 +183,7 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil setIndex = b3_disabledSet; } - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetFixedSolverSet( world, setIndex ); // Create contact key and contact int contactId = b3AllocId( &world->contactIdPool ); @@ -453,7 +453,7 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) { // contact is non-touching or is sleeping or is a sensor B3_ASSERT( contact->setIndex != b3_awakeSet || ( contact->flags & b3_contactTouchingFlag ) == 0 ); - b3SolverSet* set = b3Array_Get( world->solverSets, contact->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, contact->setIndex ); int localIndex = contact->localIndex; int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); diff --git a/src/contact_solver.c b/src/contact_solver.c index ad41b41..d1714fe 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -298,7 +298,7 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) { b3World* world = context->world; b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); b3BodyState* states = awakeSet->bodyStates.data; b3ContactConstraint* constraints = color->contactConstraints; diff --git a/src/distance_joint.c b/src/distance_joint.c index 9185010..b89c670 100644 --- a/src/distance_joint.c +++ b/src/distance_joint.c @@ -256,8 +256,8 @@ void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ) B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 5e43cf4..b1f7ca4 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1082,11 +1082,14 @@ B3_FORCE_INLINE b3TreeStats b3QueryClosestTreeView( const b3TreeView* view, b3Ve // before either is followed, then the survivors are ordered so the closer one narrows first. B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context, - bool hasProxies ) + bool hasProxies, bool relaxedFlags ) { b3TreeStats result = { 0 }; - if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) ) + const b3TreeNode* nodes = view->nodes; + + uint32_t rootFlags = b3LoadNodeFlags( nodes + B3_ROOT_NODE, relaxedFlags ); + if ( rootFlags == B3_EMPTY_NODE ) { return result; } @@ -1105,11 +1108,9 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; b3AABBV boxv = b3LoadAABBV( &segmentAABB ); - const b3TreeNode* nodes = view->nodes; - int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = b3GetRootPair( nodes ); + stack[stackCount++] = ( rootFlags & B3_LEAF_NODE ) ? B3_ROOT_NODE : (int)( rootFlags & B3_NODE_INDEX_MASK ); b3RayCastInput subInput = *input; @@ -1120,6 +1121,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R const b3TreeNode* hit[2]; bool isLeaf[2]; + int index[2]; int hitCount = 0; for ( int i = 0; i < 2; ++i ) { @@ -1138,7 +1140,9 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R continue; } - isLeaf[hitCount] = b3IsLeaf( node ); + uint32_t flags = b3LoadNodeFlags( node, relaxedFlags ); + isLeaf[hitCount] = ( flags & B3_LEAF_NODE ) != 0; + index[hitCount] = (int)( flags & B3_NODE_INDEX_MASK ); hit[hitCount] = node; hitCount += 1; } @@ -1154,6 +1158,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R if ( d1 < d2 ) { B3_SWAP( hit[0], hit[1] ); + B3_SWAP( index[0], index[1] ); } } @@ -1161,7 +1166,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R { if ( isLeaf[i] ) { - int proxyId = b3GetProxyId( hit[i] ); + int proxyId = index[i]; uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex; if ( hasProxies ) { @@ -1200,7 +1205,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R { if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = b3GetLeftChild( hit[i] ); + stack[stackCount++] = index[i]; } else { @@ -1216,11 +1221,14 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R // Follows structure of ray cast with small tweaks to handle a swept box. B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context, - bool hasProxies ) + bool hasProxies, bool relaxedFlags ) { b3TreeStats result = { 0 }; - if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) ) + const b3TreeNode* nodes = view->nodes; + + uint32_t rootFlags = b3LoadNodeFlags( nodes + B3_ROOT_NODE, relaxedFlags ); + if ( rootFlags == B3_EMPTY_NODE ) { return result; } @@ -1247,11 +1255,9 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B }; b3AABBV boxv = b3LoadAABBV( &totalAABB ); - const b3TreeNode* nodes = view->nodes; - int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = b3GetRootPair( nodes ); + stack[stackCount++] = ( rootFlags & B3_LEAF_NODE ) ? B3_ROOT_NODE : (int)( rootFlags & B3_NODE_INDEX_MASK ); b3BoxCastInput subInput = *input; @@ -1262,6 +1268,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B const b3TreeNode* hit[2]; bool isLeaf[2]; + int index[2]; int hitCount = 0; for ( int i = 0; i < 2; ++i ) { @@ -1281,7 +1288,9 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B continue; } - isLeaf[hitCount] = b3IsLeaf( node ); + uint32_t flags = b3LoadNodeFlags( node, relaxedFlags ); + isLeaf[hitCount] = ( flags & B3_LEAF_NODE ) != 0; + index[hitCount] = (int)( flags & B3_NODE_INDEX_MASK ); hit[hitCount] = node; hitCount += 1; } @@ -1297,6 +1306,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B if ( d1 < d2 ) { B3_SWAP( hit[0], hit[1] ); + B3_SWAP( index[0], index[1] ); } } @@ -1304,7 +1314,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B { if ( isLeaf[i] ) { - int proxyId = b3GetProxyId( hit[i] ); + int proxyId = index[i]; uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex; if ( hasProxies ) { @@ -1340,7 +1350,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B { if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = b3GetLeftChild( hit[i] ); + stack[stackCount++] = index[i]; } else { @@ -1353,8 +1363,9 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B return result; } -b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, - b3TreeQueryCallbackFcn* callback, void* context ) +B3_FORCE_INLINE b3TreeStats b3QueryDynamicTree( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, + bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context, + bool relaxedFlags ) { if ( tree->proxyCount == 0 ) { @@ -1362,19 +1373,19 @@ b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_ } b3TreeView view = b3MakeTreeView( tree ); - return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true, false ); + return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true, relaxedFlags ); +} + +b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ) +{ + return b3QueryDynamicTree( tree, aabb, maskBits, requireAllBits, callback, context, false ); } b3TreeStats b3DynamicTree_QueryRelaxed( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ) { - if ( tree->proxyCount == 0 ) - { - return B3_LITERAL( b3TreeStats ){ 0 }; - } - - b3TreeView view = b3MakeTreeView( tree ); - return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true, true ); + return b3QueryDynamicTree( tree, aabb, maskBits, requireAllBits, callback, context, true ); } b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, @@ -1389,8 +1400,9 @@ b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, return b3QueryClosestTreeView( &view, point, maskBits, requireAllBits, callback, context, minDistanceSqr, true ); } -b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, - b3TreeRayCastCallbackFcn* callback, void* context ) +B3_FORCE_INLINE b3TreeStats b3RayCastDynamicTree( const b3DynamicTree* tree, const b3RayCastInput* input, + uint64_t maskBits, bool requireAllBits, + b3TreeRayCastCallbackFcn* callback, void* context, bool relaxedFlags ) { if ( tree->proxyCount == 0 ) { @@ -1398,19 +1410,44 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp } b3TreeView view = b3MakeTreeView( tree ); - return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true ); + return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, relaxedFlags ); +} + +b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ) +{ + return b3RayCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, false ); +} + +b3TreeStats b3DynamicTree_RayCastRelaxed( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ) +{ + return b3RayCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, true ); +} + +B3_FORCE_INLINE b3TreeStats b3BoxCastDynamicTree( const b3DynamicTree* tree, const b3BoxCastInput* input, + uint64_t maskBits, bool requireAllBits, + b3TreeBoxCastCallbackFcn* callback, void* context, bool relaxedFlags ) +{ + if ( tree->proxyCount == 0 ) + { + return B3_LITERAL( b3TreeStats ){ 0 }; + } + + b3TreeView view = b3MakeTreeView( tree ); + return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, relaxedFlags ); } b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ) { - if ( tree->proxyCount == 0 ) - { - return B3_LITERAL( b3TreeStats ){ 0 }; - } + return b3BoxCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, false ); +} - b3TreeView view = b3MakeTreeView( tree ); - return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true ); +b3TreeStats b3DynamicTree_BoxCastRelaxed( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ) +{ + return b3BoxCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, true ); } b3TreeStats b3TreeView_Query( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits, @@ -1440,10 +1477,10 @@ b3TreeStats b3TreeView_RayCast( const b3TreeView* view, const b3RayCastInput* in { if ( view->proxies == NULL ) { - return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, false ); + return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, false, false ); } - return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, true ); + return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, true, false ); } b3TreeStats b3TreeView_BoxCast( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, @@ -1451,10 +1488,10 @@ b3TreeStats b3TreeView_BoxCast( const b3TreeView* view, const b3BoxCastInput* in { if ( view->proxies == NULL ) { - return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, false ); + return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, false, false ); } - return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, true ); + return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, true, false ); } // Median split == 0, Surface area heuristic == 1 diff --git a/src/dynamic_tree.h b/src/dynamic_tree.h index 773c445..906c59b 100644 --- a/src/dynamic_tree.h +++ b/src/dynamic_tree.h @@ -111,5 +111,9 @@ void b3DynamicTree_MarkProxyMoved( b3DynamicTree* tree, int proxyId, b3AABB aabb void b3DynamicTree_ClearMoved( b3DynamicTree* tree ); b3TreeStats b3DynamicTree_QueryRelaxed( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ); +b3TreeStats b3DynamicTree_RayCastRelaxed( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ); +b3TreeStats b3DynamicTree_BoxCastRelaxed( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ); int b3DynamicTree_GatherMovedProxies( const b3DynamicTree* tree, int* proxyIds ); void b3DynamicTree_Refit( b3DynamicTree* tree ); diff --git a/src/island.c b/src/island.c index 6a17909..111b304 100644 --- a/src/island.c +++ b/src/island.c @@ -29,7 +29,7 @@ b3Island* b3CreateIsland( b3World* world, int setIndex ) B3_ASSERT( world->islands.data[islandId].setIndex == B3_NULL_INDEX ); } - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); b3Island* island = b3Array_Get( world->islands, islandId ); island->setIndex = setIndex; @@ -55,7 +55,7 @@ void b3DestroyIsland( b3World* world, int islandId ) // assume island is empty b3Island* island = b3Array_Get( world->islands, islandId ); - b3SolverSet* set = b3Array_Get( world->solverSets, island->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, island->setIndex ); { int localIndex = island->localIndex; int lastIndex = set->islandSims.count - 1; diff --git a/src/joint.c b/src/joint.c index fa01bb5..4250ace 100644 --- a/src/joint.c +++ b/src/joint.c @@ -149,7 +149,7 @@ b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ) return b3Array_Get( color->jointSims, joint->localIndex ); } - b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, joint->setIndex ); return b3Array_Get( set->jointSims, joint->localIndex ); } @@ -287,7 +287,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) { // if either body is disabled, create in disabled set - b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_disabledSet ); joint->setIndex = b3_disabledSet; joint->localIndex = set->jointSims.count; @@ -301,7 +301,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) { // joint is not attached to a dynamic body - b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_staticSet ); joint->setIndex = b3_staticSet; joint->localIndex = set->jointSims.count; @@ -337,7 +337,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint // joint should go into the sleeping set (not static set) int setIndex = maxSetIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); joint->setIndex = setIndex; joint->localIndex = set->jointSims.count; @@ -358,7 +358,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint // fix potentially invalid set index setIndex = bodyA->setIndex; - b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* mergedSet = b3GetSolverSet( world, setIndex ); // Careful! The joint sim pointer was orphaned by the set merge. jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); @@ -888,7 +888,7 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) } else { - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); if ( movedIndex != B3_NULL_INDEX ) { diff --git a/src/motor_joint.c b/src/motor_joint.c index 9487d0c..6210d81 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -193,8 +193,8 @@ void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ) B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/mover_joint.c b/src/mover_joint.c index 51b0808..ac91319 100644 --- a/src/mover_joint.c +++ b/src/mover_joint.c @@ -65,8 +65,8 @@ void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context ) B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/parallel_joint.c b/src/parallel_joint.c index 7390a21..e476db1 100644 --- a/src/parallel_joint.c +++ b/src/parallel_joint.c @@ -81,8 +81,8 @@ void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/physics_world.c b/src/physics_world.c index bddd58a..7216420 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -264,7 +264,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) int bodyCapacity = b3MaxInt( 16, def->capacity.staticBodyCount + def->capacity.dynamicBodyCount ); b3Array_Reserve( world->bodies, bodyCapacity ); - b3Array_Reserve( world->solverSets, 8 ); + b3Array_Reserve( world->sleepingSets, 8 ); // add empty static, active, and disabled body sets world->solverSetIdPool = b3CreateIdPool(); @@ -272,22 +272,22 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) // static set set.setIndex = b3AllocId( &world->solverSetIdPool ); - b3Array_Push( world->solverSets, set ); - b3Array_Reserve( world->solverSets.data[b3_staticSet].bodySims, b3MaxInt( 16, def->capacity.staticBodyCount ) ); - B3_ASSERT( world->solverSets.data[b3_staticSet].setIndex == b3_staticSet ); + world->fixedSets[set.setIndex] = set; + b3Array_Reserve( b3GetSolverSet( world, b3_staticSet )->bodySims, b3MaxInt( 16, def->capacity.staticBodyCount ) ); + B3_ASSERT( b3GetSolverSet( world, b3_staticSet )->setIndex == b3_staticSet ); // disabled set set.setIndex = b3AllocId( &world->solverSetIdPool ); - b3Array_Push( world->solverSets, set ); - B3_ASSERT( world->solverSets.data[b3_disabledSet].setIndex == b3_disabledSet ); + world->fixedSets[set.setIndex] = set; + B3_ASSERT( b3GetSolverSet( world, b3_disabledSet )->setIndex == b3_disabledSet ); // awake set set.setIndex = b3AllocId( &world->solverSetIdPool ); - b3Array_Push( world->solverSets, set ); - b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodySims, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); - b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodyStates, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); - b3Array_Reserve( world->solverSets.data[b3_awakeSet].contactIndices, b3MaxInt( 16, def->capacity.contactCount ) ); - B3_ASSERT( world->solverSets.data[b3_awakeSet].setIndex == b3_awakeSet ); + world->fixedSets[set.setIndex] = set; + b3Array_Reserve( b3GetSolverSet( world, b3_awakeSet )->bodySims, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( b3GetSolverSet( world, b3_awakeSet )->bodyStates, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( b3GetSolverSet( world, b3_awakeSet )->contactIndices, b3MaxInt( 16, def->capacity.contactCount ) ); + B3_ASSERT( b3GetSolverSet( world, b3_awakeSet )->setIndex == b3_awakeSet ); world->shapeIdPool = b3CreateIdPool(); @@ -502,17 +502,17 @@ void b3DestroyWorld( b3WorldId worldId ) b3Array_Destroy( world->islands ); // Destroy solver sets - int setCapacity = world->solverSets.count; + int setCapacity = b3GetSolverSetCount( world ); for ( int i = 0; i < setCapacity; ++i ) { - b3SolverSet* set = world->solverSets.data + i; + b3SolverSet* set = b3GetSolverSet( world, i ); if ( set->setIndex != B3_NULL_INDEX ) { b3DestroySolverSet( world, i ); } } - b3Array_Destroy( world->solverSets ); + b3Array_Destroy( world->sleepingSets ); b3DestroyGraph( &world->constraintGraph ); b3DestroyBroadPhase( &world->broadPhase ); @@ -577,7 +577,7 @@ static inline b3BodySim* b3ResolveContactBodySim( b3World* world, b3BodySim* awa } b3Body* body = b3Array_Get( world->bodies, bodyId ); - b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* set = b3GetSolverSet( world, body->setIndex ); return b3Array_Get( set->bodySims, body->localIndex ); } @@ -593,8 +593,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3Contact* contacts = world->contacts.data; b3Shape* shapes = world->shapes.data; const b3AABB* fatAABBs = world->fatAABBs.data; - b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data; - b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data; + b3BodySim* awakeSims = b3GetSolverSet( world, b3_awakeSet )->bodySims.data; + b3BodySim* staticSims = b3GetSolverSet( world, b3_staticSet )->bodySims.data; B3_ASSERT( startIndex < endIndex ); @@ -799,7 +799,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) { B3_ASSERT( contact->setIndex == b3_awakeSet ); - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); contact->colorIndex = B3_NULL_INDEX; contact->localIndex = set->contactIndices.count; b3Array_Push( set->contactIndices, contact->contactId ); @@ -807,7 +807,7 @@ static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localIndex ) { - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex ); if ( movedIndex != B3_NULL_INDEX ) { @@ -838,7 +838,7 @@ static void b3Collide( b3StepContext* context ) touchingCount += graphColors[i].convexContacts.count + graphColors[i].contacts.count; } - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); int nonTouchingCount = awakeSet->contactIndices.count; int contactCount = touchingCount + nonTouchingCount; @@ -1083,7 +1083,7 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); - int staticBodyCount = world->solverSets.data[b3_staticSet].bodySims.count; + int staticBodyCount = b3GetSolverSet( world, b3_staticSet )->bodySims.count; c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); // this includes kinematic bodies @@ -1101,7 +1101,7 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) world->profile.pairs = b3GetMilliseconds( pairTicks ); } - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); b3StepContext context = { 0 }; context.world = world; @@ -1997,10 +1997,10 @@ void b3World_EnableSleeping( b3WorldId worldId, bool flag ) if ( flag == false ) { - int setCount = world->solverSets.count; + int setCount = b3GetSolverSetCount( world ); for ( int i = b3_firstSleepingSet; i < setCount; ++i ) { - b3SolverSet* set = b3Array_Get( world->solverSets, i ); + b3SolverSet* set = b3GetSolverSet( world, i ); if ( set->bodySims.count > 0 ) { b3WakeSolverSet( world, i ); @@ -2052,7 +2052,7 @@ int b3World_GetAwakeBodyCount( b3WorldId worldId ) { return 0; } - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); return awakeSet->bodySims.count; } @@ -2382,7 +2382,7 @@ static uint64_t b3GetWorldByteCount( b3World* world, bool log ) // world arrays int bodyArrayBytes = b3Array_ByteCount( world->bodies ); - int solverSetArrayBytes = b3Array_ByteCount( world->solverSets ); + int solverSetArrayBytes = b3Array_ByteCount( world->sleepingSets ); int jointArrayBytes = b3Array_ByteCount( world->joints ); int contactArrayBytes = b3Array_ByteCount( world->contacts ); int islandArrayBytes = b3Array_ByteCount( world->islands ); @@ -2482,10 +2482,10 @@ static uint64_t b3GetWorldByteCount( b3World* world, bool log ) int jointSimCapacity = 0; int contactIndexCapacity = 0; int islandSimCapacity = 0; - int solverSetCapacity = world->solverSets.count; + int solverSetCapacity = b3GetSolverSetCount( world ); for ( int i = 0; i < solverSetCapacity; ++i ) { - b3SolverSet* set = world->solverSets.data + i; + b3SolverSet* set = b3GetSolverSet( world, i ); if ( set->setIndex == B3_NULL_INDEX ) { continue; @@ -2657,7 +2657,7 @@ b3Counters b3World_GetCounters( b3WorldId worldId ) s.colorCounts[i] = colorContactCount + color->jointSims.count; s.awakeContactCount += colorContactCount; } - s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; + s.awakeContactCount += b3GetSolverSet( world, b3_awakeSet )->contactIndices.count; s.recycledContactCount = 0; s.arenaCapacity = 0; @@ -2865,6 +2865,21 @@ b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3Shape return treeStats; } +b3TreeStats b3World_OverlapShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin ); + WorldOverlapContext worldContext = { + world, fcn, filter, *proxy, origin, context, + }; + + return b3DynamicTree_QueryRelaxed( world->broadPhase.trees + b3_staticBody, aabb, filter.maskBits, false, + b3TreeOverlapCallback, &worldContext ); +} + typedef struct WorldMoverContext { b3World* world; @@ -3080,6 +3095,20 @@ b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation return treeStats; } +b3TreeStats b3World_CastRay_Static( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context }; + + return b3DynamicTree_RayCastRelaxed( world->broadPhase.trees + b3_staticBody, &input, filter.maskBits, false, + RayCastCallback, &worldContext ); +} + // This callback finds the closest hit. This is the most common callback used in games. static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex, int childIndex, void* context ) @@ -3293,6 +3322,32 @@ b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapePro return treeStats; } +b3TreeStats b3World_CastShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + WorldShapeCastContext worldContext = { 0 }; + worldContext.world = world; + worldContext.fcn = fcn; + worldContext.filter = filter; + worldContext.fraction = 1.0f; + worldContext.origin = origin; + worldContext.input.proxy = *proxy; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = false; + worldContext.userContext = context; + + b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius ); + b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f }; + + return b3DynamicTree_BoxCastRelaxed( world->broadPhase.trees + b3_staticBody, &treeInput, filter.maskBits, false, + b3ShapeCastCallback, &worldContext ); +} + typedef struct WorldMoverCastContext { b3World* world; @@ -3543,7 +3598,7 @@ static bool ExplosionCallback( int proxyId, uint64_t userData, void* context ) b3Vec3 impulse = b3MulSV( magnitude, b3RotateVector( xf.q, direction ) ); int localIndex = body->localIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* set = b3GetSolverSet( world, b3_awakeSet ); b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); @@ -3711,7 +3766,7 @@ void b3ValidateSolverSets( b3World* world ) B3_ASSERT( b3GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); B3_ASSERT( b3GetIdCapacity( &world->jointIdPool ) == world->joints.count ); B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count ); - B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == b3GetSolverSetCount( world ) ); // Ensure the encoded body sim indices are correct. for ( int contactIndex = 0; contactIndex < world->contacts.count; ++contactIndex ) @@ -3735,10 +3790,10 @@ void b3ValidateSolverSets( b3World* world ) int totalIslandCount = 0; // Validate all solver sets - int setCount = world->solverSets.count; + int setCount = b3GetSolverSetCount( world ); for ( int setIndex = 0; setIndex < setCount; ++setIndex ) { - b3SolverSet* set = world->solverSets.data + setIndex; + b3SolverSet* set = b3GetSolverSet( world, setIndex ); if ( set->setIndex != B3_NULL_INDEX ) { activeSetCount += 1; @@ -4111,7 +4166,7 @@ void b3ValidateContacts( b3World* world ) bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; int setId = contact->setIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + b3SolverSet* set = b3GetSolverSet( world, setId ); if ( setId == b3_awakeSet ) { diff --git a/src/physics_world.h b/src/physics_world.h index 1436ee4..2586922 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -147,10 +147,12 @@ typedef struct b3World // Provides free list for solver sets. b3IdPool solverSetIdPool; - // Solvers sets allow sims to be stored in contiguous arrays. The first - // set is all static sims. The second set is active sims. The third set is disabled - // sims. The remaining sets are sleeping islands. - b3Array( b3SolverSet ) solverSets; + // Solvers sets allow body sims to be stored in contiguous arrays. The first holds static bodies. The second set + // holds disabled bodies. The third holds awake bodies. A growable array holds the sleeping islands. + // This is split up so that the static set can be accessed without being remapped during a static query while + // the physics step is concurrently putting an island to sleep (race condition). + b3SolverSet fixedSets[b3_firstSleepingSet]; + b3Array( b3SolverSet ) sleepingSets; // Used to create stable ids for joints b3IdPool jointIdPool; @@ -299,6 +301,28 @@ typedef struct b3World bool inUse; } b3World; +static inline b3SolverSet* b3GetSolverSet( b3World* world, int setIndex ) +{ + if ( setIndex < b3_firstSleepingSet ) + { + B3_ASSERT( 0 <= setIndex ); + return world->fixedSets + setIndex; + } + + return b3Array_Get( world->sleepingSets, setIndex - b3_firstSleepingSet ); +} + +static inline b3SolverSet* b3GetFixedSolverSet( b3World* world, int setIndex ) +{ + B3_ASSERT( 0 <= setIndex && setIndex < b3_firstSleepingSet ); + return world->fixedSets + setIndex; +} + +static inline int b3GetSolverSetCount( const b3World* world ) +{ + return b3_firstSleepingSet + world->sleepingSets.count; +} + b3World* b3GetUnlockedWorldFromId( b3WorldId id ); b3World* b3GetWorldFromId( b3WorldId id ); @@ -360,4 +384,3 @@ static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int c b3FreeElement( allocator, manifolds ); b3UnlockMutex( world->manifoldAllocatorMutex ); } - diff --git a/src/pogo_joint.c b/src/pogo_joint.c index db7bffc..cd51be6 100644 --- a/src/pogo_joint.c +++ b/src/pogo_joint.c @@ -184,8 +184,8 @@ void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context ) B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index b380d37..3950e5f 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -308,8 +308,8 @@ void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 5f8b6da..899d2d8 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -267,8 +267,8 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/solver.c b/src/solver.c index 236c848..5a08eb0 100644 --- a/src/solver.c +++ b/src/solver.c @@ -67,7 +67,7 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte { b3TracyCZoneNC( integrate_velocity, "IntVel", b3_colorDeepPink, true ); - B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + B3_VALIDATE( block.startIndex + block.count <= b3GetSolverSet( context->world, b3_awakeSet )->bodyStates.count ); b3BodyState* states = context->states; b3BodySim* sims = context->sims; @@ -174,7 +174,7 @@ static void b3IntegratePositionsTask( b3SolverBlock block, b3StepContext* contex { b3TracyCZoneNC( integrate_positions, "IntPos", b3_colorDarkSeaGreen, true ); - B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + B3_VALIDATE( block.startIndex + block.count <= b3GetSolverSet( context->world, b3_awakeSet )->bodyStates.count ); b3BodyState* states = context->states; float h = context->h; @@ -491,7 +491,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* { b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true ); - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex ); B3_VALIDATE( fastBodySim->flags & b3_isFast ); @@ -1457,7 +1457,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) // Only count steps that advance the simulation world->stepIndex += 1; - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); int awakeBodyCount = awakeSet->bodySims.count; if ( awakeBodyCount == 0 ) { diff --git a/src/solver_set.c b/src/solver_set.c index 53b75dd..ff0ec53 100644 --- a/src/solver_set.c +++ b/src/solver_set.c @@ -14,7 +14,7 @@ void b3DestroySolverSet( b3World* world, int setIndex ) { - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); b3Array_Destroy( set->bodySims ); b3Array_Destroy( set->bodyStates ); @@ -36,9 +36,9 @@ void b3DestroySolverSet( b3World* world, int setIndex ) void b3WakeSolverSet( b3World* world, int setIndex ) { B3_ASSERT( setIndex >= b3_firstSleepingSet ); - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); - b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* set = b3GetSolverSet( world, setIndex ); + b3SolverSet* awakeSet = b3GetSolverSet( world, b3_awakeSet ); + b3SolverSet* disabledSet = b3GetSolverSet( world, b3_disabledSet ); b3Body* bodies = world->bodies.data; @@ -193,19 +193,18 @@ void b3TrySleepIsland( b3World* world, int islandId ) // - remove old island // - fix island int sleepSetId = b3AllocId( &world->solverSetIdPool ); - if ( sleepSetId == world->solverSets.count ) + if ( sleepSetId == b3GetSolverSetCount( world ) ) { b3SolverSet set = { 0 }; set.setIndex = B3_NULL_INDEX; - b3Array_Push( world->solverSets, set ); + b3Array_Push( world->sleepingSets, set ); } - b3SolverSet* sleepSet = b3Array_Get( world->solverSets, sleepSetId ); + b3SolverSet* sleepSet = b3GetSolverSet( world, sleepSetId ); *sleepSet = (b3SolverSet){ 0 }; - // grab awake set after creating the sleep set because the solver set array may have been resized - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); - b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* awakeSet = b3GetFixedSolverSet( world, b3_awakeSet ); + b3SolverSet* disabledSet = b3GetFixedSolverSet( world, b3_disabledSet ); B3_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); sleepSet->setIndex = sleepSetId; @@ -476,8 +475,8 @@ void b3MergeSolverSets( b3World* world, int setId1, int setId2 ) { B3_ASSERT( setId1 >= b3_firstSleepingSet ); B3_ASSERT( setId2 >= b3_firstSleepingSet ); - b3SolverSet* set1 = b3Array_Get( world->solverSets, setId1 ); - b3SolverSet* set2 = b3Array_Get( world->solverSets, setId2 ); + b3SolverSet* set1 = b3GetSolverSet( world, setId1 ); + b3SolverSet* set2 = b3GetSolverSet( world, setId2 ); // Move the fewest number of bodies if ( set1->bodySims.count < set2->bodySims.count ) diff --git a/src/spherical_joint.c b/src/spherical_joint.c index cc9692c..2058858 100644 --- a/src/spherical_joint.c +++ b/src/spherical_joint.c @@ -301,8 +301,8 @@ void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/weld_joint.c b/src/weld_joint.c index 5a379a7..21d7d35 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -92,8 +92,8 @@ void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/wheel_joint.c b/src/wheel_joint.c index fb229c3..6201cff 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -278,7 +278,7 @@ float b3WheelJoint_GetSpinSpeed( b3JointId jointId ) b3Body* bodyA = b3Array_Get( world->bodies, idA ); b3Body* bodyB = b3Array_Get( world->bodies, idB ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexB = bodyB->localIndex; b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); @@ -321,8 +321,8 @@ float b3WheelJoint_GetSteeringAngle( b3JointId jointId ) b3Body* bodyA = b3Array_Get( world->bodies, idA ); b3Body* bodyB = b3Array_Get( world->bodies, idB ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; @@ -384,8 +384,8 @@ b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ) b3Body* bodyA = b3Array_Get( world->bodies, idA ); // b3Body* bodyB = b3Array_Get( world->bodies, idB ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - // b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetSolverSet( world, bodyA->setIndex ); + // b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; // int localIndexB = bodyB->localIndex; @@ -416,8 +416,8 @@ void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyB = b3Array_Get( world->bodies, idB ); B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); - b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + b3SolverSet* setA = b3GetFixedSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetFixedSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 0f47969..4ce5240 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -1056,11 +1056,11 @@ int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ) b3SerIdPool( buf, &world->solverSetIdPool ); // Solver sets - int setCount = world->solverSets.count; + int setCount = b3GetSolverSetCount( world ); b3SnapW_I32( buf, setCount ); for ( int i = 0; i < setCount; ++i ) { - b3SerSolverSet( buf, world->solverSets.data + i ); + b3SerSolverSet( buf, b3GetSolverSet( world, i ) ); } // Sparse body array (userData is host wiring, zero it on the copy) @@ -1200,9 +1200,10 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re b3DesIdPool( r, &world->solverSetIdPool ); // 3. Solver sets: destroy inner arrays of existing sets first - for ( int i = 0; i < world->solverSets.count; ++i ) + int oldSetCount = b3GetSolverSetCount( world ); + for ( int i = 0; i < oldSetCount; ++i ) { - b3SolverSet* set = world->solverSets.data + i; + b3SolverSet* set = b3GetSolverSet( world, i ); b3Array_Destroy( set->bodySims ); b3Array_Destroy( set->bodyStates ); b3Array_Destroy( set->jointSims ); @@ -1211,17 +1212,24 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re } int setCount = b3SnapR_I32( r ); - if ( r->ok && b3SnapCheckCount( r, setCount, (int)sizeof( b3SolverSet ), 6 * (int)sizeof( int ) ) == false ) + if ( r->ok && ( setCount < b3_firstSleepingSet || + b3SnapCheckCount( r, setCount, (int)sizeof( b3SolverSet ), 6 * (int)sizeof( int ) ) == false ) ) { r->ok = false; } if ( r->ok ) { - b3Array_Resize( world->solverSets, setCount ); - memset( world->solverSets.data, 0, (size_t)setCount * sizeof( b3SolverSet ) ); + int sleepingSetCount = setCount - b3_firstSleepingSet; + b3Array_Resize( world->sleepingSets, sleepingSetCount ); + memset( world->fixedSets, 0, sizeof( world->fixedSets ) ); + if ( sleepingSetCount > 0 ) + { + memset( world->sleepingSets.data, 0, (size_t)sleepingSetCount * sizeof( b3SolverSet ) ); + } + for ( int i = 0; i < setCount; ++i ) { - b3DesSolverSet( r, world->solverSets.data + i ); + b3DesSolverSet( r, b3GetSolverSet( world, i ) ); } } diff --git a/test/test_world.c b/test/test_world.c index d2493bd..a498e68 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -1834,6 +1834,69 @@ static bool StaticQueryFcn( b3ShapeId shapeId, void* context ) return true; } +typedef struct StaticCastHit +{ + int count; + float fraction; + bool nonStatic; +} StaticCastHit; + +static float StaticCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + B3_UNUSED( point, normal, userMaterialId, triangleIndex, childIndex ); + + StaticCastHit* hit = context; + hit->count += 1; + hit->fraction = fraction; + if ( b3Body_GetType( b3Shape_GetBody( shapeId ) ) != b3_staticBody ) + { + hit->nonStatic = true; + } + return fraction; +} + +static bool CheckStaticQueries( b3WorldId worldId, int expectedCount ) +{ + b3QueryFilter filter = b3DefaultQueryFilter(); + + b3Vec3 corners[8]; + for ( int i = 0; i < 8; ++i ) + { + corners[i] = (b3Vec3){ ( i & 1 ) ? 100.0f : -100.0f, ( i & 2 ) ? 10.0f : -10.0f, ( i & 4 ) ? 100.0f : -100.0f }; + } + b3ShapeProxy boxProxy = { corners, 8, 0.0f }; + + StaticQueryHits hits = { worldId, 0, false }; + b3World_OverlapShape_Static( worldId, b3Pos_zero, &boxProxy, filter, StaticQueryFcn, &hits ); + if ( hits.count != expectedCount || hits.nonStatic ) + { + return false; + } + + // Straight down through the column of a dynamic cube, so only the static tile can answer + b3Pos origin = { 0.5f, 5.0f, -9.5f }; + b3Vec3 translation = { 0.0f, -10.0f, 0.0f }; + + StaticCastHit rayHit = { 0, 1.0f, false }; + b3World_CastRay_Static( worldId, origin, translation, filter, StaticCastFcn, &rayHit ); + if ( rayHit.count == 0 || rayHit.nonStatic || b3AbsFloat( rayHit.fraction - 0.5f ) > 0.001f ) + { + return false; + } + + b3Vec3 center = b3Vec3_zero; + b3ShapeProxy sphereProxy = { ¢er, 1, 0.25f }; + StaticCastHit shapeHit = { 0, 1.0f, false }; + b3World_CastShape_Static( worldId, origin, &sphereProxy, translation, filter, StaticCastFcn, &shapeHit ); + if ( shapeHit.count == 0 || shapeHit.nonStatic || b3AbsFloat( shapeHit.fraction - 0.475f ) > 0.001f ) + { + return false; + } + + return true; +} + static void StaticQueryThread( void* context ) { StaticQueryShared* shared = context; @@ -1854,9 +1917,10 @@ static void StaticQueryThread( void* context ) StaticQueryHits hits = { shared->worldId, 0, false }; b3World_OverlapAABB_Static( shared->worldId, box, filter, StaticQueryFcn, &hits ); + bool castsPassed = CheckStaticQueries( shared->worldId, expected ); b3AtomicFetchAddInt( &shared->queryCount, 1 ); - if ( hits.count != expected || hits.nonStatic ) + if ( hits.count != expected || hits.nonStatic || castsPassed == false ) { b3AtomicFetchAddInt( &shared->failCount, 1 ); } @@ -1895,12 +1959,19 @@ static int TestOverlapStaticDuringStep( void ) b3CreateHullShape( bodyId, &shapeDef, &cube.base ); } + b3World* world = b3GetWorldFromId( worldId ); + + // Islands falling asleep during the step grow the solver set array under the reader. Count the + // steps where that happened while at least two full query rounds, which read static transforms, + // ran inside the step. + int overlappedGrowthCount = 0; + StaticQueryShared shared = { 0 }; shared.worldId = worldId; b3Thread* thread = b3CreateThread( StaticQueryThread, &shared, "static query" ); b3BoxHull post = b3MakeBoxHull( 0.1f, 1.0f, 0.1f ); - for ( int step = 0; step < 60; ++step ) + for ( int step = 0; step < 120; ++step ) { // The query thread is idle, so the static tree may change. b3BodyDef bodyDef = b3DefaultBodyDef(); @@ -1911,6 +1982,9 @@ static int TestOverlapStaticDuringStep( void ) ENSURE( b3HasTreeMoved( b3GetWorldFromId( worldId )->broadPhase.trees + b3_staticBody ) ); + int setCapacity = world->sleepingSets.capacity; + int queryCount = b3AtomicLoadInt( &shared.queryCount ); + b3AtomicStoreInt( &shared.expectedCount, staticCount ); b3AtomicStoreInt( &shared.stepping, 1 ); @@ -1923,6 +1997,11 @@ static int TestOverlapStaticDuringStep( void ) b3Yield(); } + if ( world->sleepingSets.capacity > setCapacity && b3AtomicLoadInt( &shared.queryCount ) - queryCount >= 2 ) + { + overlappedGrowthCount += 1; + } + ENSURE( b3HasTreeMoved( b3GetWorldFromId( worldId )->broadPhase.trees + b3_staticBody ) == false ); } @@ -1931,6 +2010,7 @@ static int TestOverlapStaticDuringStep( void ) ENSURE( b3AtomicLoadInt( &shared.queryCount ) > 0 ); ENSURE( b3AtomicLoadInt( &shared.failCount ) == 0 ); + ENSURE( overlappedGrowthCount > 0 ); // The serial path agrees with the regular overlap filtered to static bodies. StaticQueryHits hits = { worldId, 0, false }; @@ -1938,6 +2018,7 @@ static int TestOverlapStaticDuringStep( void ) b3World_OverlapAABB_Static( worldId, box, b3DefaultQueryFilter(), StaticQueryFcn, &hits ); ENSURE( hits.count == staticCount ); ENSURE( hits.nonStatic == false ); + ENSURE( CheckStaticQueries( worldId, staticCount ) ); b3DestroyWorld( worldId ); return 0;