From 4d9b34777866eb6126ec9efb33a3900f86dcf0bd Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Wed, 11 Nov 2026 11:02:38 -0800 Subject: [PATCH] more custom queris for tloc --- include/box3d/box3d.h | 25 ++++++++++---- src/dynamic_tree.c | 78 +++++++++++++++++++++++++++++++------------ src/dynamic_tree.h | 4 +++ src/physics_world.c | 55 ++++++++++++++++++++++++++++++ test/test_world.c | 66 +++++++++++++++++++++++++++++++++++- 5 files changed, 199 insertions(+), 29 deletions(-) diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 301e41d..1b70d4d 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -83,12 +83,6 @@ B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ); -/// Overlap test for all shapes that *potentially* overlap the provided AABB. This only considers static bodies, -/// so it may be invoked concurrently with b3World_Step. This is not thread-safe with shape creation or destruction. -/// This is a workaround for The Legend of California and may be removed later if the workaround is no longer needed. -B3_API b3TreeStats b3World_OverlapAABB_Static( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, - b3OverlapResultFcn* fcn, void* context ); - /// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative /// to the world origin, which lets the query stay precise far from the world origin. B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, @@ -121,6 +115,25 @@ B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Ve B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); +/// Overlap test for all shapes that *potentially* overlap the provided AABB. This only considers static bodies, +/// so it may be invoked concurrently with b3World_Step. This is not thread-safe with 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 ); + +/// Similar for shape overlap. +B3_API b3TreeStats b3World_OverlapShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ); + +/// Similar for cast ray. +B3_API b3TreeStats b3World_CastRay_Static( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); + +/// Similar for cast shape. +B3_API b3TreeStats b3World_CastShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, + void* context ); + /// Cast a capsule mover through the world. This is a special shape cast that handles sliding along /// other shapes while reducing clipping. This is not a good source of information about what the /// mover is touching. Instead use the planes returned by b3World_CollideMover. Ignores back-side diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 5e43cf4..9742a0d 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1082,11 +1082,14 @@ B3_FORCE_INLINE b3TreeStats b3QueryClosestTreeView( const b3TreeView* view, b3Ve // before either is followed, then the survivors are ordered so the closer one narrows first. B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context, - bool hasProxies ) + bool hasProxies, bool relaxedFlags ) { b3TreeStats result = { 0 }; - if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) ) + const b3TreeNode* nodes = view->nodes; + + uint32_t rootFlags = b3LoadNodeFlags( nodes + B3_ROOT_NODE, relaxedFlags ); + if ( rootFlags == B3_EMPTY_NODE ) { return result; } @@ -1105,11 +1108,9 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; b3AABBV boxv = b3LoadAABBV( &segmentAABB ); - const b3TreeNode* nodes = view->nodes; - int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = b3GetRootPair( nodes ); + stack[stackCount++] = ( rootFlags & B3_LEAF_NODE ) ? B3_ROOT_NODE : (int)( rootFlags & B3_NODE_INDEX_MASK ); b3RayCastInput subInput = *input; @@ -1120,6 +1121,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R const b3TreeNode* hit[2]; bool isLeaf[2]; + int index[2]; int hitCount = 0; for ( int i = 0; i < 2; ++i ) { @@ -1138,7 +1140,9 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R continue; } - isLeaf[hitCount] = b3IsLeaf( node ); + uint32_t flags = b3LoadNodeFlags( node, relaxedFlags ); + isLeaf[hitCount] = ( flags & B3_LEAF_NODE ) != 0; + index[hitCount] = (int)( flags & B3_NODE_INDEX_MASK ); hit[hitCount] = node; hitCount += 1; } @@ -1154,6 +1158,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R if ( d1 < d2 ) { B3_SWAP( hit[0], hit[1] ); + B3_SWAP( index[0], index[1] ); } } @@ -1161,7 +1166,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R { if ( isLeaf[i] ) { - int proxyId = b3GetProxyId( hit[i] ); + int proxyId = index[i]; uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex; if ( hasProxies ) { @@ -1200,7 +1205,7 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R { if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = b3GetLeftChild( hit[i] ); + stack[stackCount++] = index[i]; } else { @@ -1216,11 +1221,14 @@ B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3R // Follows structure of ray cast with small tweaks to handle a swept box. B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context, - bool hasProxies ) + bool hasProxies, bool relaxedFlags ) { b3TreeStats result = { 0 }; - if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) ) + const b3TreeNode* nodes = view->nodes; + + uint32_t rootFlags = b3LoadNodeFlags( nodes + B3_ROOT_NODE, relaxedFlags ); + if ( rootFlags == B3_EMPTY_NODE ) { return result; } @@ -1247,11 +1255,9 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B }; b3AABBV boxv = b3LoadAABBV( &totalAABB ); - const b3TreeNode* nodes = view->nodes; - int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = b3GetRootPair( nodes ); + stack[stackCount++] = ( rootFlags & B3_LEAF_NODE ) ? B3_ROOT_NODE : (int)( rootFlags & B3_NODE_INDEX_MASK ); b3BoxCastInput subInput = *input; @@ -1262,6 +1268,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B const b3TreeNode* hit[2]; bool isLeaf[2]; + int index[2]; int hitCount = 0; for ( int i = 0; i < 2; ++i ) { @@ -1281,7 +1288,9 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B continue; } - isLeaf[hitCount] = b3IsLeaf( node ); + uint32_t flags = b3LoadNodeFlags( node, relaxedFlags ); + isLeaf[hitCount] = ( flags & B3_LEAF_NODE ) != 0; + index[hitCount] = (int)( flags & B3_NODE_INDEX_MASK ); hit[hitCount] = node; hitCount += 1; } @@ -1297,6 +1306,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B if ( d1 < d2 ) { B3_SWAP( hit[0], hit[1] ); + B3_SWAP( index[0], index[1] ); } } @@ -1304,7 +1314,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B { if ( isLeaf[i] ) { - int proxyId = b3GetProxyId( hit[i] ); + int proxyId = index[i]; uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex; if ( hasProxies ) { @@ -1340,7 +1350,7 @@ B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3B { if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = b3GetLeftChild( hit[i] ); + stack[stackCount++] = index[i]; } else { @@ -1398,7 +1408,19 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp } b3TreeView view = b3MakeTreeView( tree ); - return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true ); + return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, false ); +} + +b3TreeStats b3DynamicTree_RayCastRelaxed( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ) +{ + if ( tree->proxyCount == 0 ) + { + return B3_LITERAL( b3TreeStats ){ 0 }; + } + + b3TreeView view = b3MakeTreeView( tree ); + return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, true ); } b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, @@ -1410,7 +1432,19 @@ b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInp } b3TreeView view = b3MakeTreeView( tree ); - return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true ); + return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, false ); +} + +b3TreeStats b3DynamicTree_BoxCastRelaxed( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ) +{ + if ( tree->proxyCount == 0 ) + { + return B3_LITERAL( b3TreeStats ){ 0 }; + } + + b3TreeView view = b3MakeTreeView( tree ); + return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true, true ); } b3TreeStats b3TreeView_Query( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits, @@ -1440,10 +1474,10 @@ b3TreeStats b3TreeView_RayCast( const b3TreeView* view, const b3RayCastInput* in { if ( view->proxies == NULL ) { - return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, false ); + return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, false, false ); } - return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, true ); + return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, true, false ); } b3TreeStats b3TreeView_BoxCast( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, @@ -1451,10 +1485,10 @@ b3TreeStats b3TreeView_BoxCast( const b3TreeView* view, const b3BoxCastInput* in { if ( view->proxies == NULL ) { - return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, false ); + return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, false, false ); } - return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, true ); + return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, true, false ); } // Median split == 0, Surface area heuristic == 1 diff --git a/src/dynamic_tree.h b/src/dynamic_tree.h index 773c445..906c59b 100644 --- a/src/dynamic_tree.h +++ b/src/dynamic_tree.h @@ -111,5 +111,9 @@ void b3DynamicTree_MarkProxyMoved( b3DynamicTree* tree, int proxyId, b3AABB aabb void b3DynamicTree_ClearMoved( b3DynamicTree* tree ); b3TreeStats b3DynamicTree_QueryRelaxed( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ); +b3TreeStats b3DynamicTree_RayCastRelaxed( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ); +b3TreeStats b3DynamicTree_BoxCastRelaxed( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, + bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ); int b3DynamicTree_GatherMovedProxies( const b3DynamicTree* tree, int* proxyIds ); void b3DynamicTree_Refit( b3DynamicTree* tree ); diff --git a/src/physics_world.c b/src/physics_world.c index bddd58a..9ebc79e 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -2865,6 +2865,21 @@ b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3Shape return treeStats; } +b3TreeStats b3World_OverlapShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin ); + WorldOverlapContext worldContext = { + world, fcn, filter, *proxy, origin, context, + }; + + return b3DynamicTree_QueryRelaxed( world->broadPhase.trees + b3_staticBody, aabb, filter.maskBits, false, + b3TreeOverlapCallback, &worldContext ); +} + typedef struct WorldMoverContext { b3World* world; @@ -3080,6 +3095,20 @@ b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation return treeStats; } +b3TreeStats b3World_CastRay_Static( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context }; + + return b3DynamicTree_RayCastRelaxed( world->broadPhase.trees + b3_staticBody, &input, filter.maskBits, false, + RayCastCallback, &worldContext ); +} + // This callback finds the closest hit. This is the most common callback used in games. static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex, int childIndex, void* context ) @@ -3293,6 +3322,32 @@ b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapePro return treeStats; } +b3TreeStats b3World_CastShape_Static( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + WorldShapeCastContext worldContext = { 0 }; + worldContext.world = world; + worldContext.fcn = fcn; + worldContext.filter = filter; + worldContext.fraction = 1.0f; + worldContext.origin = origin; + worldContext.input.proxy = *proxy; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = false; + worldContext.userContext = context; + + b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius ); + b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f }; + + return b3DynamicTree_BoxCastRelaxed( world->broadPhase.trees + b3_staticBody, &treeInput, filter.maskBits, false, + b3ShapeCastCallback, &worldContext ); +} + typedef struct WorldMoverCastContext { b3World* world; diff --git a/test/test_world.c b/test/test_world.c index d2493bd..c91c7ad 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -1834,6 +1834,69 @@ static bool StaticQueryFcn( b3ShapeId shapeId, void* context ) return true; } +typedef struct StaticCastHit +{ + int count; + float fraction; + bool nonStatic; +} StaticCastHit; + +static float StaticCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + B3_UNUSED( point, normal, userMaterialId, triangleIndex, childIndex ); + + StaticCastHit* hit = context; + hit->count += 1; + hit->fraction = fraction; + if ( b3Body_GetType( b3Shape_GetBody( shapeId ) ) != b3_staticBody ) + { + hit->nonStatic = true; + } + return fraction; +} + +static bool CheckStaticQueries( b3WorldId worldId, int expectedCount ) +{ + b3QueryFilter filter = b3DefaultQueryFilter(); + + b3Vec3 corners[8]; + for ( int i = 0; i < 8; ++i ) + { + corners[i] = (b3Vec3){ ( i & 1 ) ? 100.0f : -100.0f, ( i & 2 ) ? 10.0f : -10.0f, ( i & 4 ) ? 100.0f : -100.0f }; + } + b3ShapeProxy boxProxy = { corners, 8, 0.0f }; + + StaticQueryHits hits = { worldId, 0, false }; + b3World_OverlapShape_Static( worldId, b3Pos_zero, &boxProxy, filter, StaticQueryFcn, &hits ); + if ( hits.count != expectedCount || hits.nonStatic ) + { + return false; + } + + // Straight down through the column of a dynamic cube, so only the static tile can answer + b3Pos origin = { 0.5f, 5.0f, -9.5f }; + b3Vec3 translation = { 0.0f, -10.0f, 0.0f }; + + StaticCastHit rayHit = { 0, 1.0f, false }; + b3World_CastRay_Static( worldId, origin, translation, filter, StaticCastFcn, &rayHit ); + if ( rayHit.count == 0 || rayHit.nonStatic || b3AbsFloat( rayHit.fraction - 0.5f ) > 0.001f ) + { + return false; + } + + b3Vec3 center = b3Vec3_zero; + b3ShapeProxy sphereProxy = { ¢er, 1, 0.25f }; + StaticCastHit shapeHit = { 0, 1.0f, false }; + b3World_CastShape_Static( worldId, origin, &sphereProxy, translation, filter, StaticCastFcn, &shapeHit ); + if ( shapeHit.count == 0 || shapeHit.nonStatic || b3AbsFloat( shapeHit.fraction - 0.475f ) > 0.001f ) + { + return false; + } + + return true; +} + static void StaticQueryThread( void* context ) { StaticQueryShared* shared = context; @@ -1856,7 +1919,7 @@ static void StaticQueryThread( void* context ) b3World_OverlapAABB_Static( shared->worldId, box, filter, StaticQueryFcn, &hits ); b3AtomicFetchAddInt( &shared->queryCount, 1 ); - if ( hits.count != expected || hits.nonStatic ) + if ( hits.count != expected || hits.nonStatic || CheckStaticQueries( shared->worldId, expected ) == false ) { b3AtomicFetchAddInt( &shared->failCount, 1 ); } @@ -1938,6 +2001,7 @@ static int TestOverlapStaticDuringStep( void ) b3World_OverlapAABB_Static( worldId, box, b3DefaultQueryFilter(), StaticQueryFcn, &hits ); ENSURE( hits.count == staticCount ); ENSURE( hits.nonStatic == false ); + ENSURE( CheckStaticQueries( worldId, staticCount ) ); b3DestroyWorld( worldId ); return 0;