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