This commit is contained in:
Erin Catto 2026-11-11 15:38:56 -08:00
commit 0ee063e565
23 changed files with 182 additions and 185 deletions

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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