diff --git a/src/body.c b/src/body.c index 918552b..b6049da 100644 --- a/src/body.c +++ b/src/body.c @@ -33,23 +33,11 @@ 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; } -b3WorldTransform b3GetQueryBodyTransform( b3World* world, b3Body* body ) -{ - if ( body->setIndex == b3_staticSet ) - { - // todo comment - B3_ASSERT( 0 <= body->localIndex ); - return world->staticBodySims[body->localIndex].transform; - } - - return b3GetBodyTransformQuick( world, body ); -} - b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId ) { b3Body* body = b3Array_Get( world->bodies, bodyId ); @@ -65,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; } @@ -74,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 ); } @@ -229,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 ); @@ -254,13 +242,8 @@ 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 ); - if ( setId == b3_staticSet ) - { - world->staticBodySims = set->bodySims.data; - } - *bodySim = (b3BodySim){ 0 }; bodySim->transform.p = def->position; bodySim->transform.q = def->rotation; @@ -440,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 ) { @@ -1381,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 ) ); @@ -1399,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 ); @@ -1493,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 ); @@ -1528,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 ); @@ -1563,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 ); @@ -1676,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 ) @@ -1706,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 ); } @@ -1722,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 @@ -2249,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; @@ -2276,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 ); } @@ -2317,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 ); @@ -2380,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/body.h b/src/body.h index db82e4d..b03063d 100644 --- a/src/body.h +++ b/src/body.h @@ -232,7 +232,6 @@ typedef struct b3BodySim b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ); b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body ); -b3WorldTransform b3GetQueryBodyTransform( b3World* world, b3Body* body ); b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId ); // Create a b3BodyId from a raw id. diff --git a/src/contact.c b/src/contact.c index e5c9a60..a4accdf 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 = b3GetSolverSet( 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..68c46b2 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 9742a0d..b1f7ca4 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1363,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 ) { @@ -1372,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, @@ -1399,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 ) { @@ -1408,11 +1410,24 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp } b3TreeView view = b3MakeTreeView( tree ); - return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, false ); + 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 ) { @@ -1420,31 +1435,19 @@ b3TreeStats b3DynamicTree_RayCastRelaxed( const b3DynamicTree* tree, const b3Ray } b3TreeView view = b3MakeTreeView( tree ); - return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, true ); + 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 }; - } - - b3TreeView view = b3MakeTreeView( tree ); - return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, false ); + return b3BoxCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, false ); } b3TreeStats b3DynamicTree_BoxCastRelaxed( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ) { - if ( tree->proxyCount == 0 ) - { - return B3_LITERAL( b3TreeStats ){ 0 }; - } - - b3TreeView view = b3MakeTreeView( tree ); - return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, true ); + return b3BoxCastDynamicTree( tree, input, maskBits, requireAllBits, callback, context, true ); } b3TreeStats b3TreeView_Query( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits, 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..77e8567 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..97a20c3 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..82aaddc 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/physics_world.c b/src/physics_world.c index 14a5593..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,23 +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->staticBodySims = world->solverSets.data[b3_staticSet].bodySims.data; + 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(); @@ -503,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 ); @@ -578,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 ); } @@ -594,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 ); @@ -800,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 ); @@ -808,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 ) { @@ -839,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; @@ -1084,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 @@ -1102,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; @@ -1998,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 ); @@ -2053,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; } @@ -2383,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 ); @@ -2483,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; @@ -2658,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; @@ -2801,7 +2800,7 @@ static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context // Re-center on the query origin so the overlap test stays in float precision far from the origin b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); - b3Transform transform = b3ToRelativeTransform( b3GetQueryBodyTransform( world, body ), worldContext->origin ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); bool overlapping = b3OverlapShape( shape, transform, &worldContext->proxy ); if ( overlapping == false ) @@ -3004,7 +3003,7 @@ static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t } b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); - b3WorldTransform bodyTransform = b3GetQueryBodyTransform( world, body ); + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin ); b3RayCastInput localInput = *input; @@ -3224,7 +3223,7 @@ static float b3ShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint // Re-center on the query origin so the per-shape cast stays in float precision far from the origin b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); - b3Transform transform = b3ToRelativeTransform( b3GetQueryBodyTransform( world, body ), worldContext->origin ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput ); @@ -3599,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 ); @@ -3767,8 +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( world->staticBodySims == world->solverSets.data[b3_staticSet].bodySims.data ); + B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == b3GetSolverSetCount( world ) ); // Ensure the encoded body sim indices are correct. for ( int contactIndex = 0; contactIndex < world->contacts.count; ++contactIndex ) @@ -3792,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; @@ -4168,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 7c1a24b..ae93808 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -147,13 +147,10 @@ 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; - - // todo comment - b3BodySim* staticBodySims; + // 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. + b3SolverSet fixedSets[b3_firstSleepingSet]; + b3Array( b3SolverSet ) sleepingSets; // Used to create stable ids for joints b3IdPool jointIdPool; @@ -302,6 +299,22 @@ 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 int b3GetSolverSetCount( const b3World* world ) +{ + return b3_firstSleepingSet + world->sleepingSets.count; +} + b3World* b3GetUnlockedWorldFromId( b3WorldId id ); b3World* b3GetWorldFromId( b3WorldId id ); @@ -363,4 +376,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..0a56c74 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..4c5c3a5 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..d177a4f 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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 8f6fa42..1613904 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,19 @@ 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 = b3GetSolverSet( world, b3_awakeSet ); + b3SolverSet* disabledSet = b3GetSolverSet( world, b3_disabledSet ); B3_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); sleepSet->setIndex = sleepSetId; @@ -476,8 +476,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 ) @@ -579,10 +579,6 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source int targetIndex = targetSet->bodySims.count; b3BodySim* targetSim = b3Array_Emplace( targetSet->bodySims ); memcpy( targetSim, sourceSim, sizeof( b3BodySim ) ); - if ( targetSet->setIndex == b3_staticSet ) - { - world->staticBodySims = targetSet->bodySims.data; - } // Clear transient body flags body->flags &= ~b3_bodyTransientFlags; diff --git a/src/spherical_joint.c b/src/spherical_joint.c index cc9692c..43a2abc 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..fd8c94a 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( 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..8480a0b 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 = b3GetSolverSet( world, bodyA->setIndex ); + b3SolverSet* setB = b3GetSolverSet( world, bodyB->setIndex ); int localIndexA = bodyA->localIndex; int localIndexB = bodyB->localIndex; diff --git a/src/world_snapshot.c b/src/world_snapshot.c index d25423c..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,20 +1212,25 @@ 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 ) ); - for ( int i = 0; i < setCount; ++i ) + int sleepingSetCount = setCount - b3_firstSleepingSet; + b3Array_Resize( world->sleepingSets, sleepingSetCount ); + memset( world->fixedSets, 0, sizeof( world->fixedSets ) ); + if ( sleepingSetCount > 0 ) { - b3DesSolverSet( r, world->solverSets.data + i ); + memset( world->sleepingSets.data, 0, (size_t)sleepingSetCount * sizeof( b3SolverSet ) ); } - world->staticBodySims = setCount > b3_staticSet ? world->solverSets.data[b3_staticSet].bodySims.data : NULL; + for ( int i = 0; i < setCount; ++i ) + { + b3DesSolverSet( r, b3GetSolverSet( world, i ) ); + } } if ( !r->ok ) diff --git a/test/test_world.c b/test/test_world.c index dafed60..dd46376 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -1959,7 +1959,7 @@ static int TestOverlapStaticDuringStep( void ) } // Islands falling asleep during the step grow the solver set array under the reader - int setCapacity = b3GetWorldFromId( worldId )->solverSets.capacity; + int setCapacity = b3GetWorldFromId( worldId )->sleepingSets.capacity; StaticQueryShared shared = { 0 }; shared.worldId = worldId; @@ -1997,7 +1997,7 @@ static int TestOverlapStaticDuringStep( void ) ENSURE( b3AtomicLoadInt( &shared.queryCount ) > 0 ); ENSURE( b3AtomicLoadInt( &shared.failCount ) == 0 ); - ENSURE( b3GetWorldFromId( worldId )->solverSets.capacity > setCapacity ); + ENSURE( b3GetWorldFromId( worldId )->sleepingSets.capacity > setCapacity ); // The serial path agrees with the regular overlap filtered to static bodies. StaticQueryHits hits = { worldId, 0, false };