Reduce ghosts (#212)

Full ghost collision handling for sphere, capsule, and hull versus a
single mesh or heightfield. This requires mesh to be built with edge
identification.

Optimized mesh builder.

Removed B3_MAX_MESH_CONTACT_TRIANGLES. You will still hit performance
problems for dense meshes. Fixes #184
This commit is contained in:
Erin Catto 2026-11-10 21:53:10 -08:00 • committed by GitHub
commit 38f2ae1e19
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
61 changed files with 17756 additions and 1492 deletions

View file

@ -83,6 +83,10 @@
</Expand>
</Type>
<Type Name="b3LocalManifoldPoint">
<DisplayString>s: {separation}</DisplayString>
</Type>
<Type Name="b3SeparationFunction">
<DisplayString>type: {type}</DisplayString>
<Expand>

View file

@ -581,6 +581,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact*
target->separation = source->separation;
target->featureId = b3MakeFeatureId( source->pair );
target->triangleIndex = B3_NULL_INDEX;
target->triangleFeature = b3_featureNone;
target->meshFlags = 0;
target->normalVelocity = 0.0f;
}

View file

@ -9,8 +9,6 @@
#include "box3d/collision.h"
#include "box3d/types.h"
#define B3_FORCE_GHOST_COLLISIONS 0
typedef struct b3Shape b3Shape;
typedef struct b3World b3World;

View file

@ -990,7 +990,7 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L
}
b3Point2D hull[2 * B3_MAX_CLIP_POINTS];
int hullCount = b3Hull2D( pts, count, hull );
int hullCount = b3Hull2D( pts, count, hull, 0 );
int finalCount = b3SimplifyHull2D( hull, hullCount, target );
B3_ASSERT( 0 < finalCount && finalCount <= target );

View file

@ -99,6 +99,53 @@
21 3 22
*/
static b3Vec3 b3GetGridPoint( const float* heights, int columnCount, b3Vec3 scale, int row, int column )
{
return b3Mul( scale, (b3Vec3){ (float)column, heights[row * columnCount + column], (float)row } );
}
static b3Vec3 b3GetGridTriangleNormal( const float* heights, int columnCount, b3Vec3 scale, int row, int column, bool odd )
{
b3Vec3 point11 = b3GetGridPoint( heights, columnCount, scale, row, column );
b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
b3Vec3 point22 = b3GetGridPoint( heights, columnCount, scale, row + 1, column + 1 );
b3Vec3 v1 = odd ? point22 : point11;
b3Vec3 v2 = odd ? point12 : point21;
b3Vec3 v3 = odd ? point21 : point12;
return b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) );
}
// Mark vertices as flat or concave.
static void b3MarkGridEdge( uint8_t* vertexFlags, int index1, int index2, b3Vec3 point1, b3Vec3 point2, b3Vec3 normal1,
b3Vec3 normal2, bool clockwise )
{
b3Vec3 outward = b3Cross( b3Normalize( b3Sub( point2, point1 ) ), normal1 );
float angle = b3Atan2( b3Dot( normal2, outward ), b3Dot( normal2, normal1 ) );
// Quantize to match the mesh rule.
int8_t quantized = b3QuantizeEdgeAngle( clockwise ? -angle : angle );
if ( quantized != 0 )
{
vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex );
vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex );
}
if ( quantized < 0 )
{
vertexFlags[index1] |= b3_concaveVertex;
vertexFlags[index2] |= b3_concaveVertex;
}
}
static void b3MarkGridBoundary( uint8_t* vertexFlags, int index1, int index2 )
{
vertexFlags[index1] = (uint8_t)( vertexFlags[index1] & ~b3_flatVertex );
vertexFlags[index2] = (uint8_t)( vertexFlags[index2] & ~b3_flatVertex );
}
b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
{
int columnCount = data->countX;
@ -119,6 +166,8 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) );
int flagsOffset = (int)byteCount;
byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );
int vertexFlagOffset = (int)byteCount;
byteCount += b3AlignUp8( (size_t)( ( heightCount + 7 ) / 8 ) );
// Zero the whole blob so alignment padding is defined. The construction-time hash
// sweeps raw bytes and would otherwise pick up uninitialized padding.
@ -133,6 +182,7 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
hf->heightsOffset = heightsOffset;
hf->materialOffset = materialOffset;
hf->flagsOffset = flagsOffset;
hf->vertexFlagOffset = vertexFlagOffset;
hf->clockwise = data->clockwiseWinding;
uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset );
@ -466,6 +516,79 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
B3_ASSERT( triangleIndex == triangleCount );
// b3MeshVertexFlags
uint8_t* vertexFlags = (uint8_t*)b3Alloc( heightCount * sizeof( uint8_t ) );
memset( vertexFlags, b3_flatVertex, heightCount * sizeof( uint8_t ) );
bool clockwise = hf->clockwise != 0;
int cellColumnCount = columnCount - 1;
int cellRowCount = rowCount - 1;
for ( int row = 0; row < rowCount; ++row )
{
for ( int column = 0; column < columnCount; ++column )
{
int index = row * columnCount + column;
b3Vec3 point = b3GetGridPoint( heights, columnCount, scale, row, column );
if ( column < cellColumnCount )
{
bool below = row < cellRowCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
bool above =
row > 0 && materialIndices[( row - 1 ) * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
if ( below && above )
{
b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row - 1, column, true );
b3MarkGridEdge( vertexFlags, index + 1, index, next, point, normal1, normal2, clockwise );
}
else if ( below || above )
{
b3MarkGridBoundary( vertexFlags, index, index + 1 );
}
}
if ( row < cellRowCount )
{
bool right =
column < cellColumnCount && materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE;
bool left = column > 0 && materialIndices[row * cellColumnCount + column - 1] != B3_HEIGHT_FIELD_HOLE;
if ( right && left )
{
b3Vec3 next = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column - 1, true );
b3MarkGridEdge( vertexFlags, index, index + columnCount, point, next, normal1, normal2, clockwise );
}
else if ( right || left )
{
b3MarkGridBoundary( vertexFlags, index, index + columnCount );
}
}
if ( row < cellRowCount && column < cellColumnCount &&
materialIndices[row * cellColumnCount + column] != B3_HEIGHT_FIELD_HOLE )
{
b3Vec3 point21 = b3GetGridPoint( heights, columnCount, scale, row + 1, column );
b3Vec3 point12 = b3GetGridPoint( heights, columnCount, scale, row, column + 1 );
b3Vec3 normal1 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, false );
b3Vec3 normal2 = b3GetGridTriangleNormal( heights, columnCount, scale, row, column, true );
b3MarkGridEdge( vertexFlags, index + columnCount, index + 1, point21, point12, normal1, normal2, clockwise );
}
}
}
uint8_t* vertexBits = (uint8_t*)( (intptr_t)hf + vertexFlagOffset );
for ( int i = 0; i < heightCount; ++i )
{
if ( vertexFlags[i] == 0 )
{
vertexBits[i >> 3] |= (uint8_t)( 1 << ( i & 7 ) );
}
}
b3Free( vertexFlags, heightCount * sizeof( uint8_t ) );
b3Free( decompressedHeights, heightCount * sizeof( float ) );
hf->hash = 0;
@ -474,9 +597,6 @@ b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data )
return hf;
}
_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" );
_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" );
// Decode the four corner vertices of a height field cell into local space.
// Output order matches the index naming used throughout this file:
// corners[0] = (column, row), corners[1] = (column + 1, row),
@ -518,8 +638,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t
B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) );
b3Triangle triangle;
triangle.flags = b3GetHeightFieldFlags( heightField )[triangleIndex];
int columnCount = heightField->columnCount;
int quadIndex = triangleIndex >> 1;
int row = quadIndex / ( columnCount - 1 );
@ -562,15 +680,6 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int t
{
B3_SWAP( triangle.vertices[1], triangle.vertices[2] );
B3_SWAP( triangle.i2, triangle.i3 );
// Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved.
int flags = triangle.flags;
int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 );
int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 );
flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 );
flags |= edge1Bits << 2;
flags |= edge3Bits >> 2;
triangle.flags = flags;
}
return triangle;
@ -585,6 +694,124 @@ int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangle
return b3GetHeightFieldMaterialIndices( heightField )[cellIndex];
}
bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal )
{
B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) );
B3_ASSERT( 0 <= edgeIndex && edgeIndex < 3 );
if ( heightField->clockwise )
{
// Reflected
edgeIndex = 2 - edgeIndex;
}
int cellColumnCount = heightField->columnCount - 1;
int cellRowCount = heightField->rowCount - 1;
int cellIndex = triangleIndex >> 1;
int row = cellIndex / cellColumnCount;
int column = cellIndex - row * cellColumnCount;
bool odd = ( triangleIndex & 1 ) == 1;
// Get the row and column for the neighbor cell.
if ( edgeIndex == 0 )
{
column += odd ? 1 : -1;
}
else if ( edgeIndex == 2 )
{
row += odd ? 1 : -1;
}
if ( row < 0 || row >= cellRowCount || column < 0 || column >= cellColumnCount )
{
return false;
}
int neighborCell = row * cellColumnCount + column;
const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField );
if ( materialIndices[neighborCell] == B3_HEIGHT_FIELD_HOLE )
{
return false;
}
b3Triangle neighbor = b3GetHeightFieldTriangle( heightField, 2 * neighborCell + ( odd ? 0 : 1 ) );
b3Vec3 e1 = b3Sub( neighbor.vertices[1], neighbor.vertices[0] );
b3Vec3 e2 = b3Sub( neighbor.vertices[2], neighbor.vertices[0] );
*normal = b3Normalize( b3Cross( e1, e2 ) );
return true;
}
bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex )
{
B3_ASSERT( 0 <= triangleIndex && triangleIndex < b3GetHeightFieldTriangleCount( heightField ) );
B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 );
if ( heightField->vertexFlagOffset == 0 )
{
return false;
}
if ( heightField->clockwise )
{
vertexIndex = ( 3 - vertexIndex ) % 3;
}
int columnCount = heightField->columnCount;
int cellIndex = triangleIndex >> 1;
int row = cellIndex / ( columnCount - 1 );
int column = cellIndex - row * ( columnCount - 1 );
int offsets[2][3] = {
{ 0, columnCount, 1 },
{ columnCount + 1, 1, columnCount },
};
int index = row * columnCount + column + offsets[triangleIndex & 1][vertexIndex];
const uint8_t* vertexBits = (const uint8_t*)( (intptr_t)heightField + heightField->vertexFlagOffset );
return ( vertexBits[index >> 3] & ( 1 << ( index & 7 ) ) ) != 0;
}
int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] )
{
int columnCount = heightField->columnCount;
int cellColumnCount = columnCount - 1;
int cellRowCount = heightField->rowCount - 1;
int row = vertex / columnCount;
int column = vertex - row * columnCount;
int rowOffsets[4] = { 0, 0, -1, -1 };
int columnOffsets[4] = { 0, -1, 0, -1 };
int firstParity[4] = { 0, 0, 0, 1 };
int lastParity[4] = { 0, 1, 1, 1 };
const uint8_t* materialIndices = b3GetHeightFieldMaterialIndices( heightField );
int count = 0;
for ( int i = 0; i < 4; ++i )
{
int cellRow = row + rowOffsets[i];
int cellColumn = column + columnOffsets[i];
if ( cellRow < 0 || cellRow >= cellRowCount || cellColumn < 0 || cellColumn >= cellColumnCount )
{
continue;
}
int cellIndex = cellRow * cellColumnCount + cellColumn;
if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE )
{
continue;
}
for ( int parity = firstParity[i]; parity <= lastParity[i]; ++parity )
{
triangles[count] = 2 * cellIndex + parity;
count += 1;
}
}
return count;
}
b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform )
{
return b3AABB_Transform( transform, shape->aabb );

View file

@ -7,6 +7,7 @@
#include "algorithm.h"
#include "math_internal.h"
#include "qsort.h"
#include "shape.h"
#include "simd.h"
@ -398,7 +399,8 @@ static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Ou
}
}
static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase )
static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount,
const b3Vec3* vertexBase )
{
b3Vec3 a = vertexBase[index1];
b3Vec3 b = vertexBase[index2];
@ -764,7 +766,8 @@ static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside )
b3QHVertex* maxVertex = NULL;
float maxDistance = minOutside;
for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next )
for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link;
node = node->next )
{
b3QHVertex* vertex = (b3QHVertex*)node;
float distance = b3PlaneSeparation( face->plane, vertex->position );
@ -784,7 +787,8 @@ static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b )
b3QHVertex* maxVertex = NULL;
float maxDistance = b->minOutside;
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next )
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link;
faceNode = faceNode->next )
{
const b3QHFace* face = (const b3QHFace*)faceNode;
if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance )
@ -817,7 +821,8 @@ static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face )
// `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or
// NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored
// anyway since the parent's mark is now DELETE, but skipping saves one iteration).
static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut )
static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge,
b3HorizonFrame* frameOut )
{
face->mark = B3_MARK_DELETE;
b3HullBuilder_DrainConflictList( b, face );
@ -1357,7 +1362,8 @@ static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b )
return false;
}
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next )
for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link;
faceNode = faceNode->next )
{
const b3QHFace* face = (const b3QHFace*)faceNode;
if ( face->edge->face != face )
@ -1427,8 +1433,8 @@ static bool b3HullBuilder_HasHull( const b3HullBuilder* b )
}
// Build the entire hull. Returns true iff the result satisfies Euler's identity.
static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin,
b3Vec3* shiftedPoints )
static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount,
b3Vec3 origin, b3Vec3* shiftedPoints )
{
if ( pointCount < 4 )
{
@ -2195,7 +2201,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
int faceCount = 0;
int edgeCount = 0;
for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next )
for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link;
faceNode = faceNode->next )
{
B3_ASSERT( faceCount < B3_HULL_MAX_COUNT );
@ -3098,20 +3105,20 @@ int b3SimplifyHull2D( b3Point2D* hull, int count1, int target )
return count2;
}
// Sort comparator: by x, then by y. Insertion since n is typically 20 or less.
static void b3Sort2D( b3Point2D* pts, int count )
// Comparator for sorting points for the 2D hull.
static inline bool b3IsPoint2DLess( const b3Point2D* a, const b3Point2D* b )
{
for ( int i = 1; i < count; ++i )
if ( a->p.x != b->p.x )
{
b3Point2D base = pts[i];
int j = i - 1;
while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) )
{
pts[j + 1] = pts[j];
j -= 1;
}
pts[j + 1] = base;
return a->p.x < b->p.x;
}
if ( a->p.y != b->p.y )
{
return a->p.y < b->p.y;
}
return a->originalIndex < b->originalIndex;
}
// Welds points that have been sorted O(n) time
@ -3148,79 +3155,151 @@ static int b3Weld2D( b3Point2D* pts, int count1 )
return count2;
}
// Only merge points that line up exactly.
static int b3MergeExact2D( b3Point2D* pts, int count1 )
{
float separationTol = B3_LINEAR_SLOP;
int count2 = 0;
for ( int i = 0; i < count1; i++ )
{
b3Point2D point = pts[i];
if ( count2 > 0 && pts[count2 - 1].p.x == point.p.x && pts[count2 - 1].p.y == point.p.y )
{
if ( point.separation < pts[count2 - 1].separation - separationTol )
{
pts[count2 - 1] = point;
}
}
else
{
pts[count2] = point;
count2 += 1;
}
}
return count2;
}
// Andrew's monotone chain convex hull algorithm
// hull must have space for 2 * count1 points
int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull )
int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull, int flags )
{
int count1 = count;
if ( count1 <= 0 )
if ( count <= 0 )
{
return 0;
}
if ( count1 == 1 )
#define LESS( i, j ) b3IsPoint2DLess( pts + (int)( i ), pts + (int)( j ) )
#define SWAP( i, j ) \
do \
{ \
b3Point2D swapPoint = pts[(int)( i )]; \
pts[(int)( i )] = pts[(int)( j )]; \
pts[(int)( j )] = swapPoint; \
} \
while ( 0 )
QSORT( count, LESS, SWAP );
#undef LESS
#undef SWAP
int count1;
if ( flags & b3_hull2DExactMerge )
{
hull[0] = pts[0];
return 1;
count1 = b3MergeExact2D( pts, count );
}
else
{
count1 = b3Weld2D( pts, count );
}
b3Sort2D( pts, count1 );
count1 = b3Weld2D( pts, count1 );
if ( count1 == 1 )
{
hull[0] = pts[0];
return 1;
}
if ( count1 == 2 )
{
hull[0] = pts[0];
hull[1] = pts[1];
return 2;
}
bool keepCollinear = ( flags & b3_hull2DKeepCollinear ) != 0;
int count2 = 0;
for ( int i = 0; i < count1; i++ )
if ( count1 <= 2 )
{
while ( count2 >= 2 )
for ( int i = 0; i < count1; i++ )
{
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
if ( area > 0.0f )
{
break;
}
count2 -= 1;
hull[i] = pts[i];
}
B3_VALIDATE( count2 < 2 * count1 );
hull[count2] = pts[i];
count2 += 1;
count2 = count1;
}
int lowerCount = count2 + 1;
for ( int i = count1 - 2; i >= 0; i-- )
else
{
while ( count2 >= lowerCount )
for ( int i = 0; i < count1; i++ )
{
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
if ( area > 0.0f )
while ( count2 >= 2 )
{
break;
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
if ( area > 0.0f || ( keepCollinear && area == 0.0f ) )
{
break;
}
count2 -= 1;
}
count2 -= 1;
B3_VALIDATE( count2 < 2 * count1 );
hull[count2] = pts[i];
count2 += 1;
}
B3_VALIDATE( count2 < 2 * count1 );
hull[count2] = pts[i];
count2 += 1;
int lowerCount = count2 + 1;
int lowerIndex = count2 - 2;
for ( int i = count1 - 2; i >= 0; i-- )
{
if ( lowerIndex >= 0 && hull[lowerIndex].p.x == pts[i].p.x && hull[lowerIndex].p.y == pts[i].p.y )
{
lowerIndex -= 1;
if ( i > 0 )
{
continue;
}
}
while ( count2 >= lowerCount )
{
float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p );
if ( area > 0.0f || ( keepCollinear && area == 0.0f ) )
{
break;
}
count2 -= 1;
}
B3_VALIDATE( count2 < 2 * count1 );
hull[count2] = pts[i];
count2 += 1;
}
B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex );
count2 -= 1;
}
B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex );
return count2 - 1;
B3_VALIDATE( count2 <= count1 );
if ( flags & b3_hull2DIndexOrder )
{
#define LESS( i, j ) ( hull[(int)( i )].originalIndex < hull[(int)( j )].originalIndex )
#define SWAP( i, j ) \
do \
{ \
b3Point2D swapPoint = hull[(int)( i )]; \
hull[(int)( i )] = hull[(int)( j )]; \
hull[(int)( j )] = swapPoint; \
} \
while ( 0 )
QSORT( count2, LESS, SWAP );
#undef LESS
#undef SWAP
}
return count2;
}

View file

@ -51,6 +51,12 @@ b3AxisQuery b3ComputeSeparatingAxisAtWidth( const b3HullData* hullA, const b3Hul
void b3CollideHullsAtWidth( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB,
b3Transform transformBtoA, b3SATCache* cache, int simdWidth );
void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
const b3Capsule* capsule );
void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull,
b3SATCache* cache, bool enableSpeculative );
b3AxisQuery b3ComputeSeparatingAxisW4( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );
b3AxisQuery b3ComputeSeparatingAxisW8( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn );

View file

@ -166,49 +166,6 @@ bool b3IsValidWorldTransform( b3WorldTransform t )
return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q );
}
// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
float b3Atan2( float y, float x )
{
// Added check for (0,0) to match atan2f and avoid NaN
if ( x == 0.0f && y == 0.0f )
{
return 0.0f;
}
float ax = b3AbsFloat( x );
float ay = b3AbsFloat( y );
float mx = b3MaxFloat( ay, ax );
float mn = b3MinFloat( ay, ax );
float a = mn / mx;
// Minimax polynomial approximation to atan(a) on [0,1]
float s = a * a;
float c = s * a;
float q = s * s;
float r = 0.024840285f * q + 0.18681418f;
float t = -0.094097948f * q - 0.33213072f;
r = r * s + t;
r = r * c + a;
// Map to full circle
if ( ay > ax )
{
r = 1.57079637f - r;
}
if ( x < 0 )
{
r = 3.14159274f - r;
}
if ( y < 0 )
{
r = -r;
}
return r;
}
// Approximate cosine and sine for determinism. In my testing cosf and sinf produced
// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust
// this result.

View file

@ -28,7 +28,6 @@ typedef struct b3Triangle
{
b3Vec3 vertices[3];
int i1, i2, i3;
int flags;
} b3Triangle;
typedef struct b3TrianglePoint

1244
src/mesh.c

File diff suppressed because it is too large Load diff

File diff suppressed because it is too large Load diff

View file

@ -129,6 +129,10 @@ float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction );
b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex );
int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex );
bool b3GetHeightFieldNeighborNormal( const b3HeightFieldData* heightField, int triangleIndex, int edgeIndex, b3Vec3* normal );
bool b3IsHeightFieldVertexConvex( const b3HeightFieldData* heightField, int triangleIndex, int vertexIndex );
int b3GetHeightFieldVertexTriangles( const b3HeightFieldData* heightField, int vertex, int triangles[6] );
static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField )
{
int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 );
@ -143,12 +147,10 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount );
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data );
const uint8_t* flags = b3GetMeshFlags( mesh->data );
const b3Vec3* vertices = b3GetMeshVertices( mesh->data );
b3Triangle result;
b3MeshTriangle triangle = triangles[triangleIndex];
uint8_t triangleFlags = flags[triangleIndex];
b3Vec3 scale = mesh->scale;
@ -163,11 +165,6 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
result.i2 = triangle.index3;
result.i3 = triangle.index2;
// mesh is inverted, so concave edges are now convex
result.flags = 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0;
}
else
{
@ -176,12 +173,41 @@ static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleInde
result.i2 = triangle.index2;
result.i3 = triangle.index3;
result.flags = triangleFlags;
}
return result;
}
bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeIndex, b3Vec3* normal );
// Returns false if edge is isolated.
static inline bool b3GetNeighborNormal( const b3Shape* shape, int triangleIndex, int edgeIndex, b3Vec3* normal )
{
if ( shape->type == b3_meshShape )
{
return b3GetMeshNeighborNormal( &shape->mesh, triangleIndex, edgeIndex, normal );
}
B3_ASSERT( shape->type == b3_heightShape );
return b3GetHeightFieldNeighborNormal( shape->heightField, triangleIndex, edgeIndex, normal );
}
int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex );
bool b3IsInMeshVertexRing( const b3Mesh* mesh, int triangleIndex, int vertexIndex, b3Vec3 normal, float tolerance );
int8_t b3QuantizeEdgeAngle( float radians );
static inline bool b3IsConvexVertex( const b3Shape* shape, int triangleIndex, int vertexIndex )
{
if ( shape->type == b3_meshShape )
{
int flags = b3GetMeshVertexFlag( &shape->mesh, triangleIndex, vertexIndex );
return ( flags & ( b3_flatVertex | b3_concaveVertex ) ) == 0;
}
B3_ASSERT( shape->type == b3_heightShape );
return b3IsHeightFieldVertexConvex( shape->heightField, triangleIndex, vertexIndex );
}
bool b3IsValidMesh( const b3MeshData* meshData );
static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB )

View file

@ -29,6 +29,30 @@
#endif
static inline b3FloatW4 b3Atan2W4( b3FloatW4 y, b3FloatW4 x )
{
b3FloatW4 zero = b3ZeroW4();
b3FloatW4 ax = b3AbsW4( x );
b3FloatW4 ay = b3AbsW4( y );
b3FloatW4 mx = b3MaxW4( ay, ax );
b3FloatW4 mn = b3MinW4( ay, ax );
b3FloatW4 a = b3DivW4( mn, mx );
b3FloatW4 s = b3MulW4( a, a );
b3FloatW4 c = b3MulW4( s, a );
b3FloatW4 q = b3MulW4( s, s );
b3FloatW4 r = b3MulAddW4( b3SplatW4( 0.18681418f ), b3SplatW4( 0.024840285f ), q );
b3FloatW4 t = b3SubW4( b3MulW4( b3SplatW4( -0.094097948f ), q ), b3SplatW4( 0.33213072f ) );
r = b3MulAddW4( t, r, s );
r = b3MulAddW4( a, r, c );
r = b3BlendW4( r, b3SubW4( b3SplatW4( 1.57079637f ), r ), b3GreaterThanW4( ay, ax ) );
r = b3BlendW4( r, b3SubW4( b3SplatW4( 3.14159274f ), r ), b3LessThanW4( x, zero ) );
r = b3BlendW4( r, b3NegW4( r ), b3LessThanW4( y, zero ) );
return b3BlendW4( r, zero, b3EqualsW4( b3AddW4( ax, ay ), zero ) );
}
#if !defined( B3_SIMD_SSE2 )
// I don't expect the use case of b3V32 to benefit from Neon code.

View file

@ -14,12 +14,14 @@
#include <stdbool.h>
#include <stddef.h>
// cosine of hull normal versus edge axis tolerance.
#define B3_CROSSING_TOLERANCE 0.1f
typedef struct b3TriangleData
{
b3Vec3 v1, v2, v3;
b3Vec3 e1, e2, e3;
b3Plane plane;
int flags;
} b3TriangleData;
// Indexed by the 3-bit vertex mask
@ -104,7 +106,6 @@ void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const
manifold->normal = normal;
manifold->pointCount = 1;
manifold->feature = closest.feature;
manifold->squaredDistance = squaredDistance;
b3LocalManifoldPoint* mp = manifold->points + 0;
mp->point = contactPoint;
@ -260,8 +261,8 @@ static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices,
};
}
static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
const b3Capsule* capsule )
void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
const b3Capsule* capsule )
{
B3_ASSERT( manifold->pointCount == 0 );
@ -597,15 +598,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 };
// int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 };
#if B3_FORCE_GHOST_COLLISIONS
int triangleFlags = 0xFF;
#else
int triangleFlags = triangle->flags;
#endif
(void)triangleFlags;
b3Vec3 triNormal = triangle->plane.normal;
const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull );
@ -653,7 +645,6 @@ static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* trian
axis = b3Normalize( axis );
float separation = b3Dot( axis, b3Sub( trianglePoints[j], hullPoint ) );
// if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 )
if ( separation > result.separation )
{
// Note: We don't exit early if we find a separating axis here since we want to
@ -748,7 +739,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
b3ClipVertex* clipPoint = input + i;
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET )
{
continue;
}
@ -764,7 +755,7 @@ static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, co
finalPointCount += 1;
}
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
if ( minSeparation > speculativeDistance )
{
// This can occur with a stale SAT cache
@ -864,7 +855,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
b3ClipVertex* clipPoint = input + i;
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
if ( enableSpeculative == false && clipPoint->separation > B3_MESH_REST_OFFSET )
{
continue;
}
@ -881,7 +872,7 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity
finalPointCount += 1;
}
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
if ( minSeparation >= speculativeDistance )
{
// This can happens if the objects move a part while re-using a cached axis
@ -954,11 +945,37 @@ static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capaci
manifold->feature = edgesFeatures[query.indexA];
}
// Triangle face contact wrapper.
void b3BuildTriangleAndHullFaceContact( b3LocalManifold* manifold, int capacity, const b3Vec3* triangle, const b3HullData* hull,
b3SATCache* cache, bool enableSpeculative )
{
manifold->pointCount = 0;
b3TriangleData triangleData = {
.v1 = triangle[0],
.v2 = triangle[1],
.v3 = triangle[2],
.e1 = b3Sub( triangle[1], triangle[0] ),
.e2 = b3Sub( triangle[2], triangle[1] ),
.e3 = b3Sub( triangle[0], triangle[2] ),
.plane = b3MakePlaneFromPoints( triangle[0], triangle[1], triangle[2] ),
};
b3SeparatingAxis query = b3QueryTriangleFace( &triangleData, hull );
b3SATCache faceCache = { 0 };
b3CollideTriangleFace( manifold, capacity, &triangleData, hull, query, &faceCache, enableSpeculative );
if ( manifold->pointCount > 0 )
{
*cache = faceCache;
}
}
b3AtomicInt b3_triangleConvexCalls;
b3AtomicInt b3_triangleCacheHits;
// Triangle is in the local space of the hull for efficiency.
void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags,
void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3,
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )
{
manifold->pointCount = 0;
@ -1003,14 +1020,13 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
.e2 = triangleEdges[1],
.e3 = triangleEdges[2],
.plane = trianglePlane,
.flags = triangleFlags,
};
const b3HullHalfEdge* edges = b3GetHullEdges( hullB );
const b3Plane* hullPlanes = b3GetHullPlanes( hullB );
const b3Vec3* hullPoints = b3GetHullPoints( hullB );
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : B3_MESH_REST_OFFSET;
cache->hit = 1;
// Attempt to use the cache to speed up collision
@ -1082,6 +1098,9 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
return;
}
// Note: it is possible for a hull face to lean into an edge direction while mainting
// a valid cache. But this has never been observed.
// Deep overlap may lead to an invalid cache
// todo confirm
bool isDeep = separation < -2.0f * linearSlop;
@ -1285,14 +1304,29 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
// If a face axis has positive separation there may be no points.
float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );
// The edge axis can win if the hull face axis is significantly aligned with the edge axis.
bool crossesEdge = false;
if ( manifold->pointCount >= 1 && manifold->feature == b3_featureHullFace && edgeQuery.separation > clipSeparation )
{
b3Vec3 edgeAxis = b3Normalize( triangleEdges[edgeQuery.indexA] );
// Is the angle less than ~84 degrees?
crossesEdge = b3AbsFloat( b3Dot( faceQueryB.normal, edgeAxis ) ) > B3_CROSSING_TOLERANCE;
}
if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) || crossesEdge )
{
B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 );
b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA];
b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA];
manifold->pointCount = 0;
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache );
if ( crossesEdge && manifold->pointCount == 0 )
{
b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );
}
}
}
@ -1314,7 +1348,7 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v
b3SimplexCache simplexCache = { 0 };
b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 );
if ( output.distance > 0.0f )
if ( 0.0f < output.distance && output.distance <= speculativeDistance )
{
B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 );