1612 lines
46 KiB
C
1612 lines
46 KiB
C
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "contact.h"
|
|
#include "ctz.h"
|
|
#include "manifold.h"
|
|
#include "physics_world.h"
|
|
#include "qsort.h"
|
|
#include "shape.h"
|
|
#include "simd.h"
|
|
|
|
#include "box3d/types.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#define B3_MAX_POINTS_PER_TRIANGLE 32
|
|
|
|
#if B3_ENABLE_VALIDATION
|
|
static bool b3IsSorted( const int* array, int count )
|
|
{
|
|
for ( int i = 0; i < count - 1; ++i )
|
|
{
|
|
if ( array[i] >= array[i + 1] )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
#define B3_INITIAL_TRIANGLE_CAPACITY 512
|
|
|
|
typedef struct b3TriangleQueryContext
|
|
{
|
|
b3Arena* arena;
|
|
int* indices;
|
|
int capacity;
|
|
int count;
|
|
} b3TriangleQueryContext;
|
|
|
|
static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex,
|
|
void* context )
|
|
{
|
|
B3_UNUSED( a, b, c );
|
|
b3TriangleQueryContext* triangleContext = context;
|
|
if ( triangleContext->count == triangleContext->capacity )
|
|
{
|
|
int capacity = 2 * triangleContext->capacity;
|
|
int* indices = b3Bump( triangleContext->arena, capacity * (int)sizeof( int ) );
|
|
memcpy( indices, triangleContext->indices, triangleContext->count * sizeof( int ) );
|
|
triangleContext->indices = indices;
|
|
triangleContext->capacity = capacity;
|
|
}
|
|
|
|
triangleContext->indices[triangleContext->count] = triangleIndex;
|
|
triangleContext->count += 1;
|
|
return true;
|
|
}
|
|
|
|
static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA,
|
|
const b3AABB* bounds, b3Arena arena )
|
|
{
|
|
B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape );
|
|
|
|
b3MeshContact* meshContact = &contact->meshContact;
|
|
|
|
// If the dynamic body didn't move out of the cached query bounds we are done!
|
|
if ( b3AABB_Contains( meshContact->queryBounds, *bounds ) )
|
|
{
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i )
|
|
{
|
|
B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex &&
|
|
contact->meshContact.triangleCache.data[i].triangleIndex <
|
|
shapeA->mesh.data->triangleCount );
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
// Enlarge to the query bounds to absorb small movement
|
|
float radius = B3_MAX_AABB_MARGIN + B3_SPECULATIVE_DISTANCE;
|
|
b3Vec3 extension = { radius, radius, radius };
|
|
meshContact->queryBounds.lowerBound = b3Sub( bounds->lowerBound, extension );
|
|
meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension );
|
|
|
|
// Query triangles
|
|
b3TriangleQueryContext queryContext = {
|
|
.arena = &arena,
|
|
.indices = b3Bump( &arena, B3_INITIAL_TRIANGLE_CAPACITY * (int)sizeof( int ) ),
|
|
.capacity = B3_INITIAL_TRIANGLE_CAPACITY,
|
|
.count = 0,
|
|
};
|
|
|
|
// Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float,
|
|
// so the demoted mesh transform is the matching float world frame (exact in float mode).
|
|
b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero );
|
|
b3AABB localBounds =
|
|
b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds );
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
b3QueryMesh( &shapeA->mesh, localBounds, b3CollectTriangleIndicesCallback, &queryContext );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( shapeA->type == b3_heightShape );
|
|
b3QueryHeightField( shapeA->heightField, localBounds, b3CollectTriangleIndicesCallback,
|
|
&queryContext );
|
|
}
|
|
|
|
int* triangleIndices = queryContext.indices;
|
|
int triangleCount = queryContext.count;
|
|
|
|
// Triangle indices must be sorted to match caches.
|
|
B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) );
|
|
|
|
// Create new contact cache and match with old one
|
|
b3ContactCache* contactCache = b3Bump( &arena, triangleCount * sizeof( b3ContactCache ) );
|
|
|
|
int index2 = 0;
|
|
for ( int index1 = 0; index1 < triangleCount; ++index1 )
|
|
{
|
|
contactCache[index1] = (b3ContactCache){ 0 };
|
|
|
|
while ( index2 < contact->meshContact.triangleCache.count &&
|
|
contact->meshContact.triangleCache.data[index2].triangleIndex <
|
|
triangleIndices[index1] )
|
|
{
|
|
index2 += 1;
|
|
}
|
|
|
|
if ( index2 < contact->meshContact.triangleCache.count &&
|
|
contact->meshContact.triangleCache.data[index2].triangleIndex ==
|
|
triangleIndices[index1] )
|
|
{
|
|
contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache;
|
|
}
|
|
}
|
|
|
|
// Save new cache
|
|
b3Array_Resize( contact->meshContact.triangleCache, triangleCount );
|
|
for ( int i = 0; i < triangleCount; ++i )
|
|
{
|
|
contact->meshContact.triangleCache.data[i] =
|
|
(b3TriangleCache){ triangleIndices[i], contactCache[i] };
|
|
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex &&
|
|
contact->meshContact.triangleCache.data[i].triangleIndex <
|
|
shapeA->mesh.data->triangleCount );
|
|
}
|
|
}
|
|
}
|
|
|
|
typedef struct b3TentativeTriangle
|
|
{
|
|
float squaredDistance;
|
|
int index;
|
|
} b3TentativeTriangle;
|
|
|
|
#define B3_EMPTY_EDGE_KEY UINT64_MAX
|
|
|
|
// Capacity for edge and vertex keys.
|
|
static int b3GetFoundCapacity( int keyCount )
|
|
{
|
|
int capacity = 16;
|
|
while ( capacity < 2 * keyCount )
|
|
{
|
|
capacity *= 2;
|
|
}
|
|
return capacity;
|
|
}
|
|
|
|
typedef struct b3FoundEdges
|
|
{
|
|
uint64_t* keys;
|
|
int mask;
|
|
} b3FoundEdges;
|
|
|
|
static b3FoundEdges b3CreateFoundEdges( b3Arena* arena, int edgeCount )
|
|
{
|
|
int capacity = b3GetFoundCapacity( edgeCount );
|
|
b3FoundEdges edges;
|
|
edges.keys = b3Bump( arena, capacity * (int)sizeof( uint64_t ) );
|
|
memset( edges.keys, 0xFF, capacity * sizeof( uint64_t ) );
|
|
edges.mask = capacity - 1;
|
|
return edges;
|
|
}
|
|
|
|
static inline uint64_t b3MakeEdgeKey( int vertex1, int vertex2 )
|
|
{
|
|
uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 );
|
|
uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 );
|
|
return i1 << 32 | i2;
|
|
}
|
|
|
|
static inline int b3FindEdgeSlot( const b3FoundEdges* edges, uint64_t key )
|
|
{
|
|
int index = (int)( b3KeyHash( key ) & (uint32_t)edges->mask );
|
|
while ( edges->keys[index] != key && edges->keys[index] != B3_EMPTY_EDGE_KEY )
|
|
{
|
|
index = ( index + 1 ) & edges->mask;
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
static inline bool b3AddEdge( b3FoundEdges* edges, int vertex1, int vertex2 )
|
|
{
|
|
uint64_t key = b3MakeEdgeKey( vertex1, vertex2 );
|
|
int index = b3FindEdgeSlot( edges, key );
|
|
if ( edges->keys[index] == key )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
edges->keys[index] = key;
|
|
return true;
|
|
}
|
|
|
|
static inline bool b3FindEdge( const b3FoundEdges* edges, int vertex1, int vertex2 )
|
|
{
|
|
uint64_t key = b3MakeEdgeKey( vertex1, vertex2 );
|
|
int index = b3FindEdgeSlot( edges, key );
|
|
return edges->keys[index] == key;
|
|
}
|
|
|
|
#if 0
|
|
// Two triangles share an edge iff they share at least two vertex indices.
|
|
static inline bool b3TrianglesShareEdge( int a1, int a2, int a3, int b1, int b2, int b3 )
|
|
{
|
|
int matches = 0;
|
|
matches += ( a1 == b1 || a1 == b2 || a1 == b3 );
|
|
matches += ( a2 == b1 || a2 == b2 || a2 == b3 );
|
|
matches += ( a3 == b1 || a3 == b2 || a3 == b3 );
|
|
return matches >= 2;
|
|
}
|
|
#endif
|
|
|
|
typedef struct b3FoundVertices
|
|
{
|
|
int* keys;
|
|
int mask;
|
|
} b3FoundVertices;
|
|
|
|
static b3FoundVertices b3CreateFoundVertices( b3Arena* arena, int vertexCount )
|
|
{
|
|
int capacity = b3GetFoundCapacity( vertexCount );
|
|
b3FoundVertices vertices;
|
|
vertices.keys = b3Bump( arena, capacity * (int)sizeof( int ) );
|
|
memset( vertices.keys, 0xFF, capacity * sizeof( int ) );
|
|
vertices.mask = capacity - 1;
|
|
return vertices;
|
|
}
|
|
|
|
static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex )
|
|
{
|
|
int index = (int)( b3KeyHash( (uint64_t)vertex ) & (uint32_t)vertices->mask );
|
|
while ( vertices->keys[index] != B3_NULL_INDEX )
|
|
{
|
|
if ( vertices->keys[index] == vertex )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
index = ( index + 1 ) & vertices->mask;
|
|
}
|
|
|
|
vertices->keys[index] = vertex;
|
|
return true;
|
|
}
|
|
|
|
#if B3_MAX_MANIFOLD_POINTS == 4
|
|
|
|
// Returns true if (score, separation) should replace (bestScore, bestSeparation).
|
|
static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore,
|
|
float bestSeparation, float scoreTol,
|
|
float separationTol )
|
|
{
|
|
if ( score > bestScore + scoreTol )
|
|
{
|
|
return true;
|
|
}
|
|
if ( score < bestScore - scoreTol )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Break the tie using separation
|
|
return separation < bestSeparation - separationTol;
|
|
}
|
|
|
|
static int b3CullPoints( b3Point2D* points, int count )
|
|
{
|
|
if ( count <= 1 )
|
|
{
|
|
return count;
|
|
}
|
|
|
|
float tol = 0.25f * B3_LINEAR_SLOP;
|
|
float tolSqr = tol * tol;
|
|
float separationTol = B3_LINEAR_SLOP;
|
|
|
|
b3Point2D finalPoints[4];
|
|
int count1 = count;
|
|
|
|
// Step 1: the two points with the largest distance, ties broken by deepest combined separation
|
|
float bestScore = 0.0f;
|
|
float bestSeparation = FLT_MAX;
|
|
int bestIndex1 = B3_NULL_INDEX;
|
|
int bestIndex2 = B3_NULL_INDEX;
|
|
|
|
for ( int i = 0; i < count1; ++i )
|
|
{
|
|
b3Vec2 p1 = points[i].p;
|
|
for ( int j = i + 1; j < count1; ++j )
|
|
{
|
|
float score = b3DistanceSquared2( p1, points[j].p );
|
|
// Separation sum heuristic
|
|
float separation = points[i].separation + points[j].separation;
|
|
|
|
if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr,
|
|
separationTol ) )
|
|
{
|
|
bestIndex1 = i;
|
|
bestIndex2 = j;
|
|
bestScore = score;
|
|
bestSeparation = separation;
|
|
}
|
|
}
|
|
}
|
|
|
|
if ( bestScore < tolSqr )
|
|
{
|
|
// Choose deepest point
|
|
int deepestIndex = 0;
|
|
for ( int i = 1; i < count1; ++i )
|
|
{
|
|
if ( points[i].separation < points[deepestIndex].separation )
|
|
{
|
|
deepestIndex = i;
|
|
}
|
|
}
|
|
|
|
if ( deepestIndex != 0 )
|
|
{
|
|
points[0] = points[deepestIndex];
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
finalPoints[0] = points[bestIndex1];
|
|
finalPoints[1] = points[bestIndex2];
|
|
|
|
// Cull
|
|
points[bestIndex2] = points[count1 - 1];
|
|
points[bestIndex1] = points[count1 - 2];
|
|
count1 -= 2;
|
|
|
|
if ( count1 == 0 )
|
|
{
|
|
points[0] = finalPoints[0];
|
|
points[1] = finalPoints[1];
|
|
return 2;
|
|
}
|
|
|
|
// First anchor point
|
|
b3Vec2 a = finalPoints[0].p;
|
|
|
|
// Second anchor point
|
|
b3Vec2 b = finalPoints[1].p;
|
|
b3Vec2 ba = b3Sub2( b, a );
|
|
// float length = b3Length2( ba );
|
|
// float areaTol = tol * length;
|
|
|
|
// Step 2: find the point with the maximum triangular area, ties broken by deepest separation
|
|
bestScore = 0.0f;
|
|
bestSeparation = FLT_MAX;
|
|
int bestIndex = B3_NULL_INDEX;
|
|
float bestSignedArea = 0.0f;
|
|
for ( int i = 0; i < count1; ++i )
|
|
{
|
|
b3Vec2 p = points[i].p;
|
|
float signedArea = b3Cross2( ba, b3Sub2( p, a ) );
|
|
float score = b3AbsFloat( signedArea );
|
|
|
|
if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation,
|
|
tolSqr, separationTol ) )
|
|
{
|
|
bestSignedArea = signedArea;
|
|
bestScore = score;
|
|
bestSeparation = points[i].separation;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
|
|
if ( bestIndex == B3_NULL_INDEX )
|
|
{
|
|
// All points collinear
|
|
points[0] = finalPoints[0];
|
|
points[1] = finalPoints[1];
|
|
return 2;
|
|
}
|
|
|
|
// Store best point
|
|
finalPoints[2] = points[bestIndex];
|
|
|
|
if ( count1 == 1 )
|
|
{
|
|
points[0] = finalPoints[0];
|
|
points[1] = finalPoints[1];
|
|
points[2] = finalPoints[2];
|
|
return 3;
|
|
}
|
|
|
|
// Cull
|
|
points[bestIndex] = points[count1 - 1];
|
|
count1 -= 1;
|
|
|
|
// Step 4: get the point that adds the most area outside the current triangle
|
|
|
|
// Third anchor
|
|
b3Vec2 c = finalPoints[2].p;
|
|
|
|
// Ensure CCW ordering
|
|
if ( bestSignedArea < 0.0f )
|
|
{
|
|
B3_SWAP( b, c );
|
|
ba = b3Sub2( b, a );
|
|
}
|
|
|
|
b3Vec2 cb = b3Sub2( c, b );
|
|
b3Vec2 ac = b3Sub2( a, c );
|
|
|
|
bestScore = 0.0f;
|
|
bestSeparation = FLT_MAX;
|
|
bestIndex = B3_NULL_INDEX;
|
|
for ( int i = 0; i < count1; ++i )
|
|
{
|
|
b3Vec2 p = points[i].p;
|
|
float u1 = b3Cross2( b3Sub2( p, a ), ba );
|
|
float u2 = b3Cross2( b3Sub2( p, b ), cb );
|
|
float u3 = b3Cross2( b3Sub2( p, c ), ac );
|
|
float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) );
|
|
|
|
// Use the area tolerance for collinear points and hysteresis
|
|
if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation,
|
|
tolSqr, separationTol ) )
|
|
{
|
|
bestScore = score;
|
|
bestSeparation = points[i].separation;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
|
|
if ( bestIndex == B3_NULL_INDEX )
|
|
{
|
|
// No additional area
|
|
points[0] = finalPoints[0];
|
|
points[1] = finalPoints[1];
|
|
points[2] = finalPoints[2];
|
|
return 3;
|
|
}
|
|
|
|
// Store best point
|
|
finalPoints[3] = points[bestIndex];
|
|
|
|
// Full quad
|
|
points[0] = finalPoints[0];
|
|
points[1] = finalPoints[1];
|
|
points[2] = finalPoints[2];
|
|
points[3] = finalPoints[3];
|
|
return 4;
|
|
}
|
|
|
|
#endif
|
|
|
|
static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena )
|
|
{
|
|
int targetCount = 1;
|
|
if ( count1 <= targetCount )
|
|
{
|
|
return count1;
|
|
}
|
|
|
|
b3Point2D* pts = b3Bump( &arena, count1 * sizeof( b3Point2D ) );
|
|
b3Vec3 u = b3Perp( normal );
|
|
b3Vec3 v = b3Cross( normal, u );
|
|
b3Vec3 origin = points[0].point;
|
|
|
|
for ( int i = 0; i < count1; ++i )
|
|
{
|
|
b3Vec3 d = b3Sub( points[i].point, origin );
|
|
pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) };
|
|
pts[i].separation = points[i].separation;
|
|
pts[i].originalIndex = i;
|
|
}
|
|
|
|
#if B3_MAX_MANIFOLD_POINTS == 4
|
|
b3Point2D* hull = pts;
|
|
int count2 = b3CullPoints( pts, count1 );
|
|
#else
|
|
b3Point2D* hull = b3Bump( &arena, 2 * count1 * sizeof( b3Point2D ) );
|
|
int hullCount = b3Hull2D( pts, count1, hull );
|
|
int count2 = b3SimplifyHull2D( hull, hullCount, B3_MAX_MANIFOLD_POINTS );
|
|
#endif
|
|
B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS );
|
|
|
|
b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS];
|
|
for ( int i = 0; i < count2; ++i )
|
|
{
|
|
int index = hull[i].originalIndex;
|
|
B3_ASSERT( 0 <= index && index < count1 );
|
|
finalPoints[i] = points[index];
|
|
}
|
|
|
|
memcpy( points, finalPoints, count2 * sizeof( b3LocalManifoldPoint ) );
|
|
return count2;
|
|
}
|
|
|
|
typedef struct b3Cluster
|
|
{
|
|
b3Vec3 manifoldNormal;
|
|
b3Vec3 triangleNormal;
|
|
b3LocalManifoldPoint* points;
|
|
int pointCapacity;
|
|
int pointCount;
|
|
} b3Cluster;
|
|
|
|
typedef enum b3NeighborResult
|
|
{
|
|
// Admit for flat edges
|
|
b3_neighborAdmit,
|
|
// Unknown for isolated edges
|
|
b3_neighborUnknown,
|
|
// Skip for convex edges
|
|
b3_neighborSkip,
|
|
// Snap for concave edges
|
|
b3_neighborSnap,
|
|
} b3NeighborResult;
|
|
|
|
#define B3_NEIGHBOR_TOLERANCE 0.0175f
|
|
|
|
static bool b3GetNeighborDirection( const b3Shape* shape, int triangleIndex, const b3Vec3* vertices,
|
|
b3Matrix3 meshToLocal, int edgeIndex, b3Vec3* direction )
|
|
{
|
|
b3Vec3 neighborNormal;
|
|
|
|
bool haveNeighbor = b3GetNeighborNormal( shape, triangleIndex, edgeIndex, &neighborNormal );
|
|
if ( haveNeighbor == false )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
neighborNormal = b3MulMV( meshToLocal, neighborNormal );
|
|
b3Vec3 edge = b3Normalize( b3Sub( vertices[( edgeIndex + 1 ) % 3], vertices[edgeIndex] ) );
|
|
*direction = b3Cross( edge, neighborNormal );
|
|
return true;
|
|
}
|
|
|
|
// Voronoi test of an edge or vertex normal against the neighbor normals.
|
|
static b3NeighborResult b3TestNeighbors( const b3Shape* shape, int triangleIndex,
|
|
const b3Vec3* vertices, b3Matrix3 meshToLocal,
|
|
b3TriangleFeature feature, b3Vec3 normal )
|
|
{
|
|
int edges[2] = { 0, 0 };
|
|
int edgeCount = 1;
|
|
|
|
switch ( feature )
|
|
{
|
|
case b3_featureEdge1:
|
|
edges[0] = 0;
|
|
break;
|
|
|
|
case b3_featureEdge2:
|
|
edges[0] = 1;
|
|
break;
|
|
|
|
case b3_featureEdge3:
|
|
edges[0] = 2;
|
|
break;
|
|
|
|
case b3_featureVertex1:
|
|
edges[0] = 0;
|
|
edges[1] = 2;
|
|
edgeCount = 2;
|
|
break;
|
|
|
|
case b3_featureVertex2:
|
|
edges[0] = 0;
|
|
edges[1] = 1;
|
|
edgeCount = 2;
|
|
break;
|
|
|
|
case b3_featureVertex3:
|
|
edges[0] = 1;
|
|
edges[1] = 2;
|
|
edgeCount = 2;
|
|
break;
|
|
|
|
default:
|
|
return b3_neighborUnknown;
|
|
}
|
|
|
|
b3Vec3 triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] );
|
|
float tolerance = B3_NEIGHBOR_TOLERANCE;
|
|
bool admit = true;
|
|
bool convex = false;
|
|
|
|
for ( int i = 0; i < edgeCount; ++i )
|
|
{
|
|
b3Vec3 direction;
|
|
bool haveNeighbor = b3GetNeighborDirection( shape, triangleIndex, vertices, meshToLocal,
|
|
edges[i], &direction );
|
|
if ( haveNeighbor == false )
|
|
{
|
|
// Isolated edge
|
|
return b3_neighborUnknown;
|
|
}
|
|
|
|
admit = admit && b3Dot( normal, direction ) <= tolerance;
|
|
convex = convex || b3Dot( triangleNormal, direction ) < -tolerance;
|
|
}
|
|
|
|
if ( admit )
|
|
{
|
|
return b3_neighborAdmit;
|
|
}
|
|
|
|
return convex ? b3_neighborSkip : b3_neighborSnap;
|
|
}
|
|
|
|
// Voronoi test of neighbors based on deepest point.
|
|
static b3NeighborResult b3TestHullFaceNeighbors( const b3Shape* shape, int triangleIndex,
|
|
const b3Vec3* vertices, b3Matrix3 meshToLocal,
|
|
const b3LocalManifold* manifold )
|
|
{
|
|
b3Vec3 normal = manifold->normal;
|
|
b3Vec3 triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] );
|
|
float tolerance = B3_NEIGHBOR_TOLERANCE;
|
|
if ( b3Dot( normal, triangleNormal ) > 0.0f &&
|
|
b3Length( b3Cross( normal, triangleNormal ) ) <= tolerance )
|
|
{
|
|
return b3_neighborAdmit;
|
|
}
|
|
|
|
int deepest = 0;
|
|
for ( int i = 1; i < manifold->pointCount; ++i )
|
|
{
|
|
if ( manifold->points[i].separation < manifold->points[deepest].separation )
|
|
{
|
|
deepest = i;
|
|
}
|
|
}
|
|
|
|
// Clip points were moved onto the hull face, step back along the normal to the triangle.
|
|
const b3LocalManifoldPoint* deepestPoint = manifold->points + deepest;
|
|
b3Vec3 point = b3MulSub( deepestPoint->point, deepestPoint->separation, normal );
|
|
|
|
float linearSlop = B3_LINEAR_SLOP;
|
|
int nearEdges[3];
|
|
int nearCount = 0;
|
|
bool inwardTilt = false;
|
|
bool alongEdge = false;
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
b3Vec3 vertex = vertices[i];
|
|
b3Vec3 edge = b3Normalize( b3Sub( vertices[i < 2 ? i + 1 : 0], vertex ) );
|
|
b3Vec3 inward = b3Cross( triangleNormal, edge );
|
|
if ( b3Dot( inward, b3Sub( point, vertex ) ) <= linearSlop )
|
|
{
|
|
nearEdges[nearCount] = i;
|
|
nearCount += 1;
|
|
|
|
// An edge or vertex normal cone tilts outward from the triangle and an edge cone is perpendicular to the edge, a hull face normal need be neither.
|
|
inwardTilt = inwardTilt || b3Dot( normal, inward ) > tolerance;
|
|
alongEdge = alongEdge || b3AbsFloat( b3Dot( normal, edge ) ) > tolerance;
|
|
}
|
|
}
|
|
|
|
b3TriangleFeature feature;
|
|
if ( nearCount == 0 || inwardTilt )
|
|
{
|
|
// A tilted hull face over the triangle interior, the triangle face owns the normal.
|
|
return b3_neighborSnap;
|
|
}
|
|
else if ( nearCount == 1 )
|
|
{
|
|
if ( alongEdge )
|
|
{
|
|
return b3_neighborSnap;
|
|
}
|
|
|
|
b3TriangleFeature edgeFeatures[3] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 };
|
|
feature = edgeFeatures[nearEdges[0]];
|
|
}
|
|
else if ( nearCount == 2 )
|
|
{
|
|
if ( nearEdges[0] == 0 && nearEdges[1] == 1 )
|
|
{
|
|
feature = b3_featureVertex2;
|
|
}
|
|
else if ( nearEdges[0] == 1 )
|
|
{
|
|
feature = b3_featureVertex3;
|
|
}
|
|
else
|
|
{
|
|
feature = b3_featureVertex1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return b3_neighborUnknown;
|
|
}
|
|
|
|
// Never skip a hull face, its normal can miss the whole vertex cone so every triangle would
|
|
// skip, the face contact is empty when the hull is off this triangle.
|
|
b3NeighborResult result =
|
|
b3TestNeighbors( shape, triangleIndex, vertices, meshToLocal, feature, normal );
|
|
return result == b3_neighborSkip ? b3_neighborSnap : result;
|
|
}
|
|
|
|
bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact,
|
|
const b3Shape* shapeA, const uint16_t* materialMap,
|
|
b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB,
|
|
bool isFast, b3Arena arena )
|
|
{
|
|
B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape );
|
|
B3_UNUSED( workerIndex );
|
|
B3_UNUSED( isFast );
|
|
B3_UNUSED( materialMap );
|
|
|
|
b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex );
|
|
|
|
b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena );
|
|
|
|
// Collide with triangles and build manifolds
|
|
b3MeshContact* meshContact = &contact->meshContact;
|
|
int triangleCount = meshContact->triangleCache.count;
|
|
|
|
b3LocalManifold** acceptedManifolds =
|
|
b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) );
|
|
int acceptedManifoldCount = 0;
|
|
b3LocalManifold** tentativeManifolds =
|
|
b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) );
|
|
int tentativeManifoldCount = 0;
|
|
b3TentativeTriangle* tentativeTriangles =
|
|
b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) );
|
|
int tentativeTriangleCount = 0;
|
|
|
|
// This transform converts from mesh frame into the shapeB frame
|
|
b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA );
|
|
b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q );
|
|
float linearSlop = B3_LINEAR_SLOP;
|
|
|
|
// This should push apart shapes after a time of impact event.
|
|
// In the past I've called this `polygon skin`, but PhysX and Unreal
|
|
// call it `rest offset` which seems appropriate in this case.
|
|
// It leads to a small visual gap but seems to improve the quality of mesh
|
|
// collision, especially for hull versus mesh.
|
|
float restOffset = B3_MESH_REST_OFFSET;
|
|
bool enableSpeculative = contact->flags & b3_enableSpeculativePoints;
|
|
|
|
b3TriangleCache* triangleCaches = meshContact->triangleCache.data;
|
|
|
|
const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL;
|
|
float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
|
b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance,
|
|
hullSpeculativeDistance };
|
|
|
|
int* survivorIndices = b3Bump( &arena, triangleCount * sizeof( int ) );
|
|
b3Vec3* survivorVertices = b3Bump( &arena, 3 * triangleCount * sizeof( b3Vec3 ) );
|
|
int survivorCount = 0;
|
|
|
|
const b3MeshTriangle* meshTriangles = NULL;
|
|
const b3Vec3* meshVertices = NULL;
|
|
b3Vec3 meshScale = b3Vec3_one;
|
|
bool meshFlipped = false;
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
meshTriangles = b3GetMeshTriangles( shapeA->mesh.data );
|
|
meshVertices = b3GetMeshVertices( shapeA->mesh.data );
|
|
meshScale = shapeA->mesh.scale;
|
|
meshFlipped = meshScale.x * meshScale.y * meshScale.z < 0.0f;
|
|
}
|
|
|
|
b3FloatW4 cxx = b3SplatW4( relativeMatrix.cx.x );
|
|
b3FloatW4 cxy = b3SplatW4( relativeMatrix.cx.y );
|
|
b3FloatW4 cxz = b3SplatW4( relativeMatrix.cx.z );
|
|
b3FloatW4 cyx = b3SplatW4( relativeMatrix.cy.x );
|
|
b3FloatW4 cyy = b3SplatW4( relativeMatrix.cy.y );
|
|
b3FloatW4 cyz = b3SplatW4( relativeMatrix.cy.z );
|
|
b3FloatW4 czx = b3SplatW4( relativeMatrix.cz.x );
|
|
b3FloatW4 czy = b3SplatW4( relativeMatrix.cz.y );
|
|
b3FloatW4 czz = b3SplatW4( relativeMatrix.cz.z );
|
|
b3FloatW4 px = b3SplatW4( transformAtoB.p.x );
|
|
b3FloatW4 py = b3SplatW4( transformAtoB.p.y );
|
|
b3FloatW4 pz = b3SplatW4( transformAtoB.p.z );
|
|
|
|
b3FloatW4 marginW = b3SplatW4( hullMargin.x );
|
|
b3AABB hullBounds = hullB != NULL ? hullB->aabb : (b3AABB){ 0 };
|
|
b3FloatW4 hullLowerX = b3SplatW4( hullBounds.lowerBound.x );
|
|
b3FloatW4 hullLowerY = b3SplatW4( hullBounds.lowerBound.y );
|
|
b3FloatW4 hullLowerZ = b3SplatW4( hullBounds.lowerBound.z );
|
|
b3FloatW4 hullUpperX = b3SplatW4( hullBounds.upperBound.x );
|
|
b3FloatW4 hullUpperY = b3SplatW4( hullBounds.upperBound.y );
|
|
b3FloatW4 hullUpperZ = b3SplatW4( hullBounds.upperBound.z );
|
|
|
|
for ( int baseIndex = 0; baseIndex < triangleCount; baseIndex += 4 )
|
|
{
|
|
int laneCount = b3MinInt( 4, triangleCount - baseIndex );
|
|
|
|
b3Vec3 lanes[3][4];
|
|
for ( int lane = 0; lane < 4; ++lane )
|
|
{
|
|
int index = baseIndex + b3MinInt( lane, laneCount - 1 );
|
|
int triangleIndex = triangleCaches[index].triangleIndex;
|
|
|
|
if ( meshVertices != NULL )
|
|
{
|
|
b3MeshTriangle meshTriangle = meshTriangles[triangleIndex];
|
|
lanes[0][lane] = b3Mul( meshScale, meshVertices[meshTriangle.index1] );
|
|
lanes[1][lane] =
|
|
b3Mul( meshScale,
|
|
meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2] );
|
|
lanes[2][lane] =
|
|
b3Mul( meshScale,
|
|
meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3] );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( shapeA->type == b3_heightShape );
|
|
b3Triangle triangle =
|
|
b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex );
|
|
lanes[0][lane] = triangle.vertices[0];
|
|
lanes[1][lane] = triangle.vertices[1];
|
|
lanes[2][lane] = triangle.vertices[2];
|
|
}
|
|
}
|
|
|
|
// Transform triangle into the shape frame
|
|
b3FloatW4 vx[3], vy[3], vz[3];
|
|
for ( int k = 0; k < 3; ++k )
|
|
{
|
|
b3FloatW4 ax = b3SetW4( lanes[k][0].x, lanes[k][1].x, lanes[k][2].x, lanes[k][3].x );
|
|
b3FloatW4 ay = b3SetW4( lanes[k][0].y, lanes[k][1].y, lanes[k][2].y, lanes[k][3].y );
|
|
b3FloatW4 az = b3SetW4( lanes[k][0].z, lanes[k][1].z, lanes[k][2].z, lanes[k][3].z );
|
|
|
|
vx[k] = b3AddW4(
|
|
b3AddW4( b3AddW4( b3MulW4( cxx, ax ), b3MulW4( cyx, ay ) ), b3MulW4( czx, az ) ),
|
|
px );
|
|
vy[k] = b3AddW4(
|
|
b3AddW4( b3AddW4( b3MulW4( cxy, ax ), b3MulW4( cyy, ay ) ), b3MulW4( czy, az ) ),
|
|
py );
|
|
vz[k] = b3AddW4(
|
|
b3AddW4( b3AddW4( b3MulW4( cxz, ax ), b3MulW4( cyz, ay ) ), b3MulW4( czz, az ) ),
|
|
pz );
|
|
}
|
|
|
|
uint32_t keepMask = ( 1u << laneCount ) - 1u;
|
|
if ( hullB != NULL )
|
|
{
|
|
b3FloatW4 lowerX = b3SubW4( b3MinW4( vx[0], b3MinW4( vx[1], vx[2] ) ), marginW );
|
|
b3FloatW4 lowerY = b3SubW4( b3MinW4( vy[0], b3MinW4( vy[1], vy[2] ) ), marginW );
|
|
b3FloatW4 lowerZ = b3SubW4( b3MinW4( vz[0], b3MinW4( vz[1], vz[2] ) ), marginW );
|
|
b3FloatW4 upperX = b3AddW4( b3MaxW4( vx[0], b3MaxW4( vx[1], vx[2] ) ), marginW );
|
|
b3FloatW4 upperY = b3AddW4( b3MaxW4( vy[0], b3MaxW4( vy[1], vy[2] ) ), marginW );
|
|
b3FloatW4 upperZ = b3AddW4( b3MaxW4( vz[0], b3MaxW4( vz[1], vz[2] ) ), marginW );
|
|
|
|
b3FloatW4 separated =
|
|
b3OrW4( b3LessThanW4( upperX, hullLowerX ), b3GreaterThanW4( lowerX, hullUpperX ) );
|
|
separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperY, hullLowerY ),
|
|
b3GreaterThanW4( lowerY, hullUpperY ) ) );
|
|
separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperZ, hullLowerZ ),
|
|
b3GreaterThanW4( lowerZ, hullUpperZ ) ) );
|
|
|
|
uint32_t cullMask = keepMask & (uint32_t)b3MoveMaskW4( separated );
|
|
keepMask &= ~cullMask;
|
|
|
|
while ( cullMask != 0 )
|
|
{
|
|
uint32_t lane = b3CTZ32( cullMask );
|
|
cullMask &= cullMask - 1u;
|
|
triangleCaches[baseIndex + (int)lane].cache.satCache = (b3SATCache){ 0 };
|
|
}
|
|
}
|
|
|
|
if ( keepMask == 0 )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
float x[3][4], y[3][4], z[3][4];
|
|
for ( int k = 0; k < 3; ++k )
|
|
{
|
|
b3StoreW4( x[k], vx[k] );
|
|
b3StoreW4( y[k], vy[k] );
|
|
b3StoreW4( z[k], vz[k] );
|
|
}
|
|
|
|
while ( keepMask != 0 )
|
|
{
|
|
uint32_t lane = b3CTZ32( keepMask );
|
|
keepMask &= keepMask - 1u;
|
|
|
|
b3Vec3* survivor = survivorVertices + 3 * survivorCount;
|
|
survivor[0] = (b3Vec3){ x[0][lane], y[0][lane], z[0][lane] };
|
|
survivor[1] = (b3Vec3){ x[1][lane], y[1][lane], z[1][lane] };
|
|
survivor[2] = (b3Vec3){ x[2][lane], y[2][lane], z[2][lane] };
|
|
survivorIndices[survivorCount] = baseIndex + (int)lane;
|
|
survivorCount += 1;
|
|
}
|
|
}
|
|
|
|
// Make room for clip points. Additional capacity for a heavy clip.
|
|
int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * survivorCount + 2 * B3_MAX_CLIP_POINTS;
|
|
|
|
b3LocalManifoldPoint* pointBuffer =
|
|
b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) );
|
|
int totalPointCount = 0;
|
|
|
|
b3LocalManifold* manifoldBuffer = b3Bump( &arena, survivorCount * sizeof( b3LocalManifold ) );
|
|
int manifoldCount = 0;
|
|
|
|
bool validateNeighbors = B3_VALIDATE_NEIGHBOR_NORMALS && B3_FORCE_GHOST_COLLISIONS == 0;
|
|
|
|
for ( int survivorIndex = 0;
|
|
survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity;
|
|
++survivorIndex )
|
|
{
|
|
int index = survivorIndices[survivorIndex];
|
|
int triangleIndex = triangleCaches[index].triangleIndex;
|
|
const b3Vec3* vertices = survivorVertices + 3 * survivorIndex;
|
|
|
|
b3Triangle triangle;
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
triangle = b3GetMeshTriangle( &shapeA->mesh, triangleIndex );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( shapeA->type == b3_heightShape );
|
|
triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex );
|
|
}
|
|
|
|
b3ContactCache* cache = &triangleCaches[index].cache;
|
|
int pointCapacity = pointBufferCapacity - totalPointCount;
|
|
b3LocalManifold* manifold = manifoldBuffer + manifoldCount;
|
|
manifold->points = pointBuffer + totalPointCount;
|
|
manifold->pointCount = 0;
|
|
manifold->triangleFlags = triangle.flags;
|
|
manifold->feature = b3_featureNone;
|
|
|
|
switch ( shapeB->type )
|
|
{
|
|
case b3_capsuleShape:
|
|
b3CollideTriangleAndCapsule( manifold, pointCapacity, vertices, &shapeB->capsule,
|
|
&cache->simplexCache );
|
|
break;
|
|
|
|
case b3_hullShape:
|
|
// Cached edge contact is dangerous at high speed because the hull can rotate around
|
|
// the edge and tunnel through the triangle.
|
|
if ( isFast && cache->satCache.type == b3_edgePairAxis )
|
|
{
|
|
cache->satCache = (b3SATCache){ 0 };
|
|
}
|
|
|
|
b3CollideTriangleAndHull( manifold, pointCapacity, vertices[0], vertices[1],
|
|
vertices[2], triangle.flags, hullB, &cache->satCache,
|
|
enableSpeculative );
|
|
context->satCallCount += 1;
|
|
context->satCacheHitCount += cache->satCache.hit;
|
|
break;
|
|
|
|
case b3_sphereShape:
|
|
b3CollideTriangleAndSphere( manifold, pointCapacity, vertices, &shapeB->sphere );
|
|
break;
|
|
|
|
default:
|
|
B3_ASSERT( false );
|
|
return false;
|
|
}
|
|
|
|
if ( validateNeighbors && manifold->pointCount > 0 &&
|
|
manifold->feature != b3_featureTriangleFace )
|
|
{
|
|
b3NeighborResult result =
|
|
manifold->feature == b3_featureHullFace
|
|
? b3TestHullFaceNeighbors( shapeA, triangleIndex, vertices, relativeMatrix,
|
|
manifold )
|
|
: b3TestNeighbors( shapeA, triangleIndex, vertices, relativeMatrix,
|
|
manifold->feature, manifold->normal );
|
|
|
|
bool deep = false;
|
|
if ( result == b3_neighborSkip || result == b3_neighborSnap )
|
|
{
|
|
float minSeparation = manifold->points[0].separation;
|
|
for ( int i = 1; i < manifold->pointCount; ++i )
|
|
{
|
|
minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation );
|
|
}
|
|
|
|
deep = minSeparation < -2.0f * linearSlop;
|
|
}
|
|
|
|
if ( result == b3_neighborSkip && deep )
|
|
{
|
|
// In deep overlap the owning neighbor may apply backside culling. Experiments
|
|
// show the that in this case the neighbor should snap the normal.
|
|
result = shapeB->type == b3_sphereShape ? b3_neighborUnknown : b3_neighborSnap;
|
|
}
|
|
|
|
b3LocalManifold deepManifold = { 0 };
|
|
b3LocalManifoldPoint deepPoints[B3_MAX_CLIP_POINTS];
|
|
bool keepDeep = deep && result == b3_neighborSnap && shapeB->type == b3_hullShape;
|
|
if ( keepDeep )
|
|
{
|
|
B3_ASSERT( manifold->pointCount <= B3_MAX_CLIP_POINTS );
|
|
deepManifold = *manifold;
|
|
memcpy( deepPoints, manifold->points,
|
|
manifold->pointCount * sizeof( b3LocalManifoldPoint ) );
|
|
}
|
|
|
|
if ( result == b3_neighborSkip || result == b3_neighborSnap )
|
|
{
|
|
// Skip leaves the normal to the neighbor. A sphere off the face cannot snap.
|
|
manifold->pointCount = 0;
|
|
}
|
|
|
|
if ( result == b3_neighborSnap && shapeB->type == b3_capsuleShape )
|
|
{
|
|
// Snap clips the segment to the triangle for a face contact. May clip all points.
|
|
b3Plane plane = b3MakePlaneFromPoints( vertices[0], vertices[1], vertices[2] );
|
|
b3BuildTriangleAndCapsuleFaceContact( manifold, vertices, plane, &shapeB->capsule );
|
|
}
|
|
else if ( result == b3_neighborSnap && shapeB->type == b3_hullShape )
|
|
{
|
|
b3BuildTriangleAndHullFaceContact( manifold, pointCapacity, vertices,
|
|
triangle.flags, hullB, enableSpeculative );
|
|
|
|
if ( keepDeep && manifold->pointCount == 0 )
|
|
{
|
|
// A deep hull off the triangle face keeps its own manifold.
|
|
*manifold = deepManifold;
|
|
memcpy( manifold->points, deepPoints,
|
|
manifold->pointCount * sizeof( b3LocalManifoldPoint ) );
|
|
}
|
|
}
|
|
}
|
|
|
|
int manifoldPointCount = manifold->pointCount;
|
|
|
|
if ( manifoldPointCount > 0 )
|
|
{
|
|
B3_ASSERT( manifold->feature != b3_featureNone );
|
|
|
|
manifoldCount += 1;
|
|
totalPointCount += manifoldPointCount;
|
|
manifold->triangleIndex = triangleIndex;
|
|
manifold->triangleNormal =
|
|
b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] );
|
|
manifold->i1 = triangle.i1;
|
|
manifold->i2 = triangle.i2;
|
|
manifold->i3 = triangle.i3;
|
|
|
|
if ( manifold->feature == b3_featureTriangleFace || B3_FORCE_GHOST_COLLISIONS )
|
|
{
|
|
acceptedManifolds[acceptedManifoldCount++] = manifold;
|
|
}
|
|
else if ( manifold->feature == b3_featureHullFace )
|
|
{
|
|
float cosNormalAngle = b3Dot( manifold->triangleNormal, manifold->normal );
|
|
if ( cosNormalAngle > 0.5f )
|
|
{
|
|
acceptedManifolds[acceptedManifoldCount++] = manifold;
|
|
}
|
|
else
|
|
{
|
|
float minSeparation = manifold->points[0].separation;
|
|
for ( int i = 1; i < manifoldPointCount; ++i )
|
|
{
|
|
minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation );
|
|
}
|
|
|
|
if ( minSeparation < -2.0f * linearSlop )
|
|
{
|
|
// Deep overlap
|
|
acceptedManifolds[acceptedManifoldCount++] = manifold;
|
|
}
|
|
else
|
|
{
|
|
b3TentativeTriangle tentativeTriangle = { .squaredDistance =
|
|
manifold->squaredDistance,
|
|
.index = tentativeManifoldCount };
|
|
tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle;
|
|
tentativeManifolds[tentativeManifoldCount++] = manifold;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3TentativeTriangle tentativeTriangle = {
|
|
.squaredDistance = manifold->squaredDistance, .index = tentativeManifoldCount };
|
|
tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle;
|
|
tentativeManifolds[tentativeManifoldCount++] = manifold;
|
|
}
|
|
}
|
|
}
|
|
|
|
B3_ASSERT( acceptedManifoldCount <= triangleCount );
|
|
B3_ASSERT( tentativeManifoldCount <= triangleCount );
|
|
B3_ASSERT( tentativeTriangleCount <= triangleCount );
|
|
|
|
b3FoundEdges foundEdges = b3CreateFoundEdges( &arena, 3 * manifoldCount );
|
|
b3FoundVertices foundVertices = b3CreateFoundVertices( &arena, 3 * manifoldCount );
|
|
|
|
for ( int i = 0; i < acceptedManifoldCount; ++i )
|
|
{
|
|
b3LocalManifold* m = acceptedManifolds[i];
|
|
(void)b3AddEdge( &foundEdges, m->i1, m->i2 );
|
|
(void)b3AddEdge( &foundEdges, m->i2, m->i3 );
|
|
(void)b3AddEdge( &foundEdges, m->i3, m->i1 );
|
|
(void)b3AddVertex( &foundVertices, m->i1 );
|
|
(void)b3AddVertex( &foundVertices, m->i2 );
|
|
(void)b3AddVertex( &foundVertices, m->i3 );
|
|
}
|
|
|
|
if ( shapeB->type == b3_sphereShape )
|
|
{
|
|
// Sort triangles so the closest triangles are processed first
|
|
{
|
|
#define LESS( i, j ) \
|
|
tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance
|
|
#define SWAP( i, j ) \
|
|
do \
|
|
{ \
|
|
b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \
|
|
tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \
|
|
tentativeTriangles[(int)j] = tmp; \
|
|
} \
|
|
while ( 0 )
|
|
QSORT( tentativeTriangleCount, LESS, SWAP );
|
|
#undef LESS
|
|
#undef SWAP
|
|
}
|
|
|
|
// Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost
|
|
// collisions.
|
|
for ( int i = 0; i < tentativeTriangleCount; ++i )
|
|
{
|
|
b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index];
|
|
|
|
bool addedEdge1 = b3AddEdge( &foundEdges, m->i1, m->i2 );
|
|
bool addedEdge2 = b3AddEdge( &foundEdges, m->i2, m->i3 );
|
|
bool addedEdge3 = b3AddEdge( &foundEdges, m->i3, m->i1 );
|
|
bool addedVertex1 = b3AddVertex( &foundVertices, m->i1 );
|
|
bool addedVertex2 = b3AddVertex( &foundVertices, m->i2 );
|
|
bool addedVertex3 = b3AddVertex( &foundVertices, m->i3 );
|
|
|
|
b3TriangleFeature feature = m->feature;
|
|
bool shouldCollide = false;
|
|
switch ( feature )
|
|
{
|
|
case b3_featureNone:
|
|
case b3_featureTriangleFace:
|
|
B3_ASSERT( false );
|
|
break;
|
|
|
|
case b3_featureEdge1:
|
|
shouldCollide = addedEdge1;
|
|
break;
|
|
|
|
case b3_featureEdge2:
|
|
shouldCollide = addedEdge2;
|
|
break;
|
|
|
|
case b3_featureEdge3:
|
|
shouldCollide = addedEdge3;
|
|
break;
|
|
|
|
case b3_featureVertex1:
|
|
shouldCollide = addedVertex1;
|
|
break;
|
|
|
|
case b3_featureVertex2:
|
|
shouldCollide = addedVertex2;
|
|
break;
|
|
|
|
case b3_featureVertex3:
|
|
shouldCollide = addedVertex3;
|
|
break;
|
|
|
|
default:
|
|
B3_ASSERT( false );
|
|
break;
|
|
}
|
|
|
|
if ( shouldCollide == true )
|
|
{
|
|
acceptedManifolds[acceptedManifoldCount++] = m;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Problem: hull can tunnel if time of impact is at concave edge
|
|
// Example: flat box sliding down a ramp to a flat bottom
|
|
// Solution: only ignore flat edges
|
|
for ( int i = 0; i < tentativeManifoldCount; ++i )
|
|
{
|
|
b3LocalManifold* m = tentativeManifolds[i];
|
|
int triangleFlags = m->triangleFlags;
|
|
|
|
if ( ( triangleFlags & b3_allFlatEdges ) == b3_allFlatEdges )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ( ( triangleFlags & b3_flatEdge1 ) == b3_flatEdge1 )
|
|
{
|
|
if ( b3FindEdge( &foundEdges, m->i1, m->i2 ) )
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if ( ( triangleFlags & b3_flatEdge2 ) == b3_flatEdge2 )
|
|
{
|
|
if ( b3FindEdge( &foundEdges, m->i2, m->i3 ) )
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if ( ( triangleFlags & b3_flatEdge3 ) == b3_flatEdge3 )
|
|
{
|
|
if ( b3FindEdge( &foundEdges, m->i3, m->i1 ) )
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
|
|
acceptedManifolds[acceptedManifoldCount++] = m;
|
|
}
|
|
}
|
|
|
|
B3_ASSERT( acceptedManifoldCount <= triangleCount );
|
|
|
|
if ( acceptedManifoldCount == 0 )
|
|
{
|
|
if ( contact->manifoldCount > 0 )
|
|
{
|
|
b3FreeManifolds( world, contact->manifolds, contact->manifoldCount );
|
|
contact->manifolds = NULL;
|
|
contact->manifoldCount = 0;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) );
|
|
int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) );
|
|
|
|
// Cluster tolerance is tighter than the warm starting manifold matching tolerance. These
|
|
// serve different purposes.
|
|
const float clusterThreshold = 0.996f;
|
|
int clusterCount = 0;
|
|
int clusterPointCount = 0;
|
|
for ( int i = 0; i < acceptedManifoldCount; ++i )
|
|
{
|
|
clusterMemberships[i] = B3_NULL_INDEX;
|
|
|
|
const b3LocalManifold* manifold = acceptedManifolds[i];
|
|
clusterPointCount += manifold->pointCount;
|
|
|
|
// Cluster based on the triangle normal and contact normal.
|
|
// The first cluster found is accepted because the tolerance is tight.
|
|
// todo consider requiring the triangles to be connect by an edge.
|
|
// todo consider looking for the best cluster instead of the first one within tolerance
|
|
// This bool is here to allow quick testing with and without clustering.
|
|
bool allowClustering = true;
|
|
b3Vec3 manifoldNormal = manifold->normal;
|
|
b3Vec3 triangleNormal = manifold->triangleNormal;
|
|
int clusterIndex = B3_NULL_INDEX;
|
|
for ( int j = 0; j < clusterCount && allowClustering; ++j )
|
|
{
|
|
float cosManifoldAngle = b3Dot( clusters[j].manifoldNormal, manifoldNormal );
|
|
float cosTriangleAngle = b3Dot( clusters[j].triangleNormal, triangleNormal );
|
|
if ( cosManifoldAngle <= clusterThreshold || cosTriangleAngle <= clusterThreshold )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
#if 0
|
|
// todo there could be later triangles that create the connection
|
|
// then failure to cluster breaks greedy impulse warm starting
|
|
bool edgeConnected = false;
|
|
|
|
for ( int k = 0; k < i; ++k )
|
|
{
|
|
if ( clusterMemberships[k] != j )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
const b3LocalManifold* other = acceptedManifolds[k];
|
|
if ( b3TrianglesShareEdge( manifold->i1, manifold->i2, manifold->i3, other->i1, other->i2, other->i3 ) )
|
|
{
|
|
edgeConnected = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if ( edgeConnected )
|
|
{
|
|
clusterIndex = j;
|
|
break;
|
|
}
|
|
#else
|
|
|
|
// Found a cluster
|
|
clusterIndex = j;
|
|
break;
|
|
#endif
|
|
}
|
|
|
|
if ( clusterIndex != B3_NULL_INDEX )
|
|
{
|
|
clusterMemberships[i] = clusterIndex;
|
|
clusters[clusterIndex].pointCapacity += manifold->pointCount;
|
|
}
|
|
else
|
|
{
|
|
clusters[clusterCount].manifoldNormal = manifoldNormal;
|
|
clusters[clusterCount].triangleNormal = triangleNormal;
|
|
clusters[clusterCount].pointCapacity = manifold->pointCount;
|
|
clusterMemberships[i] = clusterCount;
|
|
clusterCount += 1;
|
|
}
|
|
}
|
|
|
|
if ( clusterPointCount == 0 )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Setup clusters
|
|
b3LocalManifoldPoint* clusterPoints =
|
|
b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) );
|
|
int pointOffset = 0;
|
|
|
|
for ( int i = 0; i < clusterCount; ++i )
|
|
{
|
|
b3Cluster* cluster = clusters + i;
|
|
cluster->points = clusterPoints + pointOffset;
|
|
cluster->pointCount = 0;
|
|
pointOffset += cluster->pointCapacity;
|
|
}
|
|
|
|
// Populate clusters
|
|
for ( int i = 0; i < acceptedManifoldCount; ++i )
|
|
{
|
|
int clusterIndex = clusterMemberships[i];
|
|
if ( clusterIndex == B3_NULL_INDEX )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
B3_ASSERT( 0 <= clusterIndex && clusterIndex < clusterCount );
|
|
|
|
b3LocalManifold* am = acceptedManifolds[i];
|
|
b3Cluster* cm = clusters + clusterIndex;
|
|
for ( int j = 0; j < am->pointCount; ++j )
|
|
{
|
|
B3_ASSERT( cm->pointCount < cm->pointCapacity );
|
|
b3LocalManifoldPoint* ap = am->points + j;
|
|
b3LocalManifoldPoint* cp = cm->points + cm->pointCount;
|
|
|
|
cp->triangleIndex = am->triangleIndex;
|
|
cp->point = ap->point;
|
|
cp->separation = ap->separation;
|
|
cp->pair = ap->pair;
|
|
cm->pointCount += 1;
|
|
}
|
|
}
|
|
|
|
// Simplify clusters
|
|
for ( int i = 0; i < clusterCount; ++i )
|
|
{
|
|
b3Cluster* cm = clusters + i;
|
|
B3_ASSERT( cm->pointCount == cm->pointCapacity );
|
|
int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena );
|
|
cm->pointCount = reducedCount;
|
|
}
|
|
|
|
// Make a temporary copy of previous manifolds
|
|
int oldManifoldCount = contact->manifoldCount;
|
|
b3Manifold* oldManifolds = NULL;
|
|
if ( oldManifoldCount > 0 )
|
|
{
|
|
oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) );
|
|
memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) );
|
|
}
|
|
|
|
// Resize manifolds if needed
|
|
if ( oldManifoldCount != clusterCount )
|
|
{
|
|
b3FreeManifolds( world, contact->manifolds, contact->manifoldCount );
|
|
contact->manifolds = b3AllocateManifolds( world, clusterCount );
|
|
contact->manifoldCount = (uint16_t)clusterCount;
|
|
}
|
|
else
|
|
{
|
|
// Mem zero manifolds
|
|
memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) );
|
|
}
|
|
|
|
bool* consumed = NULL;
|
|
if ( oldManifoldCount > 0 )
|
|
{
|
|
consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) );
|
|
memset( consumed, 0, oldManifoldCount * sizeof( bool ) );
|
|
}
|
|
|
|
b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );
|
|
b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p );
|
|
|
|
const float normalMatchTolerance = 0.995f;
|
|
for ( int i = 0; i < clusterCount; ++i )
|
|
{
|
|
b3Cluster* cm = clusters + i;
|
|
int pointCount = cm->pointCount;
|
|
B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS );
|
|
|
|
b3Manifold* manifold = contact->manifolds + i;
|
|
manifold->pointCount = pointCount;
|
|
manifold->normal = b3MulMV( matrixB, cm->manifoldNormal );
|
|
|
|
b3Vec3 clusterNormal = b3MulMV( matrixB, cm->manifoldNormal );
|
|
float bestDot = normalMatchTolerance;
|
|
int bestIndex = B3_NULL_INDEX;
|
|
|
|
for ( int j = 0; j < oldManifoldCount; ++j )
|
|
{
|
|
if ( consumed[j] == true )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
float dot = b3Dot( oldManifolds[j].normal, clusterNormal );
|
|
if ( dot > bestDot )
|
|
{
|
|
bestIndex = j;
|
|
bestDot = dot;
|
|
}
|
|
}
|
|
|
|
b3Manifold* matchedManifold = NULL;
|
|
if ( bestIndex != B3_NULL_INDEX )
|
|
{
|
|
matchedManifold = oldManifolds + bestIndex;
|
|
manifold->frictionImpulse = matchedManifold->frictionImpulse;
|
|
manifold->rollingImpulse = matchedManifold->rollingImpulse;
|
|
manifold->twistImpulse = matchedManifold->twistImpulse;
|
|
consumed[bestIndex] = true;
|
|
}
|
|
|
|
for ( int j = 0; j < pointCount; ++j )
|
|
{
|
|
const b3LocalManifoldPoint* source = cm->points + j;
|
|
b3ManifoldPoint* target = manifold->points + j;
|
|
|
|
// Contact points are computed in frame B
|
|
target->anchorB = b3MulMV( matrixB, source->point );
|
|
target->anchorA = b3Add( target->anchorB, offsetA );
|
|
target->separation = source->separation - restOffset;
|
|
target->featureId = b3MakeFeatureId( source->pair );
|
|
target->triangleIndex = source->triangleIndex;
|
|
|
|
// Preserve normal impulse if possible
|
|
if ( matchedManifold != NULL )
|
|
{
|
|
int oldPointCount = matchedManifold->pointCount;
|
|
for ( int k = 0; k < oldPointCount; ++k )
|
|
{
|
|
b3ManifoldPoint* oldPt = matchedManifold->points + k;
|
|
|
|
if ( target->featureId == oldPt->featureId &&
|
|
target->triangleIndex == oldPt->triangleIndex )
|
|
{
|
|
target->normalImpulse = oldPt->normalImpulse;
|
|
target->persisted = true;
|
|
|
|
// claimed
|
|
oldPt->triangleIndex = B3_NULL_INDEX;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
const b3SurfaceMaterial* materialsA = b3GetShapeMaterials( shapeA );
|
|
const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB );
|
|
b3Vec3 tangentVelocityA = b3Vec3_zero;
|
|
|
|
// Update friction and restitution if the mesh has per triangle material
|
|
if ( shapeA->materialCount > 0 )
|
|
{
|
|
float friction = 0.0f;
|
|
float restitution = 0.0f;
|
|
float sampleCount = 0.0f;
|
|
|
|
const uint8_t* materialIndices;
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
materialIndices = b3GetMeshMaterialIndices( shapeA->mesh.data );
|
|
}
|
|
else
|
|
{
|
|
materialIndices = b3GetHeightFieldMaterialIndices( shapeA->heightField );
|
|
}
|
|
|
|
for ( int i = 0; i < clusterCount; ++i )
|
|
{
|
|
b3Manifold* manifold = contact->manifolds + i;
|
|
int pointCount = manifold->pointCount;
|
|
for ( int j = 0; j < pointCount; ++j )
|
|
{
|
|
int triangleIndex = manifold->points[j].triangleIndex;
|
|
int materialIndex;
|
|
if ( shapeA->type == b3_meshShape )
|
|
{
|
|
materialIndex = materialIndices[triangleIndex];
|
|
|
|
if ( materialMap != NULL )
|
|
{
|
|
materialIndex = materialMap[materialIndex];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
materialIndex = materialIndices[triangleIndex >> 1];
|
|
}
|
|
|
|
materialIndex = b3ClampInt( materialIndex, 0, shapeA->materialCount - 1 );
|
|
b3SurfaceMaterial material = materialsA[materialIndex];
|
|
friction +=
|
|
world->frictionCallback( material.friction, material.userMaterialId,
|
|
materialB->friction, materialB->userMaterialId );
|
|
restitution +=
|
|
world->restitutionCallback( material.restitution, material.userMaterialId,
|
|
materialB->restitution, materialB->userMaterialId );
|
|
|
|
tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity );
|
|
|
|
sampleCount += 1.0f;
|
|
}
|
|
}
|
|
|
|
if ( sampleCount > 0.0f )
|
|
{
|
|
float invCount = 1.0f / sampleCount;
|
|
contact->friction = invCount * friction;
|
|
contact->restitution = invCount * restitution;
|
|
tangentVelocityA = b3MulSV( invCount, tangentVelocityA );
|
|
}
|
|
|
|
B3_ASSERT( b3IsValidFloat( contact->friction ) && contact->friction >= 0.0f );
|
|
B3_ASSERT( b3IsValidFloat( contact->restitution ) && contact->restitution >= 0.0f );
|
|
}
|
|
else
|
|
{
|
|
// Keep these updated in case the values on the shapes are modified
|
|
contact->friction =
|
|
world->frictionCallback( materialsA[0].friction, materialsA[0].userMaterialId,
|
|
materialB->friction, materialB->userMaterialId );
|
|
contact->restitution =
|
|
world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId,
|
|
materialB->restitution, materialB->userMaterialId );
|
|
tangentVelocityA = materialsA[0].tangentVelocity;
|
|
}
|
|
|
|
tangentVelocityA = b3RotateVector( xfA.q, tangentVelocityA );
|
|
|
|
float radiusB = 0.0f;
|
|
if ( shapeB->type == b3_sphereShape )
|
|
{
|
|
radiusB = shapeB->sphere.radius;
|
|
}
|
|
else if ( shapeB->type == b3_capsuleShape )
|
|
{
|
|
radiusB = shapeB->capsule.radius;
|
|
}
|
|
else if ( shapeB->type == b3_hullShape )
|
|
{
|
|
radiusB = shapeB->hull->innerRadius;
|
|
}
|
|
|
|
contact->rollingResistance = materialB->rollingResistance * radiusB;
|
|
|
|
b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity );
|
|
contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB );
|
|
return true;
|
|
}
|