box3d/test/test_broad_phase.c

488 lines
14 KiB
C
Raw Normal View History

// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#include "body.h"
#include "broad_phase.h"
#include "physics_world.h"
#include "shape.h"
#include "table.h"
#include "test_macros.h"
#include "box3d/box3d.h"
#include "box3d/math_functions.h"
#include <stdio.h>
// Brute force check of the broad-phase pair set against every live shape pair. Must run right
// after a zero time step so the pair update and the disjoint contact purge have both happened
// against the current fat boxes.
static int CheckPairSet( b3WorldId worldId )
{
b3World* world = b3GetWorldFromId( worldId );
b3BroadPhase* bp = &world->broadPhase;
const b3Shape* shapes = world->shapes.data;
const b3AABB* fatAABBs = world->fatAABBs.data;
int shapeCount = world->shapes.count;
for ( int i = 0; i < shapeCount; ++i )
{
const b3Shape* shapeA = shapes + i;
if ( shapeA->id == B3_NULL_INDEX || shapeA->proxyKey == B3_NULL_INDEX || shapeA->sensorIndex != B3_NULL_INDEX )
{
continue;
}
b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId );
for ( int j = i + 1; j < shapeCount; ++j )
{
const b3Shape* shapeB = shapes + j;
if ( shapeB->id == B3_NULL_INDEX || shapeB->proxyKey == B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX )
{
continue;
}
if ( shapeA->bodyId == shapeB->bodyId )
{
continue;
}
b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId );
bool expected = b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) &&
b3ShouldBodiesCollide( world, bodyA, bodyB ) && b3AABB_Overlaps( fatAABBs[i], fatAABBs[j] );
bool actual = b3ContainsKey( &bp->pairSet, b3ShapePairKey( i, j, 0 ) );
// A sleeping contact is never refreshed, so a stale pair can outlive the overlap when
// the other body is teleported, and a set woken by the narrow phase carries its stale
// contacts into the awake set one collide late. With sleeping on only a missing pair
// is a bug.
if ( world->enableSleep && actual && expected == false )
{
continue;
}
if ( expected != actual )
{
printf( " pair mismatch: shapes %d (body %d type %d set %d) and %d (body %d type %d set %d), expected %d, "
"actual %d\n",
i, shapeA->bodyId, bodyA->type, bodyA->setIndex, j, shapeB->bodyId, bodyB->type, bodyB->setIndex,
expected, actual );
fflush( stdout );
return 1;
}
}
}
return 0;
}
typedef struct RandomState
{
uint32_t seed;
} RandomState;
static uint32_t NextRandom( RandomState* state )
{
// xorshift32
uint32_t x = state->seed;
x ^= x << 13;
x ^= x >> 17;
x ^= x << 5;
state->seed = x;
return x;
}
static float RandomFloat( RandomState* state, float lower, float upper )
{
float u = (float)( NextRandom( state ) & 0xFFFFFF ) / (float)0x1000000;
return lower + ( upper - lower ) * u;
}
static int RandomInt( RandomState* state, int count )
{
return (int)( NextRandom( state ) % (uint32_t)count );
}
#define MAX_BODIES 64
#define STATIC_COUNT 60
#define TILE_COUNT 9
typedef struct BodySlot
{
b3BodyId bodyId;
bool active;
} BodySlot;
typedef struct StressContext
{
b3WorldId worldId;
RandomState random;
b3Pos origin;
float extent;
BodySlot slots[MAX_BODIES];
b3JointId joints[4];
int jointCount;
b3BoxHull box;
} StressContext;
static b3Pos RandomPosition( StressContext* context )
{
float e = context->extent;
b3Vec3 offset = { RandomFloat( &context->random, -e, e ), RandomFloat( &context->random, -e, e ),
RandomFloat( &context->random, -e, e ) };
return b3OffsetPos( context->origin, offset );
}
static b3Vec3 RandomVelocity( StressContext* context )
{
// Some bodies outrun their margin every step and go through continuous collision
float s = RandomInt( &context->random, 4 ) == 0 ? 40.0f : 12.0f;
return (b3Vec3){ RandomFloat( &context->random, -s, s ), RandomFloat( &context->random, -s, s ),
RandomFloat( &context->random, -s, s ) };
}
static void CreateSlotBody( StressContext* context, int slot )
{
RandomState* random = &context->random;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = RandomInt( random, 10 ) == 0 ? b3_kinematicBody : b3_dynamicBody;
bodyDef.position = RandomPosition( context );
bodyDef.linearVelocity = RandomVelocity( context );
bodyDef.isBullet = RandomInt( random, 3 ) == 0;
bodyDef.gravityScale = 0.0f;
b3BodyId bodyId = b3CreateBody( context->worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.filter.categoryBits = 1ull << RandomInt( random, 3 );
shapeDef.filter.maskBits = RandomInt( random, 4 ) == 0 ? 0x5ull : B3_DEFAULT_MASK_BITS;
int shapeCount = 1 + RandomInt( random, 2 );
for ( int i = 0; i < shapeCount; ++i )
{
if ( RandomInt( random, 2 ) == 0 )
{
b3Sphere sphere = { { RandomFloat( random, -1.0f, 1.0f ), 0.0f, 0.0f }, RandomFloat( random, 0.2f, 1.5f ) };
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
}
else
{
b3CreateHullShape( bodyId, &shapeDef, &context->box.base );
}
}
context->slots[slot].bodyId = bodyId;
context->slots[slot].active = true;
}
// A swarm of bodies churning through every proxy mutation the world API offers, with the pair
// set checked against a brute force scan after every step.
static int RunBroadPhaseStress( int workerCount, uint32_t seed, bool enableSleep )
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.workerCount = workerCount;
worldDef.enableSleep = enableSleep;
worldDef.gravity = b3Vec3_zero;
b3WorldId worldId = b3CreateWorld( &worldDef );
StressContext context = { 0 };
context.worldId = worldId;
context.random.seed = seed;
context.origin = (b3Pos){ -1900.0, 0.0, 1330.0 };
context.extent = 12.0f;
context.box = b3MakeBoxHull( 0.95f, 0.6f, 2.2f );
// Kilometre tiles far from the origin under the swarm, like a streamed terrain
b3HeightFieldData* tile = b3CreateGrid( 65, 65, (b3Vec3){ 16.0f, 1.0f, 16.0f }, false );
for ( int i = 0; i < TILE_COUNT; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_staticBody;
bodyDef.position = (b3Pos){ -3072.0 + 1024.0 * ( i % 3 ), -30.0, 1024.0 * ( i / 3 ) };
b3BodyId tileId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.enableSpeculativeContact = false;
b3CreateHeightFieldShape( tileId, &shapeDef, tile );
}
// Static clutter inside the swarm. Some pieces ask for contacts on creation.
b3BodyId staticIds[STATIC_COUNT];
b3HeightFieldData* grid = b3CreateGrid( 5, 5, (b3Vec3){ 2.0f, 1.0f, 2.0f }, false );
b3BoxHull staticBox = b3MakeBoxHull( 3.0f, 0.5f, 3.0f );
for ( int i = 0; i < STATIC_COUNT; ++i )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_staticBody;
bodyDef.position = RandomPosition( &context );
staticIds[i] = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.invokeContactCreation = ( i % 4 ) == 0;
shapeDef.enableSpeculativeContact = ( i % 2 ) == 0;
if ( i % 3 == 0 )
{
b3CreateHeightFieldShape( staticIds[i], &shapeDef, grid );
}
else
{
b3CreateHullShape( staticIds[i], &shapeDef, &staticBox.base );
}
}
for ( int i = 0; i < MAX_BODIES; ++i )
{
if ( RandomInt( &context.random, 4 ) != 0 )
{
CreateSlotBody( &context, i );
}
}
float timeStep = 1.0f / 60.0f;
int stepCount = 400;
for ( int step = 0; step < stepCount; ++step )
{
RandomState* random = &context.random;
int mutationCount = RandomInt( random, 4 );
for ( int m = 0; m < mutationCount; ++m )
{
int slot = RandomInt( random, MAX_BODIES );
BodySlot* body = context.slots + slot;
int action = RandomInt( random, 12 );
if ( body->active == false )
{
if ( action < 6 )
{
CreateSlotBody( &context, slot );
}
continue;
}
switch ( action )
{
case 0:
b3DestroyBody( body->bodyId );
body->active = false;
break;
case 1:
case 2:
b3Body_SetTransform( body->bodyId, RandomPosition( &context ), b3Quat_identity );
break;
case 3:
b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) );
break;
case 4:
b3Body_Disable( body->bodyId );
break;
case 5:
b3Body_Enable( body->bodyId );
break;
case 6:
b3Body_SetBullet( body->bodyId, RandomInt( random, 2 ) == 0 );
break;
case 7:
{
b3BodyType types[3] = { b3_staticBody, b3_kinematicBody, b3_dynamicBody };
b3Body_SetType( body->bodyId, types[RandomInt( random, 3 )] );
if ( b3Body_GetType( body->bodyId ) != b3_staticBody )
{
b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) );
}
}
break;
case 8:
{
// Add a joint that blocks collision, or toggle one
if ( context.jointCount < 4 )
{
int otherSlot = RandomInt( random, MAX_BODIES );
BodySlot* other = context.slots + otherSlot;
if ( otherSlot != slot && other->active && b3Body_GetType( body->bodyId ) == b3_dynamicBody &&
b3Body_GetType( other->bodyId ) == b3_dynamicBody )
{
b3WeldJointDef jointDef = b3DefaultWeldJointDef();
jointDef.base.bodyIdA = body->bodyId;
jointDef.base.bodyIdB = other->bodyId;
jointDef.base.collideConnected = false;
context.joints[context.jointCount++] = b3CreateWeldJoint( worldId, &jointDef );
}
}
else
{
b3JointId jointId = context.joints[RandomInt( random, context.jointCount )];
if ( b3Joint_IsValid( jointId ) )
{
b3Joint_SetCollideConnected( jointId, RandomInt( random, 2 ) == 0 );
}
}
}
break;
case 9:
{
int staticIndex = RandomInt( random, STATIC_COUNT );
b3Body_SetTransform( staticIds[staticIndex], RandomPosition( &context ), b3Quat_identity );
}
break;
case 10:
b3World_RebuildStaticTree( worldId );
break;
default:
break;
}
}
// Joints whose bodies were destroyed are gone too
for ( int j = 0; j < context.jointCount; )
{
if ( b3Joint_IsValid( context.joints[j] ) == false )
{
context.joints[j] = context.joints[context.jointCount - 1];
context.jointCount -= 1;
}
else
{
j += 1;
}
}
// Keep the swarm inside the clutter
for ( int i = 0; i < MAX_BODIES; ++i )
{
BodySlot* body = context.slots + i;
if ( body->active == false || b3Body_IsEnabled( body->bodyId ) == false )
{
continue;
}
b3Pos p = b3Body_GetPosition( body->bodyId );
b3Vec3 d = b3SubPos( p, context.origin );
if ( b3AbsFloat( d.x ) > 2.0f * context.extent || b3AbsFloat( d.y ) > 2.0f * context.extent ||
b3AbsFloat( d.z ) > 2.0f * context.extent )
{
b3Body_SetTransform( body->bodyId, RandomPosition( &context ), b3Quat_identity );
b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) );
}
}
b3World_Step( worldId, timeStep, 4 );
// A zero step finds pairs and purges disjoint contacts without moving anything
b3World_Step( worldId, 0.0f, 1 );
if ( CheckPairSet( worldId ) != 0 )
{
printf( " workers=%d seed=%u sleep=%d step=%d\n", workerCount, seed, enableSleep, step );
fflush( stdout );
b3DestroyWorld( worldId );
b3DestroyHeightField( grid );
b3DestroyHeightField( tile );
return 1;
}
}
b3DestroyWorld( worldId );
b3DestroyHeightField( grid );
b3DestroyHeightField( tile );
return 0;
}
static int BroadPhaseStressTest( void )
{
uint32_t seeds[3] = { 12345u, 0xC0FFEEu, 777u };
for ( int i = 0; i < 3; ++i )
{
ENSURE( RunBroadPhaseStress( 1, seeds[i], false ) == 0 );
ENSURE( RunBroadPhaseStress( 4, seeds[i], false ) == 0 );
ENSURE( RunBroadPhaseStress( 3, seeds[i], true ) == 0 );
}
return 0;
}
// The broad-phase keeps its pending pair marks inside the static tree, so a full rebuild between
// a static mutation and the next step used to wipe them and the contact never appeared. Covers
// both ways a static proxy gets marked: a teleport and a shape created with invokeContactCreation.
static int RunStaticRebuildKeepsMarks( bool useInvoke )
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.gravity = b3Vec3_zero;
b3WorldId worldId = b3CreateWorld( &worldDef );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Pos){ 0.0, 1.0, 0.0 };
b3BodyId sphereId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
b3CreateSphereShape( sphereId, &shapeDef, &sphere );
bodyDef.type = b3_staticBody;
bodyDef.position = (b3Pos){ 100.0, 0.0, 0.0 };
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
b3BoxHull box = b3MakeBoxHull( 2.0f, 0.5f, 2.0f );
if ( useInvoke == false )
{
b3CreateHullShape( groundId, &shapeDef, &box.base );
}
// Settle so the resting sphere carries no mark of its own
for ( int i = 0; i < 3; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
}
b3Body_SetTransform( groundId, b3Pos_zero, b3Quat_identity );
if ( useInvoke )
{
b3ShapeDef invokeDef = b3DefaultShapeDef();
invokeDef.invokeContactCreation = true;
b3CreateHullShape( groundId, &invokeDef, &box.base );
}
b3World_RebuildStaticTree( worldId );
b3World_Step( worldId, 1.0f / 60.0f, 4 );
ENSURE( b3Body_GetContactCapacity( sphereId ) == 1 );
b3DestroyWorld( worldId );
return 0;
}
static int StaticRebuildKeepsMarks( void )
{
ENSURE( RunStaticRebuildKeepsMarks( false ) == 0 );
ENSURE( RunStaticRebuildKeepsMarks( true ) == 0 );
return 0;
}
static int FlushingAssert( const char* condition, const char* fileName, int lineNumber )
{
printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber );
fflush( stdout );
return 1;
}
int BroadPhaseTest( void )
{
// A crash through a pipe loses buffered output, so flush the assertion first
b3SetAssertFcn( FlushingAssert );
RUN_SUBTEST( StaticRebuildKeepsMarks );
RUN_SUBTEST( BroadPhaseStressTest );
return 0;
}