Add b3World_OverlapAABB_Static workaround. Not recommended. Fixed a left over ghost collision
1977 lines
61 KiB
C
1977 lines
61 KiB
C
// SPDX-FileCopyrightText: 2026 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "benchmarks.h"
|
|
#include "body.h"
|
|
#include "dynamic_tree.h"
|
|
#include "overflow_color.h"
|
|
#include "physics_world.h"
|
|
#include "platform.h"
|
|
#include "recording.h"
|
|
#include "test_macros.h"
|
|
|
|
#include "box3d/box3d.h"
|
|
#include "box3d/collision.h"
|
|
#include "box3d/constants.h"
|
|
#include "box3d/math_functions.h"
|
|
|
|
#include <float.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
// This is a simple example of building and running a simulation
|
|
// using Box3D. Here we create a large ground box and a small dynamic
|
|
// box.
|
|
// There are no graphics for this example. Box3D is meant to be used
|
|
// with your rendering engine in your game engine.
|
|
int HelloWorld( void )
|
|
{
|
|
// Construct a world object, which will hold and simulate the rigid bodies.
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.gravity = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
|
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) );
|
|
|
|
// Define the ground body.
|
|
b3BodyDef groundBodyDef = b3DefaultBodyDef();
|
|
groundBodyDef.position = (b3Pos){ 0.0f, -10.0f, 0.0f };
|
|
|
|
// Call the body factory which allocates memory for the ground body
|
|
// from a pool and creates the ground box shape (also from a pool).
|
|
// The body is also added to the world.
|
|
b3BodyId groundId = b3CreateBody( worldId, &groundBodyDef );
|
|
ENSURE( b3Body_IsValid( groundId ) );
|
|
|
|
// Define the ground box shape. The extents are the half-widths of the box.
|
|
b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 10.0f, 50.0f );
|
|
|
|
// Add the box shape to the ground body.
|
|
b3ShapeDef groundShapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base );
|
|
|
|
// Define the dynamic body. We set its position and call the body factory.
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, 4.0f, 0.0f };
|
|
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
// Define another box shape for our dynamic body.
|
|
b3BoxHull dynamicBox = b3MakeCubeHull( 1.0f );
|
|
|
|
// Define the dynamic body shape
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
|
|
// Set the box density to be non-zero, so it will be dynamic.
|
|
shapeDef.density = 1.0f;
|
|
|
|
// Override the default friction.
|
|
shapeDef.baseMaterial.friction = 0.3f;
|
|
|
|
// Add the shape to the body.
|
|
b3CreateHullShape( bodyId, &shapeDef, &dynamicBox.base );
|
|
|
|
// Prepare for simulation. Typically we use a time step of 1/60 of a
|
|
// second (60Hz) and 4 sub-steps. This provides a high quality simulation
|
|
// in most game scenarios.
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 4;
|
|
|
|
b3Pos position = b3Body_GetPosition( bodyId );
|
|
b3Quat rotation = b3Body_GetRotation( bodyId );
|
|
|
|
// This is our little game loop.
|
|
for ( int i = 0; i < 90; ++i )
|
|
{
|
|
// Instruct the world to perform a single step of simulation.
|
|
// It is generally best to keep the time step and iterations fixed.
|
|
b3World_Step( worldId, timeStep, subStepCount );
|
|
|
|
// Now print the position and angle of the body.
|
|
position = b3Body_GetPosition( bodyId );
|
|
rotation = b3Body_GetRotation( bodyId );
|
|
|
|
// printf("%4.2f %4.2f %4.2f\n", position.x, position.y, b3Rot_GetAngle(rotation));
|
|
}
|
|
|
|
// When the world destructor is called, all bodies and joints are freed. This can
|
|
// create orphaned ids, so be careful about your world management.
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE_SMALL( position.y - 1.00f, 0.01f );
|
|
ENSURE_SMALL( rotation.v.x, 0.01f );
|
|
ENSURE_SMALL( rotation.v.z, 0.01f );
|
|
|
|
return 0;
|
|
}
|
|
|
|
int EmptyWorld( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) == true );
|
|
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 1;
|
|
|
|
for ( int i = 0; i < 60; ++i )
|
|
{
|
|
b3World_Step( worldId, timeStep, subStepCount );
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( b3World_IsValid( worldId ) == false );
|
|
|
|
return 0;
|
|
}
|
|
|
|
#define BODY_COUNT 10
|
|
int DestroyAllBodiesWorld( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) == true );
|
|
|
|
int count = 0;
|
|
bool creating = true;
|
|
|
|
b3BodyId bodyIds[BODY_COUNT];
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
b3BoxHull cube = b3MakeCubeHull( 0.5f );
|
|
|
|
for ( int i = 0; i < 2 * BODY_COUNT + 10; ++i )
|
|
{
|
|
if ( creating )
|
|
{
|
|
if ( count < BODY_COUNT )
|
|
{
|
|
bodyIds[count] = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( bodyIds[count], &shapeDef, &cube.base );
|
|
count += 1;
|
|
}
|
|
else
|
|
{
|
|
creating = false;
|
|
}
|
|
}
|
|
else if ( count > 0 )
|
|
{
|
|
b3DestroyBody( bodyIds[count - 1] );
|
|
bodyIds[count - 1] = b3_nullBodyId;
|
|
count -= 1;
|
|
}
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 3 );
|
|
}
|
|
|
|
b3Counters counters = b3World_GetCounters( worldId );
|
|
ENSURE( counters.bodyCount == 0 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( b3World_IsValid( worldId ) == false );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int TestIsValid( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
|
|
b3BodyId bodyId1 = b3CreateBody( worldId, &bodyDef );
|
|
ENSURE( b3Body_IsValid( bodyId1 ) == true );
|
|
|
|
b3BodyId bodyId2 = b3CreateBody( worldId, &bodyDef );
|
|
ENSURE( b3Body_IsValid( bodyId2 ) == true );
|
|
|
|
b3DestroyBody( bodyId1 );
|
|
ENSURE( b3Body_IsValid( bodyId1 ) == false );
|
|
|
|
b3DestroyBody( bodyId2 );
|
|
ENSURE( b3Body_IsValid( bodyId2 ) == false );
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( b3World_IsValid( worldId ) == false );
|
|
ENSURE( b3Body_IsValid( bodyId2 ) == false );
|
|
ENSURE( b3Body_IsValid( bodyId1 ) == false );
|
|
|
|
return 0;
|
|
}
|
|
|
|
#define WORLD_COUNT ( B3_MAX_WORLDS / 2 )
|
|
|
|
int TestWorldRecycle( void )
|
|
{
|
|
_Static_assert( WORLD_COUNT > 0, "world count" );
|
|
|
|
int count = 100;
|
|
|
|
b3WorldId worldIds[WORLD_COUNT];
|
|
|
|
for ( int i = 0; i < count; ++i )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
for ( int j = 0; j < WORLD_COUNT; ++j )
|
|
{
|
|
worldIds[j] = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldIds[j] ) == true );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
b3CreateBody( worldIds[j], &bodyDef );
|
|
}
|
|
|
|
for ( int j = 0; j < WORLD_COUNT; ++j )
|
|
{
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 1;
|
|
|
|
for ( int k = 0; k < 10; ++k )
|
|
{
|
|
b3World_Step( worldIds[j], timeStep, subStepCount );
|
|
}
|
|
}
|
|
|
|
for ( int j = WORLD_COUNT - 1; j >= 0; --j )
|
|
{
|
|
b3DestroyWorld( worldIds[j] );
|
|
ENSURE( b3World_IsValid( worldIds[j] ) == false );
|
|
worldIds[j] = b3_nullWorldId;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static bool CustomFilter( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* context )
|
|
{
|
|
(void)shapeIdA;
|
|
(void)shapeIdB;
|
|
ENSURE( context == NULL );
|
|
return true;
|
|
}
|
|
|
|
static bool PreSolveStatic( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context )
|
|
{
|
|
(void)shapeIdA;
|
|
(void)shapeIdB;
|
|
(void)point;
|
|
(void)normal;
|
|
ENSURE( context == NULL );
|
|
return false;
|
|
}
|
|
|
|
// This test is here to ensure all API functions link correctly.
|
|
int TestWorldCoverage( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) );
|
|
|
|
b3World_EnableSleeping( worldId, true );
|
|
b3World_EnableSleeping( worldId, false );
|
|
bool flag = b3World_IsSleepingEnabled( worldId );
|
|
ENSURE( flag == false );
|
|
|
|
b3World_EnableContinuous( worldId, false );
|
|
b3World_EnableContinuous( worldId, true );
|
|
flag = b3World_IsContinuousEnabled( worldId );
|
|
ENSURE( flag == true );
|
|
|
|
b3World_SetRestitutionThreshold( worldId, 0.0f );
|
|
b3World_SetRestitutionThreshold( worldId, 2.0f );
|
|
float value = b3World_GetRestitutionThreshold( worldId );
|
|
ENSURE( value == 2.0f );
|
|
|
|
b3World_SetHitEventThreshold( worldId, 0.0f );
|
|
b3World_SetHitEventThreshold( worldId, 100.0f );
|
|
value = b3World_GetHitEventThreshold( worldId );
|
|
ENSURE( value == 100.0f );
|
|
|
|
b3World_SetCustomFilterCallback( worldId, CustomFilter, NULL );
|
|
b3World_SetPreSolveCallback( worldId, PreSolveStatic, NULL );
|
|
|
|
b3Vec3 g = { 1.0f, 2.0f };
|
|
b3World_SetGravity( worldId, g );
|
|
b3Vec3 v = b3World_GetGravity( worldId );
|
|
ENSURE( v.x == g.x );
|
|
ENSURE( v.y == g.y );
|
|
|
|
b3ExplosionDef explosionDef = b3DefaultExplosionDef();
|
|
b3World_Explode( worldId, &explosionDef );
|
|
|
|
b3World_SetContactTuning( worldId, 10.0f, 2.0f, 4.0f );
|
|
|
|
b3World_SetMaximumLinearSpeed( worldId, 10.0f );
|
|
value = b3World_GetMaximumLinearSpeed( worldId );
|
|
ENSURE( value == 10.0f );
|
|
|
|
b3World_SetIsotropicInertiaTolerance( worldId, 0.5f );
|
|
value = b3World_GetIsotropicInertiaTolerance( worldId );
|
|
ENSURE( value == 0.5f );
|
|
|
|
b3World_SetIsotropicInertiaTolerance( worldId, 2.0f );
|
|
value = b3World_GetIsotropicInertiaTolerance( worldId );
|
|
ENSURE( value == 1.0f );
|
|
|
|
b3World_EnableWarmStarting( worldId, true );
|
|
flag = b3World_IsWarmStartingEnabled( worldId );
|
|
ENSURE( flag == true );
|
|
|
|
int count = b3World_GetAwakeBodyCount( worldId );
|
|
ENSURE( count == 0 );
|
|
|
|
b3World_SetUserData( worldId, &value );
|
|
void* userData = b3World_GetUserData( worldId );
|
|
ENSURE( userData == &value );
|
|
|
|
b3World_Step( worldId, 1.0f, 1 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int TestSensor( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
// Wall from x = 1 to x = 2
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 1.5f, 11.0f, 0.0f };
|
|
b3BodyId wallId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull box = b3MakeBoxHull( 0.5f, 10.0f, 1.0f );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
shapeDef.enableSensorEvents = true;
|
|
b3CreateHullShape( wallId, &shapeDef, &box.base );
|
|
|
|
// Bullet fired towards the wall
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.isBullet = true;
|
|
bodyDef.gravityScale = 0.0f;
|
|
bodyDef.position = (b3Pos){ 7.39814f, 4.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ -20.0f, 0.0f, 0.0f };
|
|
b3BodyId bulletId = b3CreateBody( worldId, &bodyDef );
|
|
shapeDef = b3DefaultShapeDef();
|
|
shapeDef.isSensor = true;
|
|
shapeDef.enableSensorEvents = true;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f };
|
|
b3CreateSphereShape( bulletId, &shapeDef, &sphere );
|
|
|
|
int beginCount = 0;
|
|
int endCount = 0;
|
|
|
|
while ( true )
|
|
{
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 4;
|
|
b3World_Step( worldId, timeStep, subStepCount );
|
|
|
|
b3Pos bulletPos = b3Body_GetPosition( bulletId );
|
|
// printf( "Bullet pos: %g %g\n", bulletPos.x, bulletPos.y );
|
|
|
|
b3SensorEvents events = b3World_GetSensorEvents( worldId );
|
|
|
|
if ( events.beginCount > 0 )
|
|
{
|
|
beginCount += 1;
|
|
}
|
|
|
|
if ( events.endCount > 0 )
|
|
{
|
|
endCount += 1;
|
|
}
|
|
|
|
if ( bulletPos.x < -1.0f )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( beginCount == 1 );
|
|
ENSURE( endCount == 1 );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int TestContactEvents( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
// Static ground
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, -0.5f, 0.0f };
|
|
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
|
|
b3ShapeDef groundShapeDef = b3DefaultShapeDef();
|
|
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base );
|
|
|
|
// Dynamic sphere dropped onto the ground; restitution causes it to bounce so we get end events
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, 5.0f, 0.0f };
|
|
b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
shapeDef.density = 1.0f;
|
|
shapeDef.enableContactEvents = true;
|
|
shapeDef.baseMaterial.restitution = 0.6f;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
|
|
b3ShapeId sphereShapeId = b3CreateSphereShape( sphereBodyId, &shapeDef, &sphere );
|
|
|
|
int beginCount = 0;
|
|
int endCount = 0;
|
|
bool idsChecked = false;
|
|
|
|
for ( int i = 0; i < 120; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3ContactEvents events = b3World_GetContactEvents( worldId );
|
|
|
|
if ( events.beginCount > 0 && idsChecked == false )
|
|
{
|
|
b3ContactBeginTouchEvent be = events.beginEvents[0];
|
|
bool aIsSphere = B3_ID_EQUALS( be.shapeIdA, sphereShapeId );
|
|
bool bIsSphere = B3_ID_EQUALS( be.shapeIdB, sphereShapeId );
|
|
bool aIsGround = B3_ID_EQUALS( be.shapeIdA, groundShapeId );
|
|
bool bIsGround = B3_ID_EQUALS( be.shapeIdB, groundShapeId );
|
|
ENSURE( ( aIsSphere && bIsGround ) || ( aIsGround && bIsSphere ) );
|
|
ENSURE( b3Contact_IsValid( be.contactId ) );
|
|
idsChecked = true;
|
|
}
|
|
|
|
beginCount += events.beginCount;
|
|
endCount += events.endCount;
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( idsChecked );
|
|
ENSURE( beginCount >= 1 );
|
|
ENSURE( endCount >= 1 );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int TestHitEvents( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.hitEventThreshold = 1.0f;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
// Static ground
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, -0.5f, 0.0f };
|
|
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
|
|
b3ShapeDef groundShapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base );
|
|
|
|
// Sphere driven into the ground fast enough to clear the hit threshold
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.gravityScale = 0.0f;
|
|
bodyDef.position = (b3Pos){ 0.0f, 2.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 0.0f, -30.0f, 0.0f };
|
|
b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
shapeDef.density = 1.0f;
|
|
shapeDef.enableHitEvents = true;
|
|
shapeDef.baseMaterial.userMaterialId = 7;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
|
|
b3CreateSphereShape( sphereBodyId, &shapeDef, &sphere );
|
|
|
|
int hitCount = 0;
|
|
float capturedSpeed = 0.0f;
|
|
uint64_t capturedMaterialA = 0;
|
|
uint64_t capturedMaterialB = 0;
|
|
b3Vec3 capturedNormal = { 0.0f, 0.0f, 0.0f };
|
|
|
|
for ( int i = 0; i < 30; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3ContactEvents events = b3World_GetContactEvents( worldId );
|
|
if ( events.hitCount > 0 && hitCount == 0 )
|
|
{
|
|
b3ContactHitEvent hit = events.hitEvents[0];
|
|
capturedSpeed = hit.approachSpeed;
|
|
capturedNormal = hit.normal;
|
|
capturedMaterialA = hit.userMaterialIdA;
|
|
capturedMaterialB = hit.userMaterialIdB;
|
|
}
|
|
|
|
hitCount += events.hitCount;
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( hitCount >= 1 );
|
|
ENSURE( capturedSpeed > 1.0f );
|
|
// Head-on vertical impact: normal lies along Y
|
|
ENSURE_SMALL( capturedNormal.x, 0.01f );
|
|
ENSURE_SMALL( capturedNormal.z, 0.01f );
|
|
// One side of the contact carries the sphere's user material
|
|
ENSURE( capturedMaterialA == 7 || capturedMaterialB == 7 );
|
|
|
|
return 0;
|
|
}
|
|
|
|
// Hit-event material lookup must respect the compound child that participated in the
|
|
// contact. Two children with distinct userMaterialIds at separated positions, dropped
|
|
// sphere strikes one specifically. Without the fix, both children would report
|
|
// materials[0] and the strike on hull 1 would be misattributed.
|
|
static int TestCompoundHitEvents( void )
|
|
{
|
|
const uint64_t kHullMaterialA = 11;
|
|
const uint64_t kHullMaterialB = 22;
|
|
const uint64_t kSphereMaterial = 99;
|
|
const float kHullCenterX = 3.0f;
|
|
|
|
for ( int side = 0; side < 2; ++side )
|
|
{
|
|
uint64_t expectedHullMaterial = ( side == 0 ) ? kHullMaterialA : kHullMaterialB;
|
|
float spawnX = ( side == 0 ) ? -kHullCenterX : kHullCenterX;
|
|
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.hitEventThreshold = 1.0f;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
// Build a compound with two hulls at opposite x positions, distinct userMaterialIds
|
|
b3BoxHull boxA = b3MakeBoxHull( 1.0f, 1.0f, 1.0f );
|
|
b3BoxHull boxB = b3MakeBoxHull( 1.0f, 1.0f, 1.0f );
|
|
|
|
b3SurfaceMaterial matA = b3DefaultSurfaceMaterial();
|
|
matA.userMaterialId = kHullMaterialA;
|
|
|
|
b3SurfaceMaterial matB = b3DefaultSurfaceMaterial();
|
|
matB.userMaterialId = kHullMaterialB;
|
|
|
|
b3CompoundHullDef hulls[2];
|
|
hulls[0].hull = &boxA.base;
|
|
hulls[0].transform = (b3Transform){ { -kHullCenterX, 0.0f, 0.0f }, b3Quat_identity };
|
|
hulls[0].material = matA;
|
|
hulls[1].hull = &boxB.base;
|
|
hulls[1].transform = (b3Transform){ { kHullCenterX, 0.0f, 0.0f }, b3Quat_identity };
|
|
hulls[1].material = matB;
|
|
|
|
b3CompoundDef compoundDef = { 0 };
|
|
compoundDef.hulls = hulls;
|
|
compoundDef.hullCount = 2;
|
|
b3CompoundData* compound = b3CreateCompound( &compoundDef );
|
|
ENSURE( compound != NULL );
|
|
|
|
// Static body holds the compound
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
b3BodyId compoundBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef compoundShapeDef = b3DefaultShapeDef();
|
|
b3CreateBakedCompoundShape( compoundBodyId, &compoundShapeDef, compound );
|
|
|
|
// Sphere driven straight down onto the chosen child
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.gravityScale = 0.0f;
|
|
bodyDef.position = (b3Pos){ spawnX, 3.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 0.0f, -30.0f, 0.0f };
|
|
b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef sphereShapeDef = b3DefaultShapeDef();
|
|
sphereShapeDef.density = 1.0f;
|
|
sphereShapeDef.enableHitEvents = true;
|
|
sphereShapeDef.baseMaterial.userMaterialId = kSphereMaterial;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
|
|
b3CreateSphereShape( sphereBodyId, &sphereShapeDef, &sphere );
|
|
|
|
int hitCount = 0;
|
|
uint64_t capturedMaterialA = 0;
|
|
uint64_t capturedMaterialB = 0;
|
|
|
|
for ( int i = 0; i < 30; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3ContactEvents events = b3World_GetContactEvents( worldId );
|
|
if ( events.hitCount > 0 && hitCount == 0 )
|
|
{
|
|
b3ContactHitEvent hit = events.hitEvents[0];
|
|
capturedMaterialA = hit.userMaterialIdA;
|
|
capturedMaterialB = hit.userMaterialIdB;
|
|
}
|
|
hitCount += events.hitCount;
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
b3DestroyCompound( compound );
|
|
|
|
ENSURE( hitCount >= 1 );
|
|
// Sphere material on one side
|
|
ENSURE( capturedMaterialA == kSphereMaterial || capturedMaterialB == kSphereMaterial );
|
|
// Struck compound child's material on the other side. The pre-fix code returned
|
|
// materials[0] (kHullMaterialA) for both sides, so a strike on the +x child would fail.
|
|
ENSURE( capturedMaterialA == expectedHullMaterial || capturedMaterialB == expectedHullMaterial );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
enum
|
|
{
|
|
kChild0MaterialId = 101,
|
|
kChild1MaterialId = 202,
|
|
kProbeMaterialId = 999,
|
|
};
|
|
|
|
// No context pointer on mixing callbacks, so capture through file scope
|
|
static struct
|
|
{
|
|
int callCount;
|
|
bool sawChild0;
|
|
bool sawChild1;
|
|
float mixedFriction;
|
|
} materialCapture;
|
|
|
|
static float CaptureFrictionMix( float frictionA, uint64_t userMaterialIdA, float frictionB, uint64_t userMaterialIdB )
|
|
{
|
|
materialCapture.callCount += 1;
|
|
|
|
if ( userMaterialIdA == kChild0MaterialId || userMaterialIdB == kChild0MaterialId )
|
|
{
|
|
materialCapture.sawChild0 = true;
|
|
}
|
|
|
|
if ( userMaterialIdA == kChild1MaterialId || userMaterialIdB == kChild1MaterialId )
|
|
{
|
|
materialCapture.sawChild1 = true;
|
|
materialCapture.mixedFriction = sqrtf( frictionA * frictionB );
|
|
}
|
|
|
|
return sqrtf( frictionA * frictionB );
|
|
}
|
|
|
|
// Contact material selection must use the struck compound child's material, not entry 0
|
|
// of the compound material table. Child 0 gets low friction, child 1 high friction, and a
|
|
// unit friction sphere strikes only child 1, so the mixing callback must see child 1's
|
|
// values. Covers sphere, capsule, and hull children. Issue #69.
|
|
static int TestCompoundContactMaterials( void )
|
|
{
|
|
b3SurfaceMaterial mat0 = b3DefaultSurfaceMaterial();
|
|
mat0.friction = 0.04f;
|
|
mat0.userMaterialId = kChild0MaterialId;
|
|
|
|
b3SurfaceMaterial mat1 = b3DefaultSurfaceMaterial();
|
|
mat1.friction = 0.81f;
|
|
mat1.userMaterialId = kChild1MaterialId;
|
|
|
|
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
|
|
|
// Children at x = -3 and x = +3, all with their top face or surface at y = 0.5
|
|
b3CompoundSphereDef spheres[2] = {
|
|
{ .sphere = { { -3.0f, 0.0f, 0.0f }, 0.5f }, .material = mat0 },
|
|
{ .sphere = { { 3.0f, 0.0f, 0.0f }, 0.5f }, .material = mat1 },
|
|
};
|
|
|
|
b3CompoundCapsuleDef capsules[2] = {
|
|
{ .capsule = { { -3.0f, 0.0f, -0.5f }, { -3.0f, 0.0f, 0.5f }, 0.5f }, .material = mat0 },
|
|
{ .capsule = { { 3.0f, 0.0f, -0.5f }, { 3.0f, 0.0f, 0.5f }, 0.5f }, .material = mat1 },
|
|
};
|
|
|
|
b3CompoundHullDef hulls[2] = {
|
|
{ .hull = &box.base, .transform = { { -3.0f, 0.0f, 0.0f }, b3Quat_identity }, .material = mat0 },
|
|
{ .hull = &box.base, .transform = { { 3.0f, 0.0f, 0.0f }, b3Quat_identity }, .material = mat1 },
|
|
};
|
|
|
|
for ( int childType = 0; childType < 3; ++childType )
|
|
{
|
|
b3CompoundDef compoundDef = { 0 };
|
|
if ( childType == 0 )
|
|
{
|
|
compoundDef.spheres = spheres;
|
|
compoundDef.sphereCount = 2;
|
|
}
|
|
else if ( childType == 1 )
|
|
{
|
|
compoundDef.capsules = capsules;
|
|
compoundDef.capsuleCount = 2;
|
|
}
|
|
else
|
|
{
|
|
compoundDef.hulls = hulls;
|
|
compoundDef.hullCount = 2;
|
|
}
|
|
|
|
b3CompoundData* compound = b3CreateCompound( &compoundDef );
|
|
ENSURE( compound != NULL );
|
|
|
|
memset( &materialCapture, 0, sizeof( materialCapture ) );
|
|
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.frictionCallback = CaptureFrictionMix;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
b3BodyId compoundBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef compoundShapeDef = b3DefaultShapeDef();
|
|
b3CreateBakedCompoundShape( compoundBodyId, &compoundShapeDef, compound );
|
|
|
|
// Sphere driven straight down onto child 1
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.gravityScale = 0.0f;
|
|
bodyDef.position = (b3Pos){ 3.0f, 3.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 0.0f, -30.0f, 0.0f };
|
|
b3BodyId sphereBodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeDef sphereShapeDef = b3DefaultShapeDef();
|
|
sphereShapeDef.density = 1.0f;
|
|
sphereShapeDef.baseMaterial.friction = 1.0f;
|
|
sphereShapeDef.baseMaterial.userMaterialId = kProbeMaterialId;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f };
|
|
b3CreateSphereShape( sphereBodyId, &sphereShapeDef, &sphere );
|
|
|
|
for ( int i = 0; i < 30; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
b3DestroyCompound( compound );
|
|
|
|
ENSURE( materialCapture.callCount > 0 );
|
|
// The pre-fix code fed material table entry 0 to the mixing callback for every child
|
|
ENSURE( materialCapture.sawChild0 == false );
|
|
ENSURE( materialCapture.sawChild1 == true );
|
|
// sqrt(0.81 * 1.0)
|
|
ENSURE_SMALL( materialCapture.mixedFriction - 0.9f, 1e-5f );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int TestSetWorkerCount( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.workerCount = 1;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
ENSURE( b3World_IsValid( worldId ) );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == 1 );
|
|
|
|
CreateJunkyard( worldId );
|
|
StepJunkyard( worldId, 1 );
|
|
|
|
b3World_SetWorkerCount( worldId, 4 );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == 4 );
|
|
|
|
StepJunkyard( worldId, 2 );
|
|
|
|
b3World_SetWorkerCount( worldId, 4 );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == 4 );
|
|
|
|
StepJunkyard( worldId, 3 );
|
|
|
|
b3World_SetWorkerCount( worldId, 0 );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == 1 );
|
|
|
|
StepJunkyard( worldId, 4 );
|
|
|
|
b3World_SetWorkerCount( worldId, -5 );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == 1 );
|
|
|
|
StepJunkyard( worldId, 5 );
|
|
|
|
b3World_SetWorkerCount( worldId, B3_MAX_WORKERS + 10 );
|
|
ENSURE( b3World_GetWorkerCount( worldId ) == B3_MAX_WORKERS );
|
|
|
|
StepJunkyard( worldId, 2 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
return 0;
|
|
}
|
|
|
|
// Verifies the b3*_Overflow solver path. The scene puts >B3_DYNAMIC_COLOR_COUNT
|
|
// dyn-dyn contacts on a single hub body so several land in the overflow color.
|
|
// The new Prepare/Store contactId-pairing asserts in contact_solver.c fire here
|
|
// if Store reads constraints from the wrong (base, spans) pair, so the test
|
|
// catches the bug even though world state ends up plausible (memory layout is
|
|
// stable within a build, so determinism alone is not a witness).
|
|
static int TestOverflowColorPile( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.workerCount = 1;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
OverflowColorPileData data = CreateOverflowColorPile( worldId );
|
|
(void)data;
|
|
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 4;
|
|
|
|
// One step would be enough to trip the asserts, but several steps also
|
|
// exercise the warm-start path (Store -> manifold impulse -> Prepare).
|
|
int stepCount = 10;
|
|
for ( int i = 0; i < stepCount; ++i )
|
|
{
|
|
b3World_Step( worldId, timeStep, subStepCount );
|
|
}
|
|
|
|
// Confirm the scene actually populated the overflow color. Without this,
|
|
// a future change to graph coloring could silently turn the test into a
|
|
// no-op.
|
|
b3Counters counters = b3World_GetCounters( worldId );
|
|
int overflowContacts = counters.colorCounts[B3_GRAPH_COLOR_COUNT - 1];
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( overflowContacts > 0 );
|
|
return 0;
|
|
}
|
|
|
|
// Exposes that b3Body_EnableSleep mutates body->flags but never syncs
|
|
// bodySim->flags / bodyState->flags. When a body created with
|
|
// enableSleep=false is later flipped on, body->flags gains b3_enableSleep
|
|
// but bodySim/bodyState do not. The flag-sync assertion in
|
|
// b3ValidateSolverSets then fires on the next world step.
|
|
//
|
|
// Under Debug + BOX3D_VALIDATE this crashes; after b3Body_EnableSleep
|
|
// calls b3SyncBodyFlags the test runs cleanly.
|
|
static int EnableSleepFlagSyncTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.enableSleep = false;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
ENSURE( b3Body_IsSleepEnabled( bodyId ) == false );
|
|
|
|
b3Body_EnableSleep( bodyId, true );
|
|
ENSURE( b3Body_IsSleepEnabled( bodyId ) == true );
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
// Exposes that b3Body_SetBullet writes only bodySim->flags without
|
|
// touching body->flags, while b3SyncBodyFlags overwrites bodySim from
|
|
// body->flags. Any subsequent b3Body_SetMotionLocks or b3Body_SetType
|
|
// silently wipes (or re-asserts) the bullet bit.
|
|
//
|
|
// Reproducer A: create with isBullet=false, SetBullet(true), then
|
|
// SetMotionLocks. b3Body_IsBullet returns false today (wiped); should be
|
|
// true after fix.
|
|
//
|
|
// Reproducer B: create with isBullet=true, SetBullet(false), then
|
|
// SetMotionLocks. b3Body_IsBullet returns true today (re-asserted); should
|
|
// be false after fix.
|
|
static int SetBulletDriftTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
{
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.isBullet = false;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
ENSURE( b3Body_IsBullet( bodyId ) == false );
|
|
|
|
b3Body_SetBullet( bodyId, true );
|
|
ENSURE( b3Body_IsBullet( bodyId ) == true );
|
|
|
|
b3MotionLocks locks = { 0 };
|
|
locks.linearX = true;
|
|
b3Body_SetMotionLocks( bodyId, locks );
|
|
|
|
ENSURE( b3Body_IsBullet( bodyId ) == true );
|
|
}
|
|
|
|
{
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.isBullet = true;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
ENSURE( b3Body_IsBullet( bodyId ) == true );
|
|
|
|
b3Body_SetBullet( bodyId, false );
|
|
ENSURE( b3Body_IsBullet( bodyId ) == false );
|
|
|
|
b3MotionLocks locks = { 0 };
|
|
locks.linearX = true;
|
|
b3Body_SetMotionLocks( bodyId, locks );
|
|
|
|
ENSURE( b3Body_IsBullet( bodyId ) == false );
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
// Regression: b3Body_EnableSleep used to set world->locked = true before checking
|
|
// for a no-op change, then early-return without unlocking. The next mutator call
|
|
// would then trip the assert in b3GetUnlockedWorld.
|
|
static int EnableSleepNoopUnlockTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.enableSleep = true;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
// No-op: enableSleep is already true. Must not leak the world lock.
|
|
b3Body_EnableSleep( bodyId, true );
|
|
|
|
// Would assert in b3GetUnlockedWorld if the lock had leaked.
|
|
b3Body_EnableSleep( bodyId, false );
|
|
ENSURE( b3Body_IsSleepEnabled( bodyId ) == false );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
static int EnableContactRecyclingTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
|
|
// Default is enabled
|
|
b3BodyId bodyA = b3CreateBody( worldId, &bodyDef );
|
|
ENSURE( b3Body_IsContactRecyclingEnabled( bodyA ) == true );
|
|
|
|
b3Body_EnableContactRecycling( bodyA, false );
|
|
ENSURE( b3Body_IsContactRecyclingEnabled( bodyA ) == false );
|
|
|
|
b3Body_EnableContactRecycling( bodyA, true );
|
|
ENSURE( b3Body_IsContactRecyclingEnabled( bodyA ) == true );
|
|
|
|
// Per-def opt-out at creation
|
|
bodyDef.enableContactRecycling = false;
|
|
b3BodyId bodyB = b3CreateBody( worldId, &bodyDef );
|
|
ENSURE( b3Body_IsContactRecyclingEnabled( bodyB ) == false );
|
|
|
|
// Stepping after toggling must not trip the flag-sync validator
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
// A slow sphere closes on a static round shape from just outside contact. The step is too short
|
|
// to make the sphere fast, so the pair is eligible for recycling. A routine without a speculative
|
|
// margin left the pair empty and recycling then let the sphere sink without a contact. Discussion #175.
|
|
static int RunRecycledApproach( bool capsuleTarget )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.gravity = b3Vec3_zero;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
b3BodyId targetId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
float targetRadius = 0.5f;
|
|
if ( capsuleTarget )
|
|
{
|
|
b3Capsule capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, targetRadius };
|
|
b3CreateCapsuleShape( targetId, &shapeDef, &capsule );
|
|
}
|
|
else
|
|
{
|
|
b3Sphere sphere = { b3Vec3_zero, targetRadius };
|
|
b3CreateSphereShape( targetId, &shapeDef, &sphere );
|
|
}
|
|
|
|
float radius = 0.25f;
|
|
float totalRadius = targetRadius + radius;
|
|
float timeStep = 1.0f / 60.0f;
|
|
|
|
// Half the speculative distance per step
|
|
float speed = 0.5f * B3_SPECULATIVE_DISTANCE / timeStep;
|
|
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, totalRadius + 0.25f * B3_SPECULATIVE_DISTANCE, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 0.0f, -speed, 0.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3Sphere sphere = { b3Vec3_zero, radius };
|
|
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
|
|
|
|
float minSeparation = FLT_MAX;
|
|
for ( int i = 0; i < 20; ++i )
|
|
{
|
|
b3World_Step( worldId, timeStep, 4 );
|
|
|
|
b3Pos p = b3Body_GetPosition( bodyId );
|
|
minSeparation = b3MinFloat( minSeparation, (float)p.y - totalRadius );
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( minSeparation > -B3_LINEAR_SLOP );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int RecycledSphereApproachTest( void )
|
|
{
|
|
ENSURE( RunRecycledApproach( false ) == 0 );
|
|
ENSURE( RunRecycledApproach( true ) == 0 );
|
|
return 0;
|
|
}
|
|
|
|
static bool PreSolveReject( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context )
|
|
{
|
|
(void)shapeIdA;
|
|
(void)shapeIdB;
|
|
(void)point;
|
|
(void)normal;
|
|
bool* reject = context;
|
|
return *reject == false;
|
|
}
|
|
|
|
// A rejected pre-solve frees the manifold while the shapes may already touch. The next step must
|
|
// rebuild the contact and ask again instead of recycling the empty pair.
|
|
static int PreSolveRejectRecycleTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.enableSleep = false;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
bool reject = true;
|
|
b3World_SetPreSolveCallback( worldId, PreSolveReject, &reject );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, -0.5f, 0.0f };
|
|
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( groundId, &shapeDef, &groundBox.base );
|
|
|
|
float radius = 0.25f;
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, radius, 0.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
shapeDef.enablePreSolveEvents = true;
|
|
b3Sphere sphere = { b3Vec3_zero, radius };
|
|
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
reject = false;
|
|
|
|
float minSeparation = FLT_MAX;
|
|
for ( int i = 0; i < 30; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3Pos p = b3Body_GetPosition( bodyId );
|
|
minSeparation = b3MinFloat( minSeparation, (float)p.y - radius );
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
ENSURE( minSeparation > -B3_LINEAR_SLOP );
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int CountRecycledResting( bool enableSpeculative )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.enableSleep = false;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, -0.5f, 0.0f };
|
|
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull groundBox = b3MakeBoxHull( 10.0f, 0.5f, 10.0f );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( groundId, &shapeDef, &groundBox.base );
|
|
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, 0.5f, 0.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
shapeDef.enableSpeculativeContact = enableSpeculative;
|
|
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
|
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
|
|
|
int recycledCount = 0;
|
|
for ( int i = 0; i < 60; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
recycledCount += b3World_GetCounters( worldId ).recycledContactCount;
|
|
}
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
return recycledCount;
|
|
}
|
|
|
|
// Without speculative points an empty pair may be closer than the speculative distance, so these
|
|
// contacts must never recycle
|
|
static int SpeculativeOffRecycleTest( void )
|
|
{
|
|
ENSURE( CountRecycledResting( true ) > 0 );
|
|
ENSURE( CountRecycledResting( false ) == 0 );
|
|
return 0;
|
|
}
|
|
|
|
static int CountMovedNodes( const b3DynamicTree* tree )
|
|
{
|
|
int count = 0;
|
|
for ( int i = 0; i < tree->nodeEnd; ++i )
|
|
{
|
|
const b3TreeNode* node = tree->nodes + i;
|
|
if ( b3IsEmptyNode( node ) == false && b3IsNodeMoved( node ) )
|
|
{
|
|
count += 1;
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
// A proxy can be enlarged in one step and destroyed before the next, emptying the move buffer.
|
|
// The trees must still be rebuilt or the enlarged nodes survive into the next step. Issue #149.
|
|
static int TestEnlargedProxyDestroyed( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.gravity = b3Vec3_zero;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3World* world = b3GetWorldFromId( worldId );
|
|
const b3DynamicTree* tree = world->broadPhase.trees + b3_dynamicBody;
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3Sphere sphere = { b3Vec3_zero, 0.25f };
|
|
|
|
// Resting bodies keep the tree deep enough that the mover has an ancestor that outlives it
|
|
for ( int i = 0; i < 8; ++i )
|
|
{
|
|
bodyDef.position = (b3Pos){ 2.0f * i, 0.0f, 0.0f };
|
|
b3BodyId restingId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateSphereShape( restingId, &shapeDef, &sphere );
|
|
}
|
|
|
|
// This body outruns its AABB margin every step
|
|
bodyDef.position = (b3Pos){ 7.0f, 1.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 6.0f, 0.0f, 0.0f };
|
|
b3BodyId moverId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateSphereShape( moverId, &shapeDef, &sphere );
|
|
|
|
float timeStep = 1.0f / 60.0f;
|
|
b3World_Step( worldId, timeStep, 4 );
|
|
|
|
ENSURE( CountMovedNodes( tree ) > 0 );
|
|
|
|
b3DestroyBody( moverId );
|
|
|
|
// Removing the leaf re-derives the marks along its path, so the tree is already consistent
|
|
// here and nothing is left marked to drive a rebuild. What the destroy can still leave
|
|
// stale is the DFS order the sweep refit needs, so the step has to rebuild regardless.
|
|
b3DynamicTree_Validate( tree );
|
|
ENSURE( CountMovedNodes( tree ) == 0 );
|
|
|
|
b3World_Step( worldId, timeStep, 4 );
|
|
|
|
ENSURE( CountMovedNodes( tree ) == 0 );
|
|
ENSURE( tree->dfsOrdered );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
// b3DestroyBody tears shapes down inline instead of going through the shape destroy path, so it
|
|
// needs its own compound census decrement. A leaked count pins the broad-phase on the slow
|
|
// per-candidate compound path for the life of the world and rides along into snapshots.
|
|
static int TestCompoundShapeCount( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3World* world = b3GetWorldFromId( worldId );
|
|
ENSURE( world->compoundShapeCount == 0 );
|
|
|
|
b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f );
|
|
|
|
b3CompoundHullDef hull;
|
|
hull.hull = &box.base;
|
|
hull.transform = (b3Transform){ b3Vec3_zero, b3Quat_identity };
|
|
hull.material = b3DefaultSurfaceMaterial();
|
|
|
|
b3CompoundDef compoundDef = { 0 };
|
|
compoundDef.hulls = &hull;
|
|
compoundDef.hullCount = 1;
|
|
b3CompoundData* compound = b3CreateCompound( &compoundDef );
|
|
ENSURE( compound != NULL );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
|
|
// Shape destroyed on its own
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeId shapeId = b3CreateBakedCompoundShape( bodyId, &shapeDef, compound );
|
|
ENSURE( world->compoundShapeCount == 1 );
|
|
b3DestroyShape( shapeId, true );
|
|
ENSURE( world->compoundShapeCount == 0 );
|
|
b3DestroyBody( bodyId );
|
|
|
|
// Shape carried away by its body
|
|
bodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateBakedCompoundShape( bodyId, &shapeDef, compound );
|
|
ENSURE( world->compoundShapeCount == 1 );
|
|
b3DestroyBody( bodyId );
|
|
ENSURE( world->compoundShapeCount == 0 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
b3DestroyCompound( compound );
|
|
return 0;
|
|
}
|
|
|
|
// Identical hull data is shared through a reference counted world database.
|
|
static int TestHullDatabase( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
b3BodyId bodyA = b3CreateBody( worldId, &bodyDef );
|
|
b3BodyId bodyB = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
|
|
// Two shapes built from identical data share one owned copy in the world database.
|
|
b3ShapeId shapeA = b3CreateHullShape( bodyA, &shapeDef, &box.base );
|
|
b3ShapeId shapeB = b3CreateHullShape( bodyB, &shapeDef, &box.base );
|
|
|
|
const b3HullData* gotA = b3Shape_GetHull( shapeA );
|
|
const b3HullData* gotB = b3Shape_GetHull( shapeB );
|
|
|
|
// Both shapes point at the single shared copy
|
|
ENSURE( gotA == gotB );
|
|
|
|
// The shared copy is owned by the world, not the caller's stack hull
|
|
ENSURE( gotA != &box.base );
|
|
|
|
// A box built on an independent stack frame must de-duplicate to the same shared copy.
|
|
// This holds only if content hashing sees deterministic padding bytes.
|
|
b3BoxHull box2 = b3MakeBoxHull( 0.5f, 0.5f, 0.5f );
|
|
b3BodyId bodyC = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeId shapeC = b3CreateHullShape( bodyC, &shapeDef, &box2.base );
|
|
ENSURE( b3Shape_GetHull( shapeC ) == gotA );
|
|
b3DestroyShape( shapeC, true );
|
|
|
|
// Setting a shape's hull to its own sole shared copy must not free it mid update.
|
|
b3BoxHull box3 = b3MakeBoxHull( 0.3f, 0.3f, 0.3f );
|
|
b3BodyId bodyD = b3CreateBody( worldId, &bodyDef );
|
|
b3ShapeId shapeD = b3CreateHullShape( bodyD, &shapeDef, &box3.base );
|
|
const b3HullData* gotD = b3Shape_GetHull( shapeD );
|
|
b3Shape_SetHull( shapeD, gotD );
|
|
ENSURE( b3Shape_GetHull( shapeD ) == gotD );
|
|
b3DestroyShape( shapeD, true );
|
|
|
|
// Releasing one reference keeps the other alive
|
|
b3DestroyShape( shapeA, true );
|
|
const b3HullData* stillB = b3Shape_GetHull( shapeB );
|
|
ENSURE( stillB == gotB );
|
|
|
|
b3DestroyShape( shapeB, true );
|
|
|
|
// World destroy asserts the database drained to zero references
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
typedef struct ExplosionResult
|
|
{
|
|
b3Vec3 linearVelocity;
|
|
b3Vec3 angularVelocity;
|
|
} ExplosionResult;
|
|
|
|
// Explode just off the +x side of a centered sphere and capture the impulse it receives.
|
|
// The shape, the blast point, and the witness math all run in the body local frame, so the
|
|
// result must not depend on how far the body sits from the world origin.
|
|
static ExplosionResult RunExplosion( b3Pos base )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.gravity = b3Vec3_zero;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = base;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3Sphere sphere = { b3Vec3_zero, 1.0f };
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
|
|
|
|
// Blast sits 3 units along +x, so the body is pushed back along -x
|
|
b3ExplosionDef explosionDef = b3DefaultExplosionDef();
|
|
explosionDef.position = b3OffsetPos( base, (b3Vec3){ 3.0f, 0.0f, 0.0f } );
|
|
explosionDef.radius = 5.0f;
|
|
explosionDef.falloff = 0.0f;
|
|
explosionDef.impulsePerArea = 10.0f;
|
|
b3World_Explode( worldId, &explosionDef );
|
|
|
|
ExplosionResult result;
|
|
result.linearVelocity = b3Body_GetLinearVelocity( bodyId );
|
|
result.angularVelocity = b3Body_GetAngularVelocity( bodyId );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return result;
|
|
}
|
|
|
|
static int TestExplosion( void )
|
|
{
|
|
ExplosionResult origin = RunExplosion( b3Pos_zero );
|
|
|
|
// Pushed away from the blast along -x. A centered sphere has no transverse or angular component.
|
|
ENSURE( origin.linearVelocity.x < -1.0e-4f );
|
|
ENSURE_SMALL( origin.linearVelocity.y, 1.0e-6f );
|
|
ENSURE_SMALL( origin.linearVelocity.z, 1.0e-6f );
|
|
ENSURE_SMALL( b3Length( origin.angularVelocity ), 1.0e-6f );
|
|
|
|
// The same blast far from the origin must produce the same impulse. The world position only
|
|
// reaches float in the relative difference, so the result holds where a naive cast would not.
|
|
ExplosionResult far = RunExplosion( (b3Pos){ 1.0e7f, 1.0e7f, 1.0e7f } );
|
|
ENSURE_SMALL( far.linearVelocity.x - origin.linearVelocity.x, 1.0e-5f );
|
|
ENSURE_SMALL( far.linearVelocity.y - origin.linearVelocity.y, 1.0e-5f );
|
|
ENSURE_SMALL( far.linearVelocity.z - origin.linearVelocity.z, 1.0e-5f );
|
|
|
|
return 0;
|
|
}
|
|
|
|
// Ensure correct move events from bodies involved in CCD.
|
|
static int TestContinuousMoveEvent( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
b3World_EnableContinuous( worldId, true );
|
|
|
|
// Thin static wall, near face at x = 0.1
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_staticBody;
|
|
bodyDef.position = (b3Pos){ 0.0f, 0.0f, 0.0f };
|
|
b3BodyId wallId = b3CreateBody( worldId, &bodyDef );
|
|
b3BoxHull wallBox = b3MakeBoxHull( 0.1f, 5.0f, 5.0f );
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3CreateHullShape( wallId, &shapeDef, &wallBox.base );
|
|
|
|
// Fast dynamic sphere fired at the wall.
|
|
bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.gravityScale = 0.0f;
|
|
bodyDef.position = (b3Pos){ 3.0f, 0.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ -30.0f, 0.0f, 0.0f };
|
|
b3BodyId ballId = b3CreateBody( worldId, &bodyDef );
|
|
shapeDef = b3DefaultShapeDef();
|
|
shapeDef.density = 1.0f;
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.25f };
|
|
b3CreateSphereShape( ballId, &shapeDef, &sphere );
|
|
|
|
float timeStep = 1.0f / 60.0f;
|
|
int subStepCount = 4;
|
|
bool haveMove = false;
|
|
|
|
for ( int step = 0; step < 30; ++step )
|
|
{
|
|
b3World_Step( worldId, timeStep, subStepCount );
|
|
|
|
b3WorldTransform xf = b3Body_GetTransform( ballId );
|
|
|
|
b3BodyEvents events = b3World_GetBodyEvents( worldId );
|
|
for ( int i = 0; i < events.moveCount; ++i )
|
|
{
|
|
b3BodyMoveEvent* event = events.moveEvents + i;
|
|
if ( B3_ID_EQUALS( event->bodyId, ballId ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
haveMove = true;
|
|
|
|
// The move event must carry the same pose the body reports, CCD rewind included
|
|
ENSURE( event->transform.p.x == xf.p.x );
|
|
ENSURE( event->transform.p.y == xf.p.y );
|
|
ENSURE( event->transform.p.z == xf.p.z );
|
|
ENSURE( event->transform.q.v.x == xf.q.v.x );
|
|
ENSURE( event->transform.q.v.y == xf.q.v.y );
|
|
ENSURE( event->transform.q.v.z == xf.q.v.z );
|
|
ENSURE( event->transform.q.s == xf.q.s );
|
|
}
|
|
}
|
|
|
|
ENSURE( haveMove == true );
|
|
|
|
// Tunnel check
|
|
b3Pos finalPos = b3Body_GetPosition( ballId );
|
|
ENSURE( 0.2f < finalPos.x && finalPos.x < 0.8f );
|
|
|
|
b3DestroyWorld( worldId );
|
|
|
|
return 0;
|
|
}
|
|
|
|
// The contact body sim locators are persistent state. Every contact in every set carries the
|
|
// current encoding for both of its bodies: the awake local index, a static tag, or null for a
|
|
// body that fell asleep. The scenes below drive each site that can invalidate one. The gate is
|
|
// b3ValidateSolverSets, which asserts the invariant on every mutation and every step, so a
|
|
// dropped refresh trips there rather than producing a plausible but wrong simulation. Each scene
|
|
// also runs at worker counts 1 and 4 with the state hashes compared.
|
|
|
|
static b3WorldId CreateLocatorWorld( int workerCount )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.workerCount = workerCount;
|
|
return b3CreateWorld( &worldDef );
|
|
}
|
|
|
|
static b3BodyId CreateLocatorBody( b3WorldId worldId, b3BodyType type, b3Pos position, float hx, float hy, float hz )
|
|
{
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = type;
|
|
bodyDef.position = position;
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3BoxHull box = b3MakeBoxHull( hx, hy, hz );
|
|
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
|
|
|
return bodyId;
|
|
}
|
|
|
|
static void StepLocatorWorld( b3WorldId worldId, int stepCount )
|
|
{
|
|
for ( int i = 0; i < stepCount; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
}
|
|
}
|
|
|
|
// Steps until the body sleeps or the budget runs out. Returns the steps taken.
|
|
static int StepUntilAsleep( b3WorldId worldId, b3BodyId bodyId, int maxSteps )
|
|
{
|
|
for ( int i = 0; i < maxSteps; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
if ( b3Body_IsAwake( bodyId ) == false )
|
|
{
|
|
return i + 1;
|
|
}
|
|
}
|
|
|
|
return maxSteps;
|
|
}
|
|
|
|
// Sleep and wake: first touch puts contacts in the graph, sleeping nulls the locators of the
|
|
// island bodies, waking restores their awake indices. The plate beside the stack holds a
|
|
// non-touching contact across the whole cycle, and that is the contact the wake path has to fix up
|
|
// itself. A touching contact would get a fresh encoding from the graph add and hide a dropped
|
|
// refresh.
|
|
static uint64_t LocatorSleepWakeScene( int workerCount, int* failed )
|
|
{
|
|
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
|
|
|
b3BodyId stackId[3];
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
}
|
|
|
|
// Beside the top box, close enough that the fat bounds overlap and far enough that the
|
|
// shapes never touch. Kinematic and awake, so the contact stays in the awake set while the
|
|
// stack sleeps. Off to the side so it does not catch the body dropped later.
|
|
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 1.04f, 2.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
b3Body_EnableSleep( plateId, false );
|
|
|
|
int steps = StepUntilAsleep( worldId, stackId[0], 300 );
|
|
if ( steps == 300 || b3Body_IsAwake( stackId[2] ) == true )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
// Wake without contact so the non-touching contact keeps its stale locator if the wake
|
|
// path forgets to refresh
|
|
b3Body_SetAwake( stackId[0], true );
|
|
StepLocatorWorld( worldId, 5 );
|
|
|
|
if ( b3Body_IsAwake( stackId[2] ) == false )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
steps = StepUntilAsleep( worldId, stackId[0], 300 );
|
|
if ( steps == 300 )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
// Dropping a body on the pile drives the wake path through first touch. The pile can settle
|
|
// and sleep again, so catch the wake as it happens.
|
|
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 8.0f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
|
|
bool woke = false;
|
|
for ( int i = 0; i < 180; ++i )
|
|
{
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
woke = woke || b3Body_IsAwake( stackId[0] );
|
|
}
|
|
|
|
if ( woke == false )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
|
b3DestroyWorld( worldId );
|
|
return hash;
|
|
}
|
|
|
|
// An island falling asleep and a body leaving the awake set both swap the last awake sim into
|
|
// the hole. The pile that never sleeps supplies a moved body that still owns contacts.
|
|
static uint64_t LocatorAwakeNeighborScene( int workerCount, int* failed )
|
|
{
|
|
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 30.0f, 1.0f, 5.0f );
|
|
|
|
// This island sleeps
|
|
b3BodyId sleeperId[2];
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
sleeperId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
}
|
|
|
|
// Created after the sleepers so its sims sit at the end of the awake set and get swapped
|
|
// into the holes the sleeping island leaves behind
|
|
b3BodyId spareId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 20.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
b3Body_EnableSleep( spareId, false );
|
|
|
|
b3BodyId awakeId[3];
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
awakeId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
b3Body_EnableSleep( awakeId[i], false );
|
|
}
|
|
|
|
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
|
|
if ( steps == 300 || b3Body_IsAwake( awakeId[0] ) == false )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
StepLocatorWorld( worldId, 10 );
|
|
|
|
// Leaving the awake set swaps a body that owns contacts into the spare's slot
|
|
b3Body_Disable( spareId );
|
|
StepLocatorWorld( worldId, 10 );
|
|
|
|
b3Body_Enable( spareId );
|
|
StepLocatorWorld( worldId, 10 );
|
|
|
|
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
|
b3DestroyWorld( worldId );
|
|
return hash;
|
|
}
|
|
|
|
// Destroying a body swaps the last sim of its set into the hole. The moved body keeps its
|
|
// contacts, so its side of each one has to follow it.
|
|
static uint64_t LocatorDestroyScene( int workerCount, int* failed )
|
|
{
|
|
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
|
|
|
b3BodyId stackId[4];
|
|
for ( int i = 0; i < 4; ++i )
|
|
{
|
|
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
}
|
|
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
// The last awake sim moves into this hole while its contacts live
|
|
b3DestroyBody( stackId[0] );
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
// Three static boxes, one resting box each. Destroying the first moves the last static sim.
|
|
b3BodyId staticId[3];
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
float x = 10.0f + 4.0f * i;
|
|
staticId[i] = CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ x, -1.0f, 0.0f }, 1.0f, 1.0f, 1.0f );
|
|
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ x, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
}
|
|
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
b3DestroyBody( staticId[0] );
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
B3_UNUSED( failed );
|
|
|
|
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
|
b3DestroyWorld( worldId );
|
|
return hash;
|
|
}
|
|
|
|
// A kinematic body parked a hair above a pile keeps a non-touching contact to a body that falls
|
|
// asleep, which is the one case the locator cannot name and has to leave to the cold path.
|
|
static uint64_t LocatorKinematicScene( int workerCount, int* failed )
|
|
{
|
|
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
|
|
|
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
|
|
// Close enough that the fat bounds overlap and a non-touching contact exists
|
|
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 0.0f, 1.52f, 0.0f }, 1.0f, 0.5f, 1.0f );
|
|
|
|
int steps = StepUntilAsleep( worldId, boxId, 300 );
|
|
if ( steps == 300 )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
// Drive the plate down to touch and wake the sleeper
|
|
b3Body_SetLinearVelocity( plateId, (b3Vec3){ 0.0f, -1.0f, 0.0f } );
|
|
StepLocatorWorld( worldId, 60 );
|
|
|
|
if ( b3Body_IsAwake( boxId ) == false )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
|
b3DestroyWorld( worldId );
|
|
return hash;
|
|
}
|
|
|
|
// Set type and disable move a body between solver sets, which changes the encoding of every
|
|
// contact it has. A joint across two sleeping sets drives the merge path.
|
|
static uint64_t LocatorTransferScene( int workerCount, int* failed )
|
|
{
|
|
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 20.0f, 1.0f, 5.0f );
|
|
|
|
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
b3Body_SetType( boxId, b3_staticBody );
|
|
StepLocatorWorld( worldId, 5 );
|
|
b3Body_SetType( boxId, b3_dynamicBody );
|
|
StepLocatorWorld( worldId, 5 );
|
|
|
|
b3Body_Disable( boxId );
|
|
StepLocatorWorld( worldId, 5 );
|
|
b3Body_Enable( boxId );
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
// Two piles far enough apart to sleep in separate sets
|
|
b3BodyId sleeperId[2];
|
|
sleeperId[0] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
sleeperId[1] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
|
|
|
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
|
|
if ( steps == 300 || b3Body_IsAwake( sleeperId[1] ) == true )
|
|
{
|
|
*failed = 1;
|
|
}
|
|
|
|
b3SphericalJointDef jointDef = b3DefaultSphericalJointDef();
|
|
jointDef.base.bodyIdA = sleeperId[0];
|
|
jointDef.base.bodyIdB = sleeperId[1];
|
|
b3CreateSphericalJoint( worldId, &jointDef );
|
|
|
|
StepLocatorWorld( worldId, 30 );
|
|
|
|
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
|
b3DestroyWorld( worldId );
|
|
return hash;
|
|
}
|
|
|
|
// The fast flag lives on the body sim and gates contact recycling, but a flag sync rebuilds the
|
|
// sim flags from the body row, which never carries it. A public setter called on a body in
|
|
// flight must not drop it, or the recycle path can take a fast pair.
|
|
static int LocatorFastFlagTest( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
|
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ -20.0f, 2.0f, 0.0f };
|
|
bodyDef.linearVelocity = (b3Vec3){ 400.0f, 0.0f, 0.0f };
|
|
b3BodyId bulletId = b3CreateBody( worldId, &bodyDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f };
|
|
b3CreateSphereShape( bulletId, &shapeDef, &sphere );
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3World* world = b3GetWorldFromId( worldId );
|
|
b3Body* body = b3GetBodyFullId( world, bulletId );
|
|
b3BodySim* sim = b3GetBodySim( world, body );
|
|
ENSURE( ( sim->flags & b3_isFast ) != 0 );
|
|
|
|
// Any public setter that syncs flags. The locks have to actually change or the setter
|
|
// returns early and never syncs.
|
|
b3MotionLocks locks = { 0 };
|
|
locks.angularX = true;
|
|
b3Body_SetMotionLocks( bulletId, locks );
|
|
|
|
sim = b3GetBodySim( world, b3GetBodyFullId( world, bulletId ) );
|
|
ENSURE( ( sim->flags & b3_isFast ) != 0 );
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
static int BodySimLocatorTest( void )
|
|
{
|
|
typedef uint64_t ( *LocatorScene )( int workerCount, int* failed );
|
|
LocatorScene scenes[5] = {
|
|
LocatorSleepWakeScene,
|
|
LocatorAwakeNeighborScene,
|
|
LocatorDestroyScene,
|
|
LocatorKinematicScene,
|
|
LocatorTransferScene,
|
|
};
|
|
|
|
for ( int i = 0; i < 5; ++i )
|
|
{
|
|
int failed = 0;
|
|
uint64_t hash1 = scenes[i]( 1, &failed );
|
|
uint64_t hash4 = scenes[i]( 4, &failed );
|
|
ENSURE( failed == 0 );
|
|
ENSURE( hash1 == hash4 );
|
|
}
|
|
|
|
return LocatorFastFlagTest();
|
|
}
|
|
|
|
// A query thread runs static overlaps while the main thread steps. A static body is added before
|
|
// each step so the step clears moved flags in the static tree under the reader. Run under TSan
|
|
// to check the relaxed flag access.
|
|
typedef struct StaticQueryShared
|
|
{
|
|
b3WorldId worldId;
|
|
b3AtomicInt stepping;
|
|
b3AtomicInt quit;
|
|
b3AtomicInt idle;
|
|
b3AtomicInt expectedCount;
|
|
b3AtomicInt queryCount;
|
|
b3AtomicInt failCount;
|
|
} StaticQueryShared;
|
|
|
|
typedef struct StaticQueryHits
|
|
{
|
|
b3WorldId worldId;
|
|
int count;
|
|
bool nonStatic;
|
|
} StaticQueryHits;
|
|
|
|
static bool StaticQueryFcn( b3ShapeId shapeId, void* context )
|
|
{
|
|
StaticQueryHits* hits = context;
|
|
b3BodyId bodyId = b3Shape_GetBody( shapeId );
|
|
hits->count += 1;
|
|
|
|
// Body type is read through the body array, which the step mutates for awake bodies. Static
|
|
// bodies are not touched by the step.
|
|
if ( b3Body_GetType( bodyId ) != b3_staticBody )
|
|
{
|
|
hits->nonStatic = true;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void StaticQueryThread( void* context )
|
|
{
|
|
StaticQueryShared* shared = context;
|
|
b3AABB box = { { -100.0f, -10.0f, -100.0f }, { 100.0f, 10.0f, 100.0f } };
|
|
b3QueryFilter filter = b3DefaultQueryFilter();
|
|
|
|
while ( b3AtomicLoadInt( &shared->quit ) == 0 )
|
|
{
|
|
if ( b3AtomicLoadInt( &shared->stepping ) == 0 )
|
|
{
|
|
b3AtomicStoreInt( &shared->idle, 1 );
|
|
b3Yield();
|
|
continue;
|
|
}
|
|
|
|
b3AtomicStoreInt( &shared->idle, 0 );
|
|
int expected = b3AtomicLoadInt( &shared->expectedCount );
|
|
|
|
StaticQueryHits hits = { shared->worldId, 0, false };
|
|
b3World_OverlapAABB_Static( shared->worldId, box, filter, StaticQueryFcn, &hits );
|
|
b3AtomicFetchAddInt( &shared->queryCount, 1 );
|
|
|
|
if ( hits.count != expected || hits.nonStatic )
|
|
{
|
|
b3AtomicFetchAddInt( &shared->failCount, 1 );
|
|
}
|
|
}
|
|
}
|
|
|
|
static int TestOverlapStaticDuringStep( void )
|
|
{
|
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
|
worldDef.workerCount = 4;
|
|
b3WorldId worldId = b3CreateWorld( &worldDef );
|
|
|
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
b3BoxHull tile = b3MakeBoxHull( 1.0f, 0.5f, 1.0f );
|
|
int staticCount = 0;
|
|
|
|
for ( int i = -10; i < 10; ++i )
|
|
{
|
|
for ( int j = -10; j < 10; ++j )
|
|
{
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.position = (b3Pos){ 2.0f * i + 1.0f, -0.5f, 2.0f * j + 1.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateHullShape( bodyId, &shapeDef, &tile.base );
|
|
staticCount += 1;
|
|
}
|
|
}
|
|
|
|
b3BoxHull cube = b3MakeBoxHull( 0.4f, 0.4f, 0.4f );
|
|
for ( int i = 0; i < 400; ++i )
|
|
{
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.type = b3_dynamicBody;
|
|
bodyDef.position = (b3Pos){ -9.5f + ( i % 20 ), 1.0f + 0.9f * ( i / 100 ), -9.5f + ( ( i / 20 ) % 5 ) * 4.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateHullShape( bodyId, &shapeDef, &cube.base );
|
|
}
|
|
|
|
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 )
|
|
{
|
|
// The query thread is idle, so the static tree may change.
|
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
bodyDef.position = (b3Pos){ -50.0f + step, 1.0f, 50.0f };
|
|
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
|
b3CreateHullShape( bodyId, &shapeDef, &post.base );
|
|
staticCount += 1;
|
|
|
|
ENSURE( b3HasTreeMoved( b3GetWorldFromId( worldId )->broadPhase.trees + b3_staticBody ) );
|
|
|
|
b3AtomicStoreInt( &shared.expectedCount, staticCount );
|
|
b3AtomicStoreInt( &shared.stepping, 1 );
|
|
|
|
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
|
|
|
b3AtomicStoreInt( &shared.stepping, 0 );
|
|
b3AtomicStoreInt( &shared.idle, 0 );
|
|
while ( b3AtomicLoadInt( &shared.idle ) == 0 )
|
|
{
|
|
b3Yield();
|
|
}
|
|
|
|
ENSURE( b3HasTreeMoved( b3GetWorldFromId( worldId )->broadPhase.trees + b3_staticBody ) == false );
|
|
}
|
|
|
|
b3AtomicStoreInt( &shared.quit, 1 );
|
|
b3JoinThread( thread );
|
|
|
|
ENSURE( b3AtomicLoadInt( &shared.queryCount ) > 0 );
|
|
ENSURE( b3AtomicLoadInt( &shared.failCount ) == 0 );
|
|
|
|
// The serial path agrees with the regular overlap filtered to static bodies.
|
|
StaticQueryHits hits = { worldId, 0, false };
|
|
b3AABB box = { { -100.0f, -10.0f, -100.0f }, { 100.0f, 10.0f, 100.0f } };
|
|
b3World_OverlapAABB_Static( worldId, box, b3DefaultQueryFilter(), StaticQueryFcn, &hits );
|
|
ENSURE( hits.count == staticCount );
|
|
ENSURE( hits.nonStatic == false );
|
|
|
|
b3DestroyWorld( worldId );
|
|
return 0;
|
|
}
|
|
|
|
int WorldTest( void )
|
|
{
|
|
RUN_SUBTEST( HelloWorld );
|
|
RUN_SUBTEST( EmptyWorld );
|
|
RUN_SUBTEST( DestroyAllBodiesWorld );
|
|
RUN_SUBTEST( TestIsValid );
|
|
RUN_SUBTEST( TestWorldRecycle );
|
|
RUN_SUBTEST( TestWorldCoverage );
|
|
RUN_SUBTEST( TestExplosion );
|
|
RUN_SUBTEST( TestSensor );
|
|
RUN_SUBTEST( TestContinuousMoveEvent );
|
|
RUN_SUBTEST( TestContactEvents );
|
|
RUN_SUBTEST( TestHitEvents );
|
|
RUN_SUBTEST( TestCompoundHitEvents );
|
|
RUN_SUBTEST( TestCompoundContactMaterials );
|
|
RUN_SUBTEST( TestOverflowColorPile );
|
|
RUN_SUBTEST( SetBulletDriftTest );
|
|
RUN_SUBTEST( EnableSleepFlagSyncTest );
|
|
RUN_SUBTEST( EnableSleepNoopUnlockTest );
|
|
RUN_SUBTEST( EnableContactRecyclingTest );
|
|
RUN_SUBTEST( RecycledSphereApproachTest );
|
|
RUN_SUBTEST( PreSolveRejectRecycleTest );
|
|
RUN_SUBTEST( SpeculativeOffRecycleTest );
|
|
RUN_SUBTEST( TestSetWorkerCount );
|
|
RUN_SUBTEST( TestHullDatabase );
|
|
RUN_SUBTEST( TestEnlargedProxyDestroyed );
|
|
RUN_SUBTEST( TestCompoundShapeCount );
|
|
RUN_SUBTEST( BodySimLocatorTest );
|
|
RUN_SUBTEST( TestOverlapStaticDuringStep );
|
|
|
|
return 0;
|
|
}
|