From 5d6364d7313e185e85e74674649f8e6c2ce50939 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 10 Nov 2026 23:05:04 -0800 Subject: [PATCH] Static query (#214) Add b3World_OverlapAABB_Static workaround. Not recommended. Fixed a left over ghost collision --- benchmark/ghost_baseline.csv | 4 + benchmark/ghost_baseline_hf.csv | 4 + benchmark/ghost_scan.c | 20 ++++- benchmark/soak_baseline.csv | 8 +- include/box3d/base.h | 2 +- include/box3d/box3d.h | 8 +- include/box3d/collision.h | 3 +- include/box3d/config.h | 8 +- include/box3d/math_functions.h | 12 ++- include/box3d/types.h | 4 +- samples/sample_ghost.cpp | 13 +++ shared/ghost_collisions.c | 35 +++++++- shared/ghost_collisions.h | 4 + src/dynamic_tree.c | 46 +++++++--- src/dynamic_tree.h | 2 + src/mesh_contact.c | 2 +- src/physics_world.c | 11 +++ src/platform.h | 15 +++- test/test_determinism.c | 12 +-- test/test_world.c | 147 ++++++++++++++++++++++++++++++++ 20 files changed, 311 insertions(+), 49 deletions(-) diff --git a/benchmark/ghost_baseline.csv b/benchmark/ghost_baseline.csv index 3d7f972..2a569ec 100644 --- a/benchmark/ghost_baseline.csv +++ b/benchmark/ghost_baseline.csv @@ -103,3 +103,7 @@ rail,0,1,capsule,0,2 rail,0,1,upright,0,5 rail,0,1,sphere,0,4 rail,0,1,cylinder,0,3 +beams,0,0,box,0,3 +beams,0,0,cylinder,0,3 +beams,0,1,box,0,3 +beams,0,1,cylinder,0,3 diff --git a/benchmark/ghost_baseline_hf.csv b/benchmark/ghost_baseline_hf.csv index 932a909..b4b385c 100644 --- a/benchmark/ghost_baseline_hf.csv +++ b/benchmark/ghost_baseline_hf.csv @@ -103,3 +103,7 @@ rail,0,1,capsule,0,4 rail,0,1,upright,0,5 rail,0,1,sphere,0,5 rail,0,1,cylinder,0,4 +beams,0,0,box,0,3 +beams,0,0,cylinder,0,3 +beams,0,1,box,0,3 +beams,0,1,cylinder,0,3 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c index f61e50c..25aa4d6 100644 --- a/benchmark/ghost_scan.c +++ b/benchmark/ghost_scan.c @@ -20,7 +20,9 @@ #include // Command line and result file keys -static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "peel", "chamfer", "rail" }; +static const char* s_sceneKeys[e_ghostSceneCount] = { + "flat", "ramp", "cliff", "crest", "bumps", "peel", "chamfer", "rail", "beams", +}; static const char* s_bodyKeys[e_ghostBodyCount] = { "box", "capsule", "upright", "sphere", "cylinder" }; #define MAX_CONFIGS 256 @@ -99,6 +101,15 @@ static void RunConfig( Config* config, GhostSettings settings, int stepCount, bo settings.speculative = false; } + // The s&box character over the beam pits, hovering on speculative contacts. A resting cull that keeps + // the chamfers flush with the top launches it. + if ( config->scene == e_ghostBeams ) + { + settings.fixedRotation = true; + settings.speculative = true; + settings.speed = GHOST_BEAM_SPEED; + } + b3WorldDef worldDef = b3DefaultWorldDef(); b3WorldId worldId = b3CreateWorld( &worldDef ); if ( s_recycling == false ) @@ -185,7 +196,8 @@ static int BuildConfigs( Config* configs, int sceneFilter, int presetFilter, int } // Only a flat bottom can rest on the chamfer edges - if ( scene == e_ghostChamfer && body != e_ghostBox && body != e_ghostCylinder ) + bool chamferScene = scene == e_ghostChamfer || scene == e_ghostBeams; + if ( chamferScene && body != e_ghostBox && body != e_ghostCylinder ) { continue; } @@ -385,8 +397,10 @@ int main( int argc, char** argv ) { printf( "Usage: ghost_scan [options]\n" "Runs the Ghost Collisions sample headless and counts ghost manifolds per scene and body.\n" - " -s, --scene= flat, ramp, cliff, crest, bumps, peel, chamfer, rail (default all)\n" + " -s, --scene= flat, ramp, cliff, crest, bumps, peel, chamfer, rail, beams (default all)\n" " chamfer runs only the box and cylinder, fixed rotation without speculative\n" + " beams runs only the box and cylinder, fixed rotation with speculative,\n" + " always at 8 m/s\n" " -b, --body= box, capsule, upright, sphere, cylinder (default all)\n" " -m, --mirror=<0|1|all> mirrored mesh (default all)\n" " -p, --preset=<0-3|all> PEEL preset (default all)\n" diff --git a/benchmark/soak_baseline.csv b/benchmark/soak_baseline.csv index 7db4e34..ab4be06 100644 --- a/benchmark/soak_baseline.csv +++ b/benchmark/soak_baseline.csv @@ -1,9 +1,9 @@ filter,base,generations,shape,tunnels,awake,depth4,depth8,depth16,depth32 -all,12345,1000,box,0,0,1538,220,3,0 -all,12345,1000,capsule,0,0,14507,3488,28,0 -all,12345,1000,cylinder,0,0,11210,2353,37,0 -all,12345,1000,sphere,0,0,56,0,0,0 all,12345,16000,box,0,0,23676,3191,44,0 all,12345,16000,capsule,0,0,231496,54552,515,0 all,12345,16000,cylinder,0,0,178981,37441,480,0 all,12345,16000,sphere,0,0,987,0,0,0 +all,12345,1000,box,0,0,1503,218,3,0 +all,12345,1000,capsule,0,0,14507,3488,28,0 +all,12345,1000,cylinder,0,0,10859,2193,36,0 +all,12345,1000,sphere,0,0,56,0,0,0 diff --git a/include/box3d/base.h b/include/box3d/base.h index eb85047..8ae8e05 100644 --- a/include/box3d/base.h +++ b/include/box3d/base.h @@ -134,7 +134,7 @@ B3_API void b3SetAssertFcn( b3AssertFcn* assertFcn ); /// Internal assertion handler. Allows for host intervention. B3_API int b3InternalAssert( const char* condition, const char* fileName, int lineNumber ); /// Assert that a condition is true. -#define B3_ASSERT( condition ) \ +#define B3_ASSERT( condition ) \ ( (void)( ( !!( condition ) ) || ( b3InternalAssert( #condition, __FILE__, (int)( __LINE__ ) ), 0 ) ) ) #else #define B3_ASSERT( ... ) ( (void)0 ) diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index c24c542..301e41d 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -79,10 +79,16 @@ B3_API b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ); /// reference to this data. B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); -/// Overlap test for all shapes that *potentially* overlap the provided AABB +/// Overlap test for all shapes that *potentially* overlap the provided AABB. 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, diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 64b1e56..cae180a 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -682,8 +682,7 @@ B3_API void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity /// Collide a triangle and hull. Normal points from triangle to hull. B3_API void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, - const b3HullData* hullB, b3SATCache* cache, - bool enableSpeculative ); + const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative ); /// Collide a triangle and sphere. Normal points from triangle to sphere. B3_API void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, diff --git a/include/box3d/config.h b/include/box3d/config.h index f39eb7e..011a5cf 100644 --- a/include/box3d/config.h +++ b/include/box3d/config.h @@ -15,19 +15,19 @@ // A define passed on the compiler command line still wins over this file. // Large world mode. Stores world positions in double precision. Affects ABI. -//#define BOX3D_DOUBLE_PRECISION +// #define BOX3D_DOUBLE_PRECISION // Build the scalar fallback instead of SSE2/NEON. -//#define BOX3D_DISABLE_SIMD +// #define BOX3D_DISABLE_SIMD // Enable internal validation in debug builds. -//#define BOX3D_VALIDATE +// #define BOX3D_VALIDATE // Decorate the public API with your own export macro instead of Box3D's // box3d_EXPORTS/BOX3D_DLL scheme, for example when compiling Box3D into another // shared library. A single value cannot switch between dllexport and dllimport, // so this suits embedding more than shipping Box3D as its own DLL. -//#define BOX3D_EXPORT MYENGINE_API +// #define BOX3D_EXPORT MYENGINE_API // Enable assertions when NDEBUG is defined // #define B3_ENABLE_ASSERT diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index 811eea8..86c77c3 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -236,14 +236,14 @@ B3_INLINE float b3Atan2( float y, float x ) /// for cross-platform determinism. B3_API b3CosSin b3ComputeCosSin( float radians ); -/// @deprecated +/// @deprecated B3_INLINE float b3Sin( float radians ) { b3CosSin cs = b3ComputeCosSin( radians ); return cs.sine; } -/// @deprecated +/// @deprecated B3_INLINE float b3Cos( float radians ) { b3CosSin cs = b3ComputeCosSin( radians ); @@ -797,7 +797,7 @@ B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t ) return w; } -/// Translate a local AABB by a world position, rounding outward so the single precision box +/// Translate a local AABB by a world position, rounding outward so the single precision box /// encloses the double precision box. Far from the origin a plain conversion could clip a /// shape out of its own box. /// In single precision mode the the rounding is a no-op. @@ -1099,13 +1099,11 @@ B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a ) /// position += timeStep * newVelocity /// This drives towards a zero position. By using implicit integration we get a stable solution that /// doesn't require transcendental functions. -B3_INLINE float b3SpringDamper( float hertz, float dampingRatio, float position, float velocity, - float timeStep ) +B3_INLINE float b3SpringDamper( float hertz, float dampingRatio, float position, float velocity, float timeStep ) { float omega = 2.0f * B3_PI * hertz; float omegaH = omega * timeStep; - return ( velocity - omega * omegaH * position ) / - ( 1.0f + 2.0f * dampingRatio * omegaH + omegaH * omegaH ); + return ( velocity - omega * omegaH * position ) / ( 1.0f + 2.0f * dampingRatio * omegaH + omegaH * omegaH ); } /// The closest points between to segments or infinite lines. diff --git a/include/box3d/types.h b/include/box3d/types.h index 85d0ced..b850b7a 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -2879,8 +2879,8 @@ typedef enum b3TriangleFeature typedef enum b3MeshContactFlags { b3_meshContactHullFace = 0x01, ///< Normal is a hull face normal. - b3_meshContactSnapped = 0x02, ///< The normal is snapped to the triangle normal. - b3_meshContactDeep = 0x04, ///< The contact is considered deep (2 * linear slop). + b3_meshContactSnapped = 0x02, ///< The normal is snapped to the triangle normal. + b3_meshContactDeep = 0x04, ///< The contact is considered deep (2 * linear slop). } b3MeshContactFlags; /// Separating axis test cache. Provides temporal acceleration of collision routines. diff --git a/samples/sample_ghost.cpp b/samples/sample_ghost.cpp index 3ce5e77..e2c9ee6 100644 --- a/samples/sample_ghost.cpp +++ b/samples/sample_ghost.cpp @@ -2385,6 +2385,19 @@ public: relaunch = true; } + // The beams ghost needs the s&box character hovering on speculative contacts + if ( settings.scene == e_ghostBeams ) + { + settings.fixedRotation = true; + settings.speculative = true; + settings.speed = GHOST_BEAM_SPEED; + if ( settings.body != e_ghostBox && settings.body != e_ghostCylinder ) + { + settings.body = e_ghostBox; + } + relaunch = true; + } + if ( settings.scene == e_ghostInternal ) { m_camera->SetView( 60.0f, 20.0f, 160.0f, { 0.0f, 30.0f, 0.0f } ); diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c index 226ccd3..2caf0a0 100644 --- a/shared/ghost_collisions.c +++ b/shared/ghost_collisions.c @@ -28,6 +28,14 @@ #define GHOST_RAIL_SKIRT 0.4f +// The s&box beam floor at 1/30 m per inch, rounded to whole chamfer cells so a height field grid holds every +// vertex. Beams with chamfered tops over deep pits, the 0.8 m hull always spans a pit. +#define GHOST_BEAM_COUNT 20 +#define GHOST_BEAM_START_X -8.0f +#define GHOST_BEAM_PITCH 0.75f +#define GHOST_BEAM_WIDTH 0.4f +#define GHOST_BEAM_PIT_DEPTH 0.8f + typedef struct PeelPreset { int columnCount; @@ -43,7 +51,7 @@ static const PeelPreset s_peelPresets[GHOST_PEEL_PRESET_COUNT] = { }; const char* g_ghostSceneNames[e_ghostSceneCount] = { - "Flat", "Concave", "Cliff", "Crest", "Bumps", "Internal", "Chamfer", "Rail", + "Flat", "Concave", "Cliff", "Crest", "Bumps", "Internal", "Chamfer", "Rail", "Beams", }; const char* g_ghostBodyNames[e_ghostBodyCount] = { "Box", "H Capsule", "V Capsule", "Sphere", "Cylinder" }; @@ -484,6 +492,26 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) PushProfile( data, GHOST_RAIL_THICKNESS, -GHOST_RAIL_SKIRT ); break; + case e_ghostBeams: + { + // A height field cannot hold the vertical pit walls, so there each wall takes one cell + float wallRun = settings->heightField ? GHOST_CHAMFER_WIDTH : 0.0f; + PushProfile( data, -h, 0.0f ); + for ( int i = 0; i + 1 < GHOST_BEAM_COUNT; ++i ) + { + float pitLeft = GHOST_BEAM_START_X + GHOST_BEAM_PITCH * (float)i + GHOST_BEAM_WIDTH; + float pitRight = GHOST_BEAM_START_X + GHOST_BEAM_PITCH * (float)( i + 1 ); + PushProfile( data, pitLeft - GHOST_CHAMFER_WIDTH, 0.0f ); + PushProfile( data, pitLeft, -GHOST_CHAMFER_DROP ); + PushProfile( data, pitLeft + wallRun, -GHOST_BEAM_PIT_DEPTH ); + PushProfile( data, pitRight - wallRun, -GHOST_BEAM_PIT_DEPTH ); + PushProfile( data, pitRight, -GHOST_CHAMFER_DROP ); + PushProfile( data, pitRight + GHOST_CHAMFER_WIDTH, 0.0f ); + } + PushProfile( data, h, 0.0f ); + } + break; + default: PushProfile( data, -h, 0.0f ); PushProfile( data, h, 0.0f ); @@ -512,7 +540,7 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) b3ShapeDef shapeDef = b3DefaultShapeDef(); b3CreateHullShape( data->groundId, &shapeDef, &box.base ); } - else if ( settings->scene == e_ghostChamfer ) + else if ( settings->scene == e_ghostChamfer || settings->scene == e_ghostBeams ) { // One cell per chamfer puts every notch vertex on the grid AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, GHOST_CHAMFER_WIDTH ), groundHalfWidth, cellCountZ, @@ -723,7 +751,8 @@ static bool IsGhost( const GhostCollisionsData* data, b3Vec3 normal, float xMin, } GhostBody body = data->settings.body; - bool chamferHull = data->settings.scene == e_ghostChamfer && ( body == e_ghostBox || body == e_ghostCylinder ); + bool chamferScene = data->settings.scene == e_ghostChamfer || data->settings.scene == e_ghostBeams; + bool chamferHull = chamferScene && ( body == e_ghostBox || body == e_ghostCylinder ); float lower = B3_PI; float upper = -B3_PI; diff --git a/shared/ghost_collisions.h b/shared/ghost_collisions.h index c307bc4..8c6448a 100644 --- a/shared/ghost_collisions.h +++ b/shared/ghost_collisions.h @@ -23,6 +23,9 @@ extern "C" #define GHOST_RAIL_HEIGHT 0.35f #define GHOST_RAIL_THICKNESS 0.08f +// The beam ghost depends on how the body phases with the pits, at the default speed the box misses it +#define GHOST_BEAM_SPEED 8.0f + typedef enum GhostScene { e_ghostFlat, @@ -33,6 +36,7 @@ typedef enum GhostScene e_ghostInternal, e_ghostChamfer, e_ghostRail, + e_ghostBeams, e_ghostSceneCount } GhostScene; diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 5206637..5e43cf4 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -867,21 +867,28 @@ B3_FORCE_INLINE bool b3TestCategory( uint64_t categoryBits, uint64_t maskBits, b return requireAllBits ? ( categoryBits & maskBits ) == maskBits : ( categoryBits & maskBits ) != 0; } +B3_FORCE_INLINE uint32_t b3LoadNodeFlags( const b3TreeNode* node, bool relaxedFlags ) +{ + return relaxedFlags ? b3AtomicLoadU32Raw( &node->flagIndex ) : node->flagIndex; +} + B3_FORCE_INLINE b3TreeStats b3QueryTreeView( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits, - b3TreeQueryCallbackFcn* callback, void* context, bool hasProxies ) + b3TreeQueryCallbackFcn* callback, void* context, 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; } - 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 ); b3AABBV boxv = b3LoadAABBV( &aabb ); @@ -898,9 +905,10 @@ B3_FORCE_INLINE b3TreeStats b3QueryTreeView( const b3TreeView* view, b3AABB aabb continue; } - if ( b3IsLeaf( node ) ) + uint32_t flags = b3LoadNodeFlags( node, relaxedFlags ); + if ( flags & B3_LEAF_NODE ) { - int proxyId = b3GetProxyId( node ); + int proxyId = (int)( flags & B3_NODE_INDEX_MASK ); uint64_t userData = (uint64_t)(uint32_t)node->shapeIndex; if ( hasProxies ) { @@ -924,7 +932,7 @@ B3_FORCE_INLINE b3TreeStats b3QueryTreeView( const b3TreeView* view, b3AABB aabb { if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = b3GetLeftChild( node ); + stack[stackCount++] = (int)( flags & B3_NODE_INDEX_MASK ); } else { @@ -1354,7 +1362,19 @@ b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_ } b3TreeView view = b3MakeTreeView( tree ); - return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true ); + return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true, 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 ); } b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, @@ -1398,10 +1418,10 @@ b3TreeStats b3TreeView_Query( const b3TreeView* view, b3AABB aabb, uint64_t mask { if ( view->proxies == NULL ) { - return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, false ); + return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, false, false ); } - return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, true ); + return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, true, false ); } b3TreeStats b3TreeView_QueryClosest( const b3TreeView* view, b3Vec3 point, uint64_t maskBits, bool requireAllBits, @@ -2176,7 +2196,7 @@ void b3DynamicTree_ClearMoved( b3DynamicTree* tree ) return; } - root->flagIndex &= ~B3_MOVED_NODE; + b3AtomicStoreU32Raw( &root->flagIndex, root->flagIndex & ~B3_MOVED_NODE ); if ( b3IsLeaf( root ) ) { return; @@ -2194,7 +2214,7 @@ void b3DynamicTree_ClearMoved( b3DynamicTree* tree ) b3TreeNode* node = nodes + pair + i; if ( node->flagIndex & B3_MOVED_NODE ) { - node->flagIndex &= ~B3_MOVED_NODE; + b3AtomicStoreU32Raw( &node->flagIndex, node->flagIndex & ~B3_MOVED_NODE ); if ( b3IsLeaf( node ) == false ) { if ( stackCount < B3_TREE_STACK_SIZE ) diff --git a/src/dynamic_tree.h b/src/dynamic_tree.h index cee702e..773c445 100644 --- a/src/dynamic_tree.h +++ b/src/dynamic_tree.h @@ -109,5 +109,7 @@ void b3DynamicTree_MoveProxyInternal( b3DynamicTree* tree, int proxyId, b3AABB a void b3DynamicTree_MarkProxyMovedSerial( b3DynamicTree* tree, int proxyId ); 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 ); int b3DynamicTree_GatherMovedProxies( const b3DynamicTree* tree, int* proxyIds ); void b3DynamicTree_Refit( b3DynamicTree* tree ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index b5d4396..c7437e5 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -1551,7 +1551,7 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape } // Padding - hullBottom -= b3MaxFloat( 0.0f, restingSeparation ) + linearSlop; + hullBottom = b3MinFloat( hullBottom, hullBottom - restingSeparation + 0.25f * linearSlop ); for ( int i = 0; i < survivorCount; ++i ) { diff --git a/src/physics_world.c b/src/physics_world.c index ce86343..bddd58a 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -2759,6 +2759,17 @@ b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter f return treeStats; } +b3TreeStats b3World_OverlapAABB_Static( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidAABB( aabb ) ); + + WorldQueryContext worldContext = { world, fcn, filter, context }; + return b3DynamicTree_QueryRelaxed( world->broadPhase.trees + b3_staticBody, aabb, filter.maskBits, false, + TreeQueryCallback, &worldContext ); +} + typedef struct WorldOverlapContext { b3World* world; diff --git a/src/platform.h b/src/platform.h index 856b19b..b2d6fa3 100644 --- a/src/platform.h +++ b/src/platform.h @@ -111,10 +111,10 @@ static inline uint32_t b3AtomicLoadU32( b3AtomicU32* a ) } // Relaxed load on a plain word. Lets a racing reader peek before paying for a read modify write. -static inline uint32_t b3AtomicLoadU32Raw( uint32_t* a ) +static inline uint32_t b3AtomicLoadU32Raw( const uint32_t* a ) { #if defined( _MSC_VER ) && !defined( __clang__ ) - return (uint32_t)__iso_volatile_load32( (volatile __int32*)a ); + return (uint32_t)__iso_volatile_load32( (const volatile __int32*)a ); #elif defined( __GNUC__ ) || defined( __clang__ ) return __atomic_load_n( a, __ATOMIC_RELAXED ); #else @@ -122,6 +122,17 @@ static inline uint32_t b3AtomicLoadU32Raw( uint32_t* a ) #endif } +static inline void b3AtomicStoreU32Raw( uint32_t* a, uint32_t value ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) + __iso_volatile_store32( (volatile __int32*)a, (__int32)value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( a, value, __ATOMIC_RELAXED ); +#else +#error "Unsupported platform" +#endif +} + static inline uint32_t b3AtomicFetchOrU32( uint32_t* a, uint32_t mask ) { #if defined( _MSC_VER ) diff --git a/test/test_determinism.c b/test/test_determinism.c index 9a3f5fd..ae60cb4 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -21,25 +21,25 @@ #if defined( BOX3D_DOUBLE_PRECISION ) #define RAGDOLL_SLEEP_STEP 282 #define RAGDOLL_HASH 0x6E4B13AD -#define WAVE_PILE_SLEEP_STEP 315 -#define WAVE_PILE_HASH 0x15007937 +#define WAVE_PILE_SLEEP_STEP 316 +#define WAVE_PILE_HASH 0x7CB7D931 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x489585E6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0x5B4429DC #define MESH_DROP_SLEEP_STEP 197 -#define MESH_DROP_HASH 0x5DA091C3 +#define MESH_DROP_HASH 0xA2E8BB21 #else #define RAGDOLL_SLEEP_STEP 277 #define RAGDOLL_HASH 0x5E042B6E -#define WAVE_PILE_SLEEP_STEP 455 -#define WAVE_PILE_HASH 0x854F313D +#define WAVE_PILE_SLEEP_STEP 300 +#define WAVE_PILE_HASH 0x3155DA54 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xF3AD9BA6 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0xE3271F3D #define MESH_DROP_SLEEP_STEP 197 -#define MESH_DROP_HASH 0x96D47D4F +#define MESH_DROP_HASH 0x96B62947 #endif // The goldens above pin exact values for the default four point manifold. A build that diff --git a/test/test_world.c b/test/test_world.c index c9022ef..d2493bd 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -6,6 +6,7 @@ #include "dynamic_tree.h" #include "overflow_color.h" #include "physics_world.h" +#include "platform.h" #include "recording.h" #include "test_macros.h" @@ -1797,6 +1798,151 @@ static int BodySimLocatorTest( void ) 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 ); @@ -1825,6 +1971,7 @@ int WorldTest( void ) RUN_SUBTEST( TestEnlargedProxyDestroyed ); RUN_SUBTEST( TestCompoundShapeCount ); RUN_SUBTEST( BodySimLocatorTest ); + RUN_SUBTEST( TestOverlapStaticDuringStep ); return 0; }