Added a compile time option to use double precision body positions. Most math remains single precision with minimal performance loss. `BOX3D_DOUBLE_PRECISION` Improved accuracy of body inertia computation when shapes are far from the body origin. WARNING: several APIs changed but migration should be straight forward if doubles are not enabled.
452 lines
13 KiB
C
452 lines
13 KiB
C
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "sensor.h"
|
|
|
|
#include "body.h"
|
|
#include "contact.h"
|
|
#include "ctz.h"
|
|
#include "physics_world.h"
|
|
#include "shape.h"
|
|
|
|
#include "box3d/collision.h"
|
|
|
|
// for qsort
|
|
#include "parallel_for.h"
|
|
|
|
#include <stdlib.h>
|
|
|
|
typedef struct b3SensorQueryContext
|
|
{
|
|
b3World* world;
|
|
b3SensorTaskContext* taskContext;
|
|
b3Sensor* sensor;
|
|
b3Shape* sensorShape;
|
|
b3Transform transform;
|
|
} b3SensorQueryContext;
|
|
|
|
static bool b3OverlapSensor( b3Shape* sensorShape, b3Transform sensorTransform, b3Shape* visitorShape,
|
|
b3Transform visitorTransform )
|
|
{
|
|
b3ShapeType type = sensorShape->type;
|
|
|
|
b3ShapeProxy proxy = b3MakeShapeProxy( visitorShape );
|
|
|
|
// Get the visitor shape in the frame of the sensor
|
|
b3Transform relativeTransform = b3InvMulTransforms( sensorTransform, visitorTransform );
|
|
|
|
b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS];
|
|
b3ShapeProxy localProxy;
|
|
|
|
localProxy.count = b3MinInt( proxy.count, B3_MAX_SHAPE_CAST_POINTS );
|
|
for ( int i = 0; i < localProxy.count; ++i )
|
|
{
|
|
localPoints[i] = b3TransformPoint( relativeTransform, proxy.points[i] );
|
|
}
|
|
|
|
localProxy.points = localPoints;
|
|
localProxy.radius = proxy.radius;
|
|
|
|
switch ( type )
|
|
{
|
|
case b3_capsuleShape:
|
|
return b3OverlapCapsule( &sensorShape->capsule, b3Transform_identity, &localProxy );
|
|
|
|
case b3_compoundShape:
|
|
return b3OverlapCompound( sensorShape->compound , b3Transform_identity, &localProxy );
|
|
|
|
case b3_heightShape:
|
|
return b3OverlapHeightField( sensorShape->heightField, b3Transform_identity, &localProxy );
|
|
|
|
case b3_hullShape:
|
|
return b3OverlapHull( sensorShape->hull, b3Transform_identity, &localProxy );
|
|
|
|
case b3_meshShape:
|
|
return b3OverlapMesh( &sensorShape->mesh, b3Transform_identity, &localProxy );
|
|
|
|
case b3_sphereShape:
|
|
return b3OverlapSphere( &sensorShape->sphere, b3Transform_identity, &localProxy );
|
|
|
|
default:
|
|
B3_ASSERT( false );
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Sensor shapes need to
|
|
// - detect begin and end overlap events
|
|
// - events must be reported in deterministic order
|
|
// - maintain an active list of overlaps for query
|
|
|
|
// Assumption
|
|
// - sensors don't detect shapes on the same body
|
|
|
|
// Algorithm
|
|
// Query all sensors for overlaps
|
|
// Check against previous overlaps
|
|
|
|
// Data structures
|
|
// Each sensor has an double buffered array of overlaps
|
|
// These overlaps use a shape reference with index and generation
|
|
|
|
static bool b3SensorQueryCallback( int proxyId, uint64_t userData, void* context )
|
|
{
|
|
B3_UNUSED( proxyId );
|
|
|
|
int shapeId = (int)userData;
|
|
|
|
b3SensorQueryContext* queryContext = (b3SensorQueryContext*)context;
|
|
b3Shape* sensorShape = queryContext->sensorShape;
|
|
int sensorShapeId = sensorShape->id;
|
|
|
|
if ( shapeId == sensorShapeId )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
b3World* world = queryContext->world;
|
|
b3Shape* otherShape = b3Array_Get( world->shapes, shapeId );
|
|
|
|
// Mesh vs mesh is not supported
|
|
if ( ( otherShape->type == b3_meshShape || otherShape->type == b3_heightShape ) &&
|
|
( sensorShape->type == b3_meshShape || sensorShape->type == b3_heightShape ) )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Are sensor events enabled on the other shape?
|
|
if ( otherShape->enableSensorEvents == false )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Skip shapes on the same body
|
|
if ( otherShape->bodyId == sensorShape->bodyId )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Check filter
|
|
if ( b3ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Custom user filter
|
|
if ( sensorShape->enableCustomFiltering || otherShape->enableCustomFiltering )
|
|
{
|
|
b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn;
|
|
if ( customFilterFcn != NULL )
|
|
{
|
|
b3ShapeId idA = { sensorShapeId + 1, world->worldId, sensorShape->generation };
|
|
b3ShapeId idB = { shapeId + 1, world->worldId, otherShape->generation };
|
|
bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext );
|
|
if ( shouldCollide == false )
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
b3Transform otherTransform = b3ToRelativeTransform( b3GetBodyTransform( world, otherShape->bodyId ), b3Pos_zero );
|
|
|
|
bool overlap = b3OverlapSensor( sensorShape, queryContext->transform, otherShape, otherTransform );
|
|
if ( overlap == false )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Record the overlap
|
|
b3Sensor* sensor = queryContext->sensor;
|
|
b3Visitor* shapeRef = b3Array_Emplace( sensor->overlaps2 );
|
|
shapeRef->shapeId = shapeId;
|
|
shapeRef->generation = otherShape->generation;
|
|
|
|
return true;
|
|
}
|
|
|
|
static int b3CompareVisitors( const void* a, const void* b )
|
|
{
|
|
const b3Visitor* sa = (const b3Visitor*)a;
|
|
const b3Visitor* sb = (const b3Visitor*)b;
|
|
|
|
if ( sa->shapeId < sb->shapeId )
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* context )
|
|
{
|
|
b3TracyCZoneNC( sensor_task, "Overlap", b3_colorBrown, true );
|
|
|
|
b3World* world = (b3World*)context;
|
|
B3_ASSERT( (int)workerIndex < world->workerCount );
|
|
b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + workerIndex;
|
|
|
|
B3_ASSERT( startIndex < endIndex );
|
|
|
|
b3DynamicTree* trees = world->broadPhase.trees;
|
|
for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex )
|
|
{
|
|
b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex );
|
|
b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId );
|
|
|
|
// Swap overlap arrays
|
|
b3Array( b3Visitor ) temp = sensor->overlaps1;
|
|
sensor->overlaps1 = sensor->overlaps2;
|
|
sensor->overlaps2 = temp;
|
|
b3Array_Clear( sensor->overlaps2 );
|
|
|
|
// Append sensor hits
|
|
b3Array_Append( sensor->overlaps2, sensor->hits.data, sensor->hits.count );
|
|
|
|
// Clear the hits
|
|
b3Array_Clear( sensor->hits );
|
|
|
|
b3Body* body = b3Array_Get( world->bodies, sensorShape->bodyId );
|
|
if ( body->setIndex == b3_disabledSet || sensorShape->enableSensorEvents == false )
|
|
{
|
|
if ( sensor->overlaps1.count != 0 )
|
|
{
|
|
// This sensor is dropping all overlaps because it has been disabled.
|
|
b3SetBit( &taskContext->eventBits, sensorIndex );
|
|
}
|
|
continue;
|
|
}
|
|
|
|
b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );
|
|
|
|
b3SensorQueryContext queryContext = {
|
|
.world = world,
|
|
.taskContext = taskContext,
|
|
.sensor = sensor,
|
|
.sensorShape = sensorShape,
|
|
.transform = transform,
|
|
};
|
|
|
|
B3_ASSERT( sensorShape->sensorIndex == sensorIndex );
|
|
b3AABB queryBounds = sensorShape->aabb;
|
|
|
|
// Query all trees
|
|
b3DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
|
|
b3DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
|
|
b3DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
|
|
|
|
// Sort the overlaps to enable finding begin and end events.
|
|
qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b3Visitor ), b3CompareVisitors );
|
|
|
|
// Remove duplicates from overlaps2 (sorted). Duplicates are possible due to the hit events appended earlier.
|
|
int uniqueCount = 0;
|
|
int overlapCount = sensor->overlaps2.count;
|
|
b3Visitor* overlapData = sensor->overlaps2.data;
|
|
for ( int i = 0; i < overlapCount; ++i )
|
|
{
|
|
if ( uniqueCount == 0 || overlapData[i].shapeId != overlapData[uniqueCount - 1].shapeId )
|
|
{
|
|
overlapData[uniqueCount] = overlapData[i];
|
|
uniqueCount += 1;
|
|
}
|
|
}
|
|
sensor->overlaps2.count = uniqueCount;
|
|
|
|
int count1 = sensor->overlaps1.count;
|
|
int count2 = sensor->overlaps2.count;
|
|
if ( count1 != count2 )
|
|
{
|
|
// something changed
|
|
b3SetBit( &taskContext->eventBits, sensorIndex );
|
|
}
|
|
else
|
|
{
|
|
for ( int i = 0; i < count1; ++i )
|
|
{
|
|
b3Visitor* s1 = sensor->overlaps1.data + i;
|
|
b3Visitor* s2 = sensor->overlaps2.data + i;
|
|
|
|
if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation )
|
|
{
|
|
// something changed
|
|
b3SetBit( &taskContext->eventBits, sensorIndex );
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
b3TracyCZoneEnd( sensor_task );
|
|
}
|
|
|
|
void b3OverlapSensors( b3World* world )
|
|
{
|
|
int sensorCount = world->sensors.count;
|
|
if ( sensorCount == 0 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
B3_ASSERT( world->workerCount > 0 );
|
|
|
|
b3TracyCZoneNC( overlap_sensors, "Sensors", b3_colorMediumPurple, true );
|
|
|
|
for ( int i = 0; i < world->workerCount; ++i )
|
|
{
|
|
b3SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount );
|
|
}
|
|
|
|
// Parallel-for sensors overlaps
|
|
int minRange = 16;
|
|
b3ParallelFor( world, b3SensorTask, sensorCount, minRange, world, "sensors" );
|
|
|
|
b3TracyCZoneNC( sensor_state, "Events", b3_colorLightSlateGray, true );
|
|
|
|
b3BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits;
|
|
for ( int i = 1; i < world->workerCount; ++i )
|
|
{
|
|
b3InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits );
|
|
}
|
|
|
|
// Iterate sensors bits and publish events
|
|
// Process sensor state changes. Iterate over set bits
|
|
uint64_t* bits = bitSet->bits;
|
|
uint32_t blockCount = bitSet->blockCount;
|
|
|
|
for ( uint32_t k = 0; k < blockCount; ++k )
|
|
{
|
|
uint64_t word = bits[k];
|
|
while ( word != 0 )
|
|
{
|
|
uint32_t ctz = b3CTZ64( word );
|
|
int sensorIndex = (int)( 64 * k + ctz );
|
|
|
|
b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex );
|
|
b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId );
|
|
b3ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation };
|
|
|
|
int count1 = sensor->overlaps1.count;
|
|
int count2 = sensor->overlaps2.count;
|
|
const b3Visitor* refs1 = sensor->overlaps1.data;
|
|
const b3Visitor* refs2 = sensor->overlaps2.data;
|
|
|
|
// overlaps1 can have overlaps that end
|
|
// overlaps2 can have overlaps that begin
|
|
int index1 = 0, index2 = 0;
|
|
while ( index1 < count1 && index2 < count2 )
|
|
{
|
|
const b3Visitor* r1 = refs1 + index1;
|
|
const b3Visitor* r2 = refs2 + index2;
|
|
if ( r1->shapeId == r2->shapeId )
|
|
{
|
|
if ( r1->generation < r2->generation )
|
|
{
|
|
// end
|
|
b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
|
|
b3SensorEndTouchEvent event = {
|
|
.sensorShapeId = sensorId,
|
|
.visitorShapeId = visitorId,
|
|
};
|
|
b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
|
|
index1 += 1;
|
|
}
|
|
else if ( r1->generation > r2->generation )
|
|
{
|
|
// begin
|
|
b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
|
|
b3SensorBeginTouchEvent event = { sensorId, visitorId };
|
|
b3Array_Push( world->sensorBeginEvents, event );
|
|
index2 += 1;
|
|
}
|
|
else
|
|
{
|
|
// persisted
|
|
index1 += 1;
|
|
index2 += 1;
|
|
}
|
|
}
|
|
else if ( r1->shapeId < r2->shapeId )
|
|
{
|
|
// end
|
|
b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
|
|
b3SensorEndTouchEvent event = { sensorId, visitorId };
|
|
b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
|
|
index1 += 1;
|
|
}
|
|
else
|
|
{
|
|
// begin
|
|
b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
|
|
b3SensorBeginTouchEvent event = { sensorId, visitorId };
|
|
b3Array_Push( world->sensorBeginEvents, event );
|
|
index2 += 1;
|
|
}
|
|
}
|
|
|
|
while ( index1 < count1 )
|
|
{
|
|
// end
|
|
const b3Visitor* r1 = refs1 + index1;
|
|
b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
|
|
b3SensorEndTouchEvent event = { sensorId, visitorId };
|
|
b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
|
|
index1 += 1;
|
|
}
|
|
|
|
while ( index2 < count2 )
|
|
{
|
|
// begin
|
|
const b3Visitor* r2 = refs2 + index2;
|
|
b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
|
|
b3SensorBeginTouchEvent event = { sensorId, visitorId };
|
|
b3Array_Push( world->sensorBeginEvents, event );
|
|
index2 += 1;
|
|
}
|
|
|
|
// Clear the smallest set bit
|
|
word = word & ( word - 1 );
|
|
}
|
|
}
|
|
|
|
b3TracyCZoneEnd( sensor_state );
|
|
b3TracyCZoneEnd( overlap_sensors );
|
|
}
|
|
|
|
void b3DestroySensor( b3World* world, b3Shape* sensorShape )
|
|
{
|
|
b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex );
|
|
for ( int i = 0; i < sensor->overlaps2.count; ++i )
|
|
{
|
|
b3Visitor* ref = sensor->overlaps2.data + i;
|
|
b3SensorEndTouchEvent event = {
|
|
.sensorShapeId =
|
|
{
|
|
.index1 = sensorShape->id + 1,
|
|
.world0 = world->worldId,
|
|
.generation = sensorShape->generation,
|
|
},
|
|
.visitorShapeId =
|
|
{
|
|
.index1 = ref->shapeId + 1,
|
|
.world0 = world->worldId,
|
|
.generation = ref->generation,
|
|
},
|
|
};
|
|
|
|
b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
|
|
}
|
|
|
|
// Destroy sensor
|
|
b3Array_Destroy( sensor->hits );
|
|
b3Array_Destroy( sensor->overlaps1 );
|
|
b3Array_Destroy( sensor->overlaps2 );
|
|
|
|
int movedIndex = b3Array_RemoveSwap( world->sensors, sensorShape->sensorIndex );
|
|
if ( movedIndex != B3_NULL_INDEX )
|
|
{
|
|
// Fixup moved sensor
|
|
b3Sensor* movedSensor = b3Array_Get( world->sensors, sensorShape->sensorIndex );
|
|
b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId );
|
|
otherSensorShape->sensorIndex = sensorShape->sensorIndex;
|
|
}
|
|
}
|