more custom queris for tloc

This commit is contained in:
Erin Catto 2026-11-11 11:02:38 -08:00
commit 4d9b347778
5 changed files with 199 additions and 29 deletions

View file

@ -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

View file

@ -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

View file

@ -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 );

View file

@ -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;

View file

@ -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 = { &center, 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;