less bad
This commit is contained in:
parent
67ef937cd0
commit
0ee063e565
23 changed files with 182 additions and 185 deletions
67
src/body.c
67
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.
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
12
src/joint.c
12
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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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 };
|
||||
|
|
|
|||
Loading…
Reference in a new issue