1334 lines
40 KiB
C
1334 lines
40 KiB
C
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "algorithm.h"
|
|
#include "contact.h"
|
|
#include "core.h"
|
|
#include "manifold.h"
|
|
#include "shape.h"
|
|
|
|
#include "box3d/base.h"
|
|
#include "box3d/collision.h"
|
|
#include "box3d/constants.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
|
|
typedef struct b3TriangleData
|
|
{
|
|
b3Vec3 v1, v2, v3;
|
|
b3Vec3 e1, e2, e3;
|
|
b3Plane plane;
|
|
int flags;
|
|
} b3TriangleData;
|
|
|
|
// Indexed by the 3-bit vertex mask
|
|
static const b3TriangleFeature s_triangleFeatures[8] = {
|
|
b3_featureNone, // 000 (unreachable)
|
|
b3_featureVertex1, // 001
|
|
b3_featureVertex2, // 010
|
|
b3_featureEdge1, // 011 v1,v2
|
|
b3_featureVertex3, // 100
|
|
b3_featureEdge3, // 101 v1,v3
|
|
b3_featureEdge2, // 110 v2,v3
|
|
b3_featureTriangleFace, // 111
|
|
};
|
|
|
|
static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache )
|
|
{
|
|
int count = cache->count;
|
|
B3_ASSERT( 0 < count && count < 4 );
|
|
|
|
// Bit i set means triangle vertex i participates in the simplex.
|
|
int mask = 0;
|
|
for ( int i = 0; i < count; ++i )
|
|
{
|
|
B3_ASSERT( cache->indexA[i] < 3 );
|
|
mask |= 1 << cache->indexA[i];
|
|
}
|
|
|
|
return s_triangleFeatures[mask];
|
|
}
|
|
|
|
void b3CollideTriangleAndSphere( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Sphere* sphereB )
|
|
{
|
|
manifold->pointCount = 0;
|
|
|
|
if ( capacity == 0 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
b3Vec3 center = sphereB->center;
|
|
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
|
|
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
|
|
|
|
float offset = b3PlaneSeparation( plane, center );
|
|
if ( offset < 0.0f )
|
|
{
|
|
// Cull back side collision
|
|
return;
|
|
}
|
|
|
|
float radius = sphereB->radius;
|
|
|
|
// Closest point on triangle to sphere center
|
|
b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center );
|
|
|
|
// Test separating axis
|
|
float squaredDistance = b3DistanceSquared( closest.point, center );
|
|
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
|
|
float maxDistance = radius + speculativeDistance;
|
|
if ( squaredDistance > maxDistance * maxDistance )
|
|
{
|
|
return;
|
|
}
|
|
|
|
float distance = sqrtf( squaredDistance );
|
|
|
|
// Normal points from triangle to sphere
|
|
b3Vec3 normal;
|
|
if ( distance * distance > 1000.0f * FLT_MIN )
|
|
{
|
|
normal = b3MulSV( 1.0f / distance, b3Sub( center, closest.point ) );
|
|
}
|
|
else
|
|
{
|
|
normal = b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) );
|
|
}
|
|
|
|
// contact point mid-way
|
|
// p = 0.5 * (c + q) - 0.5 * r * n
|
|
b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( radius, normal ) ), closest.point ) );
|
|
|
|
manifold->normal = normal;
|
|
manifold->pointCount = 1;
|
|
manifold->feature = closest.feature;
|
|
manifold->squaredDistance = squaredDistance;
|
|
|
|
b3LocalManifoldPoint* mp = manifold->points + 0;
|
|
mp->point = contactPoint;
|
|
mp->separation = distance - radius;
|
|
mp->pair = b3FeaturePair_single;
|
|
}
|
|
|
|
static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3* points, b3Plane plane )
|
|
{
|
|
b3Vec3 vertex1 = points[2];
|
|
for ( int i = 0; i < 3; ++i )
|
|
{
|
|
b3Vec3 vertex2 = points[i];
|
|
b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) );
|
|
b3Vec3 binormal = b3Cross( tangent, plane.normal );
|
|
|
|
b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 );
|
|
|
|
int vertexCount = 0;
|
|
b3ClipVertex p1 = segment[0];
|
|
b3ClipVertex p2 = segment[1];
|
|
|
|
float distance1 = b3PlaneSeparation( clipPlane, p1.position );
|
|
float distance2 = b3PlaneSeparation( clipPlane, p2.position );
|
|
|
|
// If the points are behind the plane
|
|
if ( distance1 <= 0.0f )
|
|
{
|
|
segment[vertexCount++] = p1;
|
|
}
|
|
if ( distance2 <= 0.0f )
|
|
{
|
|
segment[vertexCount++] = p2;
|
|
}
|
|
|
|
// If the points are on different sides of the plane
|
|
if ( ( distance1 > 0.0f ) != ( distance2 > 0.0f ) )
|
|
{
|
|
// Find intersection point of edge and plane
|
|
float t = distance1 / ( distance1 - distance2 );
|
|
segment[vertexCount].position = b3Lerp( p1.position, p2.position, t );
|
|
segment[vertexCount].pair = distance1 > 0.0f ? p1.pair : p2.pair;
|
|
vertexCount++;
|
|
}
|
|
|
|
if ( vertexCount != 2 )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
vertex1 = vertex2;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static b3SeparatingAxis b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule )
|
|
{
|
|
float separation1 = b3PlaneSeparation( plane, capsule->center1 );
|
|
float separation2 = b3PlaneSeparation( plane, capsule->center2 );
|
|
|
|
if ( separation1 < separation2 )
|
|
{
|
|
return (b3SeparatingAxis){
|
|
.normal = plane.normal,
|
|
.separation = separation1,
|
|
.indexA = 0,
|
|
.indexB = 0,
|
|
};
|
|
}
|
|
|
|
return (b3SeparatingAxis){
|
|
.normal = plane.normal,
|
|
.separation = separation2,
|
|
.indexA = 0,
|
|
.indexB = 1,
|
|
};
|
|
}
|
|
|
|
static b3SeparatingAxis b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, b3Plane plane, const b3Capsule* capsule )
|
|
{
|
|
// Work in the local space of the capsule
|
|
b3Vec3 p1 = capsule->center1;
|
|
b3Vec3 p2 = capsule->center2;
|
|
b3Vec3 capsuleEdge = b3Sub( p2, p1 );
|
|
|
|
// Find axis of minimum penetration
|
|
b3Vec3 maxNormal = b3Vec3_zero;
|
|
float maxSeparation = -FLT_MAX;
|
|
int maxIndex1 = B3_NULL_INDEX;
|
|
int maxIndex2 = B3_NULL_INDEX;
|
|
float squaredTolerance = 0.005f * 0.005f;
|
|
|
|
int edgeIndex = 2;
|
|
float a = b3Dot( capsuleEdge, plane.normal );
|
|
b3Vec3 v1 = vertices[2];
|
|
for ( int index = 0; index < 3; ++index )
|
|
{
|
|
b3Vec3 v2 = vertices[index];
|
|
b3Vec3 triangleEdge = b3Sub( v2, v1 );
|
|
b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );
|
|
|
|
// Pretend the triangle edge embeds a zero area face with a side normal. This
|
|
// provides a way to find an edge-edge normal that points outward from
|
|
// the triangle.
|
|
float b = b3Dot( capsuleEdge, sideNormal );
|
|
|
|
// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.
|
|
if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )
|
|
{
|
|
v1 = v2;
|
|
edgeIndex = index;
|
|
continue;
|
|
}
|
|
|
|
// Similar to hull vs hull (b3QueryEdgeDirections)
|
|
b3Vec3 axis;
|
|
if ( a * b <= 0.0f )
|
|
{
|
|
float t = b / ( b - a );
|
|
axis = b3Lerp( sideNormal, plane.normal, t );
|
|
}
|
|
else
|
|
{
|
|
float t = b / ( a + b );
|
|
axis = b3Lerp( sideNormal, b3Neg( plane.normal ), t );
|
|
}
|
|
|
|
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
|
axis = b3Normalize( axis );
|
|
float separation = b3Dot( axis, b3Sub( p1, v1 ) );
|
|
|
|
if ( separation > maxSeparation )
|
|
{
|
|
// Note: We don't exit early if we find a separating axis here since we want to
|
|
// find the best one for caching and account for the convex radius later.
|
|
maxNormal = axis;
|
|
maxSeparation = separation;
|
|
maxIndex1 = edgeIndex;
|
|
maxIndex2 = 0;
|
|
}
|
|
|
|
v1 = v2;
|
|
edgeIndex = index;
|
|
}
|
|
|
|
// Save result
|
|
return (b3SeparatingAxis){
|
|
.normal = maxNormal,
|
|
.separation = maxSeparation,
|
|
.indexA = maxIndex1,
|
|
.indexB = maxIndex2,
|
|
};
|
|
}
|
|
|
|
static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
|
const b3Capsule* capsule )
|
|
{
|
|
B3_ASSERT( manifold->pointCount == 0 );
|
|
|
|
b3ClipVertex segment[2];
|
|
segment[0].position = capsule->center1;
|
|
segment[0].separation = 0.0f;
|
|
segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 );
|
|
segment[1].position = capsule->center2;
|
|
segment[1].separation = 0.0f;
|
|
segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 );
|
|
|
|
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangle, plane );
|
|
if ( havePoints == false )
|
|
{
|
|
return;
|
|
}
|
|
|
|
float radius = capsule->radius;
|
|
float distance1 = b3PlaneSeparation( plane, segment[0].position );
|
|
float distance2 = b3PlaneSeparation( plane, segment[1].position );
|
|
|
|
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
|
|
if ( distance1 > speculativeDistance + radius && distance2 > speculativeDistance + radius )
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Average points. Half-way between capsule bottom and triangle plane.
|
|
b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( distance1 + capsule->radius ), plane.normal );
|
|
b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( distance2 + capsule->radius ), plane.normal );
|
|
|
|
manifold->normal = plane.normal;
|
|
manifold->feature = b3_featureTriangleFace;
|
|
manifold->pointCount = 2;
|
|
|
|
b3LocalManifoldPoint* pt = manifold->points + 0;
|
|
pt->point = point1;
|
|
pt->separation = distance1 - capsule->radius;
|
|
pt->pair = segment[0].pair;
|
|
|
|
pt = manifold->points + 1;
|
|
pt->point = point2;
|
|
pt->separation = distance2 - capsule->radius;
|
|
pt->pair = segment[1].pair;
|
|
}
|
|
|
|
static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane,
|
|
const b3Capsule* capsule, b3SeparatingAxis query )
|
|
{
|
|
B3_ASSERT( 0 <= query.indexA && query.indexA < 3 );
|
|
|
|
b3Vec3 p1 = capsule->center1;
|
|
b3Vec3 p2 = capsule->center2;
|
|
b3Vec3 capsuleEdge = b3Sub( p2, p1 );
|
|
|
|
const b3Vec3* vs = triangle;
|
|
|
|
b3Vec3 v1 = vs[query.indexA];
|
|
b3Vec3 v2 = vs[( query.indexA + 1 ) % 3];
|
|
b3Vec3 triangleEdge = b3Sub( v2, v1 );
|
|
|
|
b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) );
|
|
|
|
// Pretend the triangle edge embeds a zero area face with a side normal. This
|
|
// provides a way to find an edge-edge normal that points outward from
|
|
// the triangle.
|
|
float a = b3Dot( capsuleEdge, plane.normal );
|
|
float b = b3Dot( capsuleEdge, sideNormal );
|
|
|
|
// Is the capsule edge parallel to the triangle edge? If so, face contact can handle it.
|
|
float squaredTolerance = 0.005f * 0.005f;
|
|
if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) )
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Similar to hull vs hull (b3QueryEdgeDirections)
|
|
b3Vec3 normal = query.normal;
|
|
b3SegmentDistanceResult result = b3LineDistance( v1, triangleEdge, p1, capsuleEdge );
|
|
|
|
if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 )
|
|
{
|
|
// closest point beyond end points
|
|
return;
|
|
}
|
|
|
|
b3Vec3 point = b3Lerp( result.point1, b3MulSub( result.point2, capsule->radius, normal ), 0.5f );
|
|
float separation = b3Dot( normal, b3Sub( p1, v1 ) );
|
|
|
|
manifold->normal = normal;
|
|
manifold->pointCount = 1;
|
|
|
|
b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 };
|
|
manifold->feature = edgesFeatures[query.indexA];
|
|
|
|
b3LocalManifoldPoint* pt = manifold->points + 0;
|
|
pt->point = point;
|
|
pt->separation = separation - capsule->radius;
|
|
pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB );
|
|
}
|
|
|
|
void b3CollideTriangleAndCapsule( b3LocalManifold* manifold, int capacity, const b3Vec3* triangleA, const b3Capsule* capsuleB,
|
|
b3SimplexCache* cache )
|
|
{
|
|
manifold->pointCount = 0;
|
|
|
|
if ( capacity < 2 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
b3Vec3 v1 = triangleA[0], v2 = triangleA[1], v3 = triangleA[2];
|
|
b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 );
|
|
b3Vec3 capsuleCenter = b3Lerp( capsuleB->center1, capsuleB->center2, 0.5f );
|
|
|
|
float offset = b3PlaneSeparation( plane, capsuleCenter );
|
|
if ( offset < 0.0f )
|
|
{
|
|
// Cull back side collision
|
|
return;
|
|
}
|
|
|
|
b3DistanceInput distanceInput;
|
|
distanceInput.proxyA = (b3ShapeProxy){ triangleA, 3, 0.0f };
|
|
distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f };
|
|
distanceInput.transform = b3Transform_identity;
|
|
distanceInput.useRadii = false;
|
|
|
|
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 );
|
|
float speculativeDistance = B3_SPECULATIVE_DISTANCE;
|
|
float radius = capsuleB->radius;
|
|
if ( distanceOutput.distance > radius + speculativeDistance )
|
|
{
|
|
// Shapes are separated, persist the cache
|
|
return;
|
|
}
|
|
|
|
if ( distanceOutput.distance > 100.0f * FLT_EPSILON )
|
|
{
|
|
// Shallow penetration
|
|
b3Vec3 delta = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) );
|
|
|
|
// Try to create two contact points if closest points difference is nearly parallel to face normal
|
|
const float kTolerance = 0.2f;
|
|
float cosAngle = b3AbsFloat( b3Dot( plane.normal, delta ) );
|
|
if ( cosAngle > kTolerance )
|
|
{
|
|
// Clip capsule segment against side planes of reference face
|
|
b3ClipVertex segment[2];
|
|
segment[0].position = capsuleB->center1;
|
|
segment[0].separation = 0.0f;
|
|
segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 );
|
|
segment[1].position = capsuleB->center2;
|
|
segment[1].separation = 0.0f;
|
|
segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 );
|
|
|
|
bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleA, plane );
|
|
|
|
if ( havePoints == true )
|
|
{
|
|
float distance1 = b3PlaneSeparation( plane, segment[0].position );
|
|
float distance2 = b3PlaneSeparation( plane, segment[1].position );
|
|
|
|
b3Vec3 normal = plane.normal;
|
|
b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal );
|
|
b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal );
|
|
|
|
// Normal points from triangle to capsule
|
|
manifold->normal = normal;
|
|
manifold->feature = b3_featureTriangleFace;
|
|
manifold->pointCount = 2;
|
|
|
|
b3LocalManifoldPoint* mp = manifold->points + 0;
|
|
mp->point = point1;
|
|
mp->separation = distance1 - radius;
|
|
mp->pair = segment[0].pair;
|
|
|
|
mp = manifold->points + 1;
|
|
mp->point = point2;
|
|
mp->separation = distance2 - radius;
|
|
mp->pair = segment[1].pair;
|
|
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Create contact from closest points.
|
|
b3Vec3 point = b3Lerp( distanceOutput.pointA, b3MulSub( distanceOutput.pointB, radius, delta ), 0.5f );
|
|
|
|
// Normal points from triangle to capsule.
|
|
manifold->normal = delta;
|
|
manifold->pointCount = 1;
|
|
manifold->feature = b3GetTriangleFeature( cache );
|
|
|
|
b3LocalManifoldPoint* mp = manifold->points + 0;
|
|
mp->point = point;
|
|
mp->separation = distanceOutput.distance - radius;
|
|
mp->pair = b3FeaturePair_single;
|
|
|
|
return;
|
|
}
|
|
|
|
// Deep penetration
|
|
|
|
b3SeparatingAxis faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleB );
|
|
if ( faceQuery.separation > radius )
|
|
{
|
|
// Shapes are separated. Should be impossible for a reasonable capsule radius.
|
|
return;
|
|
}
|
|
|
|
b3SeparatingAxis edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleA, plane, capsuleB );
|
|
if ( edgeQuery.separation > radius )
|
|
{
|
|
// Shapes are separated. Should be impossible for a reasonable capsule radius.
|
|
return;
|
|
}
|
|
|
|
// Create face contact
|
|
float faceSeparation = faceQuery.separation - radius;
|
|
b3BuildTriangleAndCapsuleFaceContact( manifold, triangleA, plane, capsuleB );
|
|
B3_VALIDATE( manifold->pointCount == 0 || manifold->pointCount == 2 );
|
|
if ( manifold->pointCount == 2 )
|
|
{
|
|
// This becomes the clipped separation.
|
|
faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation );
|
|
}
|
|
|
|
if ( edgeQuery.indexA == B3_NULL_INDEX )
|
|
{
|
|
// No valid edge contact.
|
|
return;
|
|
}
|
|
|
|
// Face contact can be empty if it does not realize the axis of minimum penetration.
|
|
// Create edge contact if face contact fails or edge contact is significantly better!
|
|
float linearSlop = B3_LINEAR_SLOP;
|
|
float edgeSeparation = edgeQuery.separation - radius;
|
|
if ( manifold->pointCount == 0 || edgeSeparation > faceSeparation + linearSlop )
|
|
{
|
|
// Edge contact
|
|
b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleA, plane, capsuleB, edgeQuery );
|
|
}
|
|
}
|
|
|
|
static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction )
|
|
{
|
|
int index = 0;
|
|
float distance = b3Dot( points[0], direction );
|
|
|
|
float d = b3Dot( points[1], direction );
|
|
if ( d > distance )
|
|
{
|
|
distance = d;
|
|
index = 1;
|
|
}
|
|
|
|
d = b3Dot( points[2], direction );
|
|
if ( d > distance )
|
|
{
|
|
return 2;
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
static b3SeparatingAxis b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull )
|
|
{
|
|
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
|
b3Plane plane = triangle->plane;
|
|
b3Vec3 normal = b3Neg( plane.normal );
|
|
int vertexIndex = b3FindHullSupportVertex( hull, normal );
|
|
b3Vec3 support = hullPoints[vertexIndex];
|
|
float separation = b3PlaneSeparation( plane, support );
|
|
|
|
return (b3SeparatingAxis){
|
|
.normal = plane.normal,
|
|
.separation = separation,
|
|
.indexA = 0,
|
|
.indexB = vertexIndex,
|
|
.type = b3_faceAxisA,
|
|
};
|
|
}
|
|
|
|
static b3SeparatingAxis b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull )
|
|
{
|
|
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
|
|
int faceCount = hull->faceCount;
|
|
|
|
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
|
|
|
|
b3Vec3 maxNormal = b3Vec3_zero;
|
|
float maxFaceSeparation = -INFINITY;
|
|
int maxFaceIndex = B3_NULL_INDEX;
|
|
int maxVertexIndex = B3_NULL_INDEX;
|
|
|
|
for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex )
|
|
{
|
|
b3Plane plane = hullPlanes[faceIndex];
|
|
|
|
int vertexIndex = b3GetTriangleSupport( trianglePoints, b3Neg( plane.normal ) );
|
|
b3Vec3 support = trianglePoints[vertexIndex];
|
|
float separation = b3PlaneSeparation( plane, support );
|
|
if ( separation > maxFaceSeparation )
|
|
{
|
|
maxNormal = plane.normal;
|
|
maxFaceSeparation = separation;
|
|
maxFaceIndex = faceIndex;
|
|
maxVertexIndex = vertexIndex;
|
|
}
|
|
}
|
|
|
|
// Normal points from triangle to hull
|
|
return (b3SeparatingAxis){
|
|
.normal = b3Neg( maxNormal ),
|
|
.separation = maxFaceSeparation,
|
|
.indexA = maxVertexIndex,
|
|
.indexB = maxFaceIndex,
|
|
.type = b3_faceAxisB,
|
|
};
|
|
}
|
|
|
|
// A: hull, B: triangle
|
|
static b3SeparatingAxis b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull )
|
|
{
|
|
b3SeparatingAxis result = {
|
|
.normal = b3Vec3_zero,
|
|
.separation = -INFINITY,
|
|
.indexA = B3_NULL_INDEX,
|
|
.indexB = B3_NULL_INDEX,
|
|
.type = b3_edgePairAxis,
|
|
};
|
|
|
|
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 );
|
|
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
|
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
|
|
int edgeCount = hull->edgeCount;
|
|
float squaredTolerance = 0.005f * 0.005f;
|
|
|
|
for ( int i = 0; i < edgeCount; i += 2 )
|
|
{
|
|
const b3HullHalfEdge* edge = hullEdges + i;
|
|
const b3HullHalfEdge* twin = hullEdges + i + 1;
|
|
B3_ASSERT( edge->twin == i + 1 && twin->twin == i );
|
|
|
|
b3Vec3 hullPoint = hullPoints[edge->origin];
|
|
b3Vec3 hullEdge = b3Sub( hullPoints[twin->origin], hullPoint );
|
|
|
|
b3Vec3 hullNormal1 = hullPlanes[edge->face].normal;
|
|
b3Vec3 hullNormal2 = hullPlanes[twin->face].normal;
|
|
|
|
for ( int j = 0; j < 3; ++j )
|
|
{
|
|
b3Vec3 triEdge = triangleEdges[j];
|
|
|
|
float cab = b3Dot( hullNormal1, triEdge );
|
|
float dab = b3Dot( hullNormal2, triEdge );
|
|
float bcd = b3Dot( triNormal, hullEdge );
|
|
if ( cab * dab >= 0.0f || cab * bcd <= 0.0f )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
|
if ( b3MaxFloat( cab * cab, dab * dab ) < squaredTolerance * b3LengthSquared( triEdge ) )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Similar to hull vs hull (b3QueryEdgeDirections)
|
|
// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0
|
|
// Normal points out of hull by construction.
|
|
float t = cab / ( cab - dab );
|
|
b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );
|
|
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
|
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
|
|
// find the best one for caching.
|
|
// Flip normal to point from triangle to hull.
|
|
result.normal = b3Neg( axis );
|
|
result.separation = separation;
|
|
result.indexA = j;
|
|
result.indexB = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
|
|
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
|
|
{
|
|
B3_VALIDATE( query.type == b3_faceAxisB );
|
|
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
|
|
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->faceCount );
|
|
|
|
manifold->pointCount = 0;
|
|
|
|
const b3HullFace* hullFaces = b3GetHullFaces( hull );
|
|
const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull );
|
|
const b3Plane* hullPlanes = b3GetHullPlanes( hull );
|
|
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
|
|
|
// Reference hull face
|
|
b3Plane refPlane = hullPlanes[query.indexB];
|
|
|
|
// Build clip polygon from triangle face (the incident face)
|
|
b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS];
|
|
|
|
b3Vec3 v1 = triangle->v1;
|
|
b3Vec3 v2 = triangle->v2;
|
|
b3Vec3 v3 = triangle->v3;
|
|
buffer1[0].position = v1;
|
|
buffer1[0].separation = b3PlaneSeparation( refPlane, v1 );
|
|
buffer1[0].pair = b3MakeFeaturePair( b3_featureShapeB, 2, b3_featureShapeB, 0 );
|
|
buffer1[1].position = v2;
|
|
buffer1[1].separation = b3PlaneSeparation( refPlane, v2 );
|
|
buffer1[1].pair = b3MakeFeaturePair( b3_featureShapeB, 0, b3_featureShapeB, 1 );
|
|
buffer1[2].position = v3;
|
|
buffer1[2].separation = b3PlaneSeparation( refPlane, v3 );
|
|
buffer1[2].pair = b3MakeFeaturePair( b3_featureShapeB, 1, b3_featureShapeB, 2 );
|
|
int pointCount = 3;
|
|
|
|
// Clip triangle face against side planes of reference face
|
|
b3ClipVertex* input = buffer1;
|
|
b3ClipVertex* output = buffer2;
|
|
|
|
const b3HullFace* face = hullFaces + query.indexB;
|
|
int edgeIndex = face->edge;
|
|
|
|
do
|
|
{
|
|
const b3HullHalfEdge* edge = hullEdges + edgeIndex;
|
|
int nextEdgeIndex = edge->next;
|
|
const b3HullHalfEdge* next = hullEdges + nextEdgeIndex;
|
|
b3Vec3 vertex1 = hullPoints[edge->origin];
|
|
b3Vec3 vertex2 = hullPoints[next->origin];
|
|
b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) );
|
|
b3Vec3 binormal = b3Cross( tangent, refPlane.normal );
|
|
|
|
b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 );
|
|
|
|
pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane );
|
|
B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS );
|
|
|
|
if ( pointCount < 3 )
|
|
{
|
|
// Using a stale cache
|
|
*cache = (b3SATCache){ 0 };
|
|
return query.separation;
|
|
}
|
|
|
|
// Swap buffers, output becomes input for the next clipping plane
|
|
B3_SWAP( output, input );
|
|
edgeIndex = nextEdgeIndex;
|
|
}
|
|
while ( edgeIndex != face->edge );
|
|
|
|
pointCount = b3MinInt( pointCount, pointCapacity );
|
|
float minSeparation = FLT_MAX;
|
|
int finalPointCount = 0;
|
|
|
|
for ( int i = 0; i < pointCount; ++i )
|
|
{
|
|
b3ClipVertex* clipPoint = input + i;
|
|
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
|
|
|
|
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Move point onto hull face improved culling
|
|
b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal );
|
|
|
|
b3LocalManifoldPoint* pt = manifold->points + finalPointCount;
|
|
pt->point = point;
|
|
pt->separation = clipPoint->separation;
|
|
pt->pair = b3FlipPair( clipPoint->pair );
|
|
|
|
finalPointCount += 1;
|
|
}
|
|
|
|
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
|
if ( minSeparation > speculativeDistance )
|
|
{
|
|
// This can occur with a stale SAT cache
|
|
manifold->pointCount = 0;
|
|
*cache = (b3SATCache){ 0 };
|
|
return minSeparation;
|
|
}
|
|
|
|
manifold->pointCount = finalPointCount;
|
|
manifold->normal = b3Neg( refPlane.normal );
|
|
manifold->feature = b3_featureHullFace;
|
|
|
|
// Save cache
|
|
cache->separation = minSeparation;
|
|
cache->type = b3_faceAxisB;
|
|
cache->indexA = (uint8_t)query.indexA;
|
|
cache->indexB = (uint8_t)query.indexB;
|
|
return minSeparation;
|
|
}
|
|
|
|
static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle,
|
|
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache, bool enableSpeculative )
|
|
{
|
|
B3_VALIDATE( query.type == b3_faceAxisA );
|
|
B3_VALIDATE( query.indexA == 0 );
|
|
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->vertexCount );
|
|
B3_VALIDATE( manifold->pointCount == 0 );
|
|
|
|
const b3HullFace* hullFaces = b3GetHullFaces( hull );
|
|
const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull );
|
|
const b3Vec3* hullPoints = b3GetHullPoints( hull );
|
|
|
|
// Find incident face
|
|
b3Plane refPlane = triangle->plane;
|
|
|
|
int incFace = b3FindIncidentFace( hull, refPlane.normal, query.indexB );
|
|
|
|
// Build clip polygon from incident face
|
|
b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS];
|
|
int pointCount = 0;
|
|
const b3HullFace* face = hullFaces + incFace;
|
|
int hullEdgeIndex = face->edge;
|
|
|
|
do
|
|
{
|
|
const b3HullHalfEdge* edge = hullEdges + hullEdgeIndex;
|
|
|
|
int nextEdgeIndex = edge->next;
|
|
const b3HullHalfEdge* next = hullEdges + nextEdgeIndex;
|
|
|
|
b3Vec3 hullPoint = hullPoints[next->origin];
|
|
buffer1[pointCount].position = hullPoint;
|
|
buffer1[pointCount].separation = b3PlaneSeparation( refPlane, hullPoint );
|
|
buffer1[pointCount].pair = b3MakeFeaturePair( b3_featureShapeB, hullEdgeIndex, b3_featureShapeB, nextEdgeIndex );
|
|
|
|
pointCount += 1;
|
|
hullEdgeIndex = nextEdgeIndex;
|
|
}
|
|
while ( hullEdgeIndex != face->edge && pointCount < 2 * B3_MAX_CLIP_POINTS );
|
|
|
|
B3_ASSERT( pointCount >= 3 );
|
|
|
|
// Clip incident face against side planes of reference face (triangle)
|
|
b3ClipVertex* input = buffer1;
|
|
b3ClipVertex* output = buffer2;
|
|
|
|
b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 };
|
|
b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 };
|
|
|
|
for ( int i = 0; i < 3 && pointCount > 0; ++i )
|
|
{
|
|
b3Vec3 sideNormal = b3Cross( triangleEdges[i], refPlane.normal );
|
|
sideNormal = b3Normalize( sideNormal );
|
|
|
|
b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( sideNormal, trianglePoints[i] );
|
|
|
|
pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, i, refPlane );
|
|
B3_ASSERT( pointCount <= 2 * B3_MAX_CLIP_POINTS );
|
|
|
|
B3_SWAP( output, input );
|
|
}
|
|
|
|
if ( pointCount == 0 )
|
|
{
|
|
// Triangle face clipped away. Invalidate cache.
|
|
*cache = (b3SATCache){ 0 };
|
|
return FLT_MAX;
|
|
}
|
|
|
|
pointCount = b3MinInt( pointCount, pointCapacity );
|
|
|
|
float minSeparation = FLT_MAX;
|
|
|
|
int finalPointCount = 0;
|
|
for ( int i = 0; i < pointCount; ++i )
|
|
{
|
|
b3ClipVertex* clipPoint = input + i;
|
|
minSeparation = b3MinFloat( minSeparation, clipPoint->separation );
|
|
|
|
if ( enableSpeculative == false && clipPoint->separation > 0.0f )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Move point onto triangle surface for improved culling
|
|
// b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal );
|
|
b3Vec3 point = clipPoint->position;
|
|
|
|
b3LocalManifoldPoint* pt = manifold->points + finalPointCount;
|
|
pt->point = point;
|
|
pt->separation = clipPoint->separation;
|
|
pt->pair = clipPoint->pair;
|
|
|
|
finalPointCount += 1;
|
|
}
|
|
|
|
float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
|
|
if ( minSeparation >= speculativeDistance )
|
|
{
|
|
// This can happens if the objects move a part while re-using a cached axis
|
|
*cache = (b3SATCache){ 0 };
|
|
return minSeparation;
|
|
}
|
|
|
|
manifold->pointCount = finalPointCount;
|
|
manifold->normal = refPlane.normal;
|
|
manifold->feature = b3_featureTriangleFace;
|
|
|
|
// Save cache
|
|
cache->separation = minSeparation;
|
|
cache->type = b3_faceAxisA;
|
|
cache->indexA = (uint8_t)query.indexA;
|
|
cache->indexB = (uint8_t)query.indexB;
|
|
return minSeparation;
|
|
}
|
|
|
|
static void b3CollideTriangleAndHullEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge,
|
|
const b3HullData* hull, b3SeparatingAxis query, b3SATCache* cache )
|
|
{
|
|
B3_VALIDATE( query.type == b3_edgePairAxis );
|
|
B3_VALIDATE( 0 <= query.indexA && query.indexA < 3 );
|
|
B3_VALIDATE( 0 <= query.indexB && query.indexB < hull->edgeCount );
|
|
B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE );
|
|
|
|
b3Vec3 pA = trianglePoint;
|
|
b3Vec3 eA = triangleEdge;
|
|
|
|
const b3HullHalfEdge* edgesB = b3GetHullEdges( hull );
|
|
const b3Vec3* pointsB = b3GetHullPoints( hull );
|
|
const b3HullHalfEdge* edgeB = edgesB + query.indexB;
|
|
const b3HullHalfEdge* twinB = edgesB + edgeB->twin;
|
|
b3Vec3 pB = pointsB[edgeB->origin];
|
|
b3Vec3 qB = pointsB[twinB->origin];
|
|
b3Vec3 eB = b3Sub( qB, pB );
|
|
|
|
// Get the closest points between the infinite edge lines
|
|
b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB );
|
|
|
|
// Is one of the closest points outside of the associated edge segment?
|
|
if ( capacity == 0 || result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f ||
|
|
1.0f < result.fraction2 )
|
|
{
|
|
// Invalid edge pair, no points generated
|
|
B3_ASSERT( manifold->pointCount == 0 );
|
|
*cache = (b3SATCache){ 0 };
|
|
return;
|
|
}
|
|
|
|
float separation = b3Dot( query.normal, b3Sub( pB, pA ) );
|
|
b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) );
|
|
|
|
b3LocalManifoldPoint* pt = manifold->points + 0;
|
|
pt->point = point;
|
|
pt->separation = separation;
|
|
pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB );
|
|
|
|
// Save cache
|
|
cache->separation = separation;
|
|
cache->type = b3_edgePairAxis;
|
|
cache->indexA = (uint8_t)query.indexA;
|
|
cache->indexB = (uint8_t)query.indexB;
|
|
|
|
manifold->normal = query.normal;
|
|
manifold->pointCount = 1;
|
|
|
|
b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 };
|
|
manifold->feature = edgesFeatures[query.indexA];
|
|
}
|
|
|
|
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,
|
|
const b3HullData* hullB, b3SATCache* cache, bool enableSpeculative )
|
|
{
|
|
manifold->pointCount = 0;
|
|
manifold->feature = b3_featureNone;
|
|
|
|
if ( capacity < 4 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 );
|
|
float linearSlop = B3_LINEAR_SLOP;
|
|
|
|
float offset = b3PlaneSeparation( trianglePlane, hullB->center );
|
|
if ( cache->type == b3_backsideAxis )
|
|
{
|
|
// Use hysteresis to avoid jitter on wavy meshes
|
|
if ( b3AbsFloat( cache->separation - offset ) < linearSlop )
|
|
{
|
|
return;
|
|
}
|
|
|
|
cache->type = b3_invalidAxis;
|
|
}
|
|
|
|
if ( offset < -linearSlop )
|
|
{
|
|
// Cull back side collision. Cache offset to add hysteresis.
|
|
cache->type = b3_backsideAxis;
|
|
cache->separation = offset;
|
|
return;
|
|
}
|
|
|
|
b3Vec3 trianglePoints[] = { v1, v2, v3 };
|
|
b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) };
|
|
|
|
b3TriangleData triangle = {
|
|
.v1 = v1,
|
|
.v2 = v2,
|
|
.v3 = v3,
|
|
.e1 = triangleEdges[0],
|
|
.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;
|
|
cache->hit = 1;
|
|
|
|
// Attempt to use the cache to speed up collision
|
|
switch ( cache->type )
|
|
{
|
|
case b3_faceAxisA:
|
|
{
|
|
B3_ASSERT( cache->indexA == 0 );
|
|
|
|
int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( trianglePlane.normal ) );
|
|
b3Vec3 support = hullPoints[vertexIndex];
|
|
float separation = b3PlaneSeparation( trianglePlane, support );
|
|
if ( separation > speculativeDistance )
|
|
{
|
|
// Cache hit, shapes are separated
|
|
return;
|
|
}
|
|
|
|
b3SeparatingAxis faceQuery;
|
|
faceQuery.normal = trianglePlane.normal;
|
|
faceQuery.separation = separation;
|
|
faceQuery.indexA = cache->indexA;
|
|
faceQuery.indexB = vertexIndex;
|
|
faceQuery.type = b3_faceAxisA;
|
|
|
|
// Read cache but don't modify it
|
|
b3SATCache localCache = *cache;
|
|
float clippedSeparation =
|
|
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
|
|
|
|
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
|
|
{
|
|
// Cache hit, contact points generated
|
|
return;
|
|
}
|
|
|
|
// Invalidate cache and fall through
|
|
manifold->pointCount = 0;
|
|
*cache = (b3SATCache){ 0 };
|
|
}
|
|
break;
|
|
|
|
case b3_faceAxisB:
|
|
{
|
|
B3_ASSERT( cache->indexB < hullB->faceCount );
|
|
|
|
b3Plane plane = hullPlanes[cache->indexB];
|
|
|
|
// Get triangle support point
|
|
int vertexIndex = 0;
|
|
float distance = -b3Dot( v1, plane.normal );
|
|
for ( int i = 1; i < 3; ++i )
|
|
{
|
|
float d = -b3Dot( trianglePoints[i], plane.normal );
|
|
if ( d > distance )
|
|
{
|
|
distance = d;
|
|
vertexIndex = i;
|
|
}
|
|
}
|
|
|
|
b3Vec3 support = trianglePoints[vertexIndex];
|
|
|
|
// Separation of triangle support point with hull plane
|
|
float separation = b3PlaneSeparation( plane, support );
|
|
if ( separation > speculativeDistance )
|
|
{
|
|
// Cache hit, shapes are separated
|
|
return;
|
|
}
|
|
|
|
// Deep overlap may lead to an invalid cache
|
|
// todo confirm
|
|
bool isDeep = separation < -2.0f * linearSlop;
|
|
|
|
// Don't persist deep cache or allow separation to change too much
|
|
if ( isDeep == false )
|
|
{
|
|
// Try to rebuild contact from last features
|
|
b3SeparatingAxis faceQuery;
|
|
faceQuery.normal = b3Neg( plane.normal );
|
|
faceQuery.separation = separation;
|
|
faceQuery.indexA = vertexIndex;
|
|
faceQuery.indexB = cache->indexB;
|
|
faceQuery.type = b3_faceAxisB;
|
|
|
|
// Read cache but don't modify it
|
|
b3SATCache localCache = *cache;
|
|
float clippedSeparation =
|
|
b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQuery, &localCache, enableSpeculative );
|
|
|
|
// Cache reuse is only successful if it creates contact points and the clipped separation didn't change much.
|
|
if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop )
|
|
{
|
|
// Cache hit, contact points generated
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Invalidate cache and fall through
|
|
manifold->pointCount = 0;
|
|
*cache = (b3SATCache){ 0 };
|
|
}
|
|
break;
|
|
|
|
case b3_edgePairAxis:
|
|
{
|
|
B3_ASSERT( cache->indexA < 3 );
|
|
int indexA = cache->indexA;
|
|
|
|
b3Vec3 triPoint = trianglePoints[indexA];
|
|
b3Vec3 triEdge = triangleEdges[indexA];
|
|
|
|
B3_ASSERT( cache->indexB < hullB->edgeCount - 1 );
|
|
int indexB = cache->indexB;
|
|
|
|
const b3HullHalfEdge* edge2 = edges + indexB;
|
|
const b3HullHalfEdge* twin2 = edges + indexB + 1;
|
|
B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB );
|
|
|
|
b3Vec3 hullPoint = hullPoints[edge2->origin];
|
|
b3Vec3 hullEdge = b3Sub( hullPoints[twin2->origin], hullPoint );
|
|
b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal;
|
|
b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal;
|
|
|
|
// Confirm the edge pair is still a Minkowski face.
|
|
// See "Collision Detection of Convex Polyhedra Based on Duality Transformation"
|
|
// Simplified for triangle versus hull.
|
|
float cab = b3Dot( hullNormal1, triEdge );
|
|
float dab = b3Dot( hullNormal2, triEdge );
|
|
float bcd = b3Dot( trianglePlane.normal, hullEdge );
|
|
|
|
if ( cab * dab < 0.0f && cab * bcd > 0.0f )
|
|
{
|
|
float squaredTolerance = 0.005f * 0.005f;
|
|
|
|
// Avoid nearly parallel edges that may lead to invalid separation values at the noise floor.
|
|
if ( b3MaxFloat( cab * cab, dab * dab ) >= squaredTolerance * b3LengthSquared( triEdge ) )
|
|
{
|
|
// Similar to hull vs hull (b3QueryEdgeDirections)
|
|
// dot(hullNormal1 + t * (hullNormal2 - hullNormal1), triEdge) = 0
|
|
// Normal points out of hull by construction.
|
|
float t = cab / ( cab - dab );
|
|
b3Vec3 axis = b3Lerp( hullNormal1, hullNormal2, t );
|
|
B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN );
|
|
axis = b3Normalize( axis );
|
|
float separation = b3Dot( axis, b3Sub( triPoint, hullPoint ) );
|
|
if ( separation > speculativeDistance )
|
|
{
|
|
// Cache hit, shapes are separated
|
|
return;
|
|
}
|
|
|
|
if ( b3AbsFloat( cache->separation - separation ) < linearSlop )
|
|
{
|
|
// Try to rebuild contact from last features
|
|
// Flip normal to point from triangle to hull
|
|
b3SeparatingAxis edgeQuery;
|
|
edgeQuery.normal = b3Neg( axis );
|
|
edgeQuery.indexA = indexA;
|
|
edgeQuery.indexB = indexB;
|
|
edgeQuery.separation = separation;
|
|
edgeQuery.type = b3_edgePairAxis;
|
|
|
|
// Read cache but don't modify it
|
|
b3SATCache localCache = *cache;
|
|
b3CollideTriangleAndHullEdges( manifold, capacity, triPoint, triEdge, hullB, edgeQuery, &localCache );
|
|
|
|
if ( manifold->pointCount > 0 )
|
|
{
|
|
// Cache hit, contact point generated
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Invalidate cache and fall through
|
|
*cache = (b3SATCache){ 0 };
|
|
}
|
|
break;
|
|
|
|
// This case is for testing
|
|
case b3_manualFaceAxisA:
|
|
{
|
|
b3SeparatingAxis query = b3QueryTriangleFace( &triangle, hullB );
|
|
b3CollideTriangleFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
|
|
return;
|
|
}
|
|
|
|
// This case is for testing
|
|
case b3_manualFaceAxisB:
|
|
{
|
|
b3SeparatingAxis query = b3QueryHullFace( &triangle, hullB );
|
|
b3CollideHullFace( manifold, capacity, &triangle, hullB, query, cache, enableSpeculative );
|
|
return;
|
|
}
|
|
|
|
// This case is for testing
|
|
case b3_manualEdgePairAxis:
|
|
{
|
|
b3SeparatingAxis query = b3QueryTriangleAndHullEdges( &triangle, hullB );
|
|
if ( query.indexA != B3_NULL_INDEX )
|
|
{
|
|
b3Vec3 trianglePoint = trianglePoints[query.indexA];
|
|
b3Vec3 triangleEdge = triangleEdges[query.indexA];
|
|
b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, query, cache );
|
|
}
|
|
return;
|
|
}
|
|
|
|
default:
|
|
B3_ASSERT( cache->type == b3_invalidAxis );
|
|
break;
|
|
}
|
|
|
|
// Cache miss
|
|
cache->hit = 0;
|
|
|
|
// Find axis of minimum penetration
|
|
b3SeparatingAxis faceQueryA = b3QueryTriangleFace( &triangle, hullB );
|
|
if ( faceQueryA.separation > speculativeDistance )
|
|
{
|
|
// Separating axis found
|
|
cache->separation = faceQueryA.separation;
|
|
cache->type = b3_faceAxisA;
|
|
cache->indexA = (uint8_t)faceQueryA.indexA;
|
|
cache->indexB = (uint8_t)faceQueryA.indexB;
|
|
return;
|
|
}
|
|
|
|
b3SeparatingAxis faceQueryB = b3QueryHullFace( &triangle, hullB );
|
|
if ( faceQueryB.separation > speculativeDistance )
|
|
{
|
|
// Separating axis found
|
|
cache->separation = faceQueryB.separation;
|
|
cache->type = b3_faceAxisB;
|
|
cache->indexA = (uint8_t)faceQueryB.indexA;
|
|
cache->indexB = (uint8_t)faceQueryB.indexB;
|
|
return;
|
|
}
|
|
|
|
b3SeparatingAxis edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullB );
|
|
if ( edgeQuery.separation > speculativeDistance )
|
|
{
|
|
// Separating axis found
|
|
cache->separation = edgeQuery.separation;
|
|
cache->type = b3_edgePairAxis;
|
|
cache->indexA = (uint8_t)edgeQuery.indexA;
|
|
cache->indexB = (uint8_t)edgeQuery.indexB;
|
|
return;
|
|
}
|
|
|
|
float clipSeparation;
|
|
|
|
// Don't admit a hull face significantly opposed to the triangle face.
|
|
// Need a tolerance to avoid ghost collisions.
|
|
bool pushingDown = b3Dot( faceQueryB.normal, trianglePlane.normal ) < -0.25f;
|
|
if ( faceQueryB.separation >= faceQueryA.separation && pushingDown == false )
|
|
{
|
|
clipSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative );
|
|
}
|
|
else
|
|
{
|
|
clipSeparation = b3CollideTriangleFace( manifold, capacity, &triangle, hullB, faceQueryA, cache, enableSpeculative );
|
|
}
|
|
|
|
// Does an edge axis exist?
|
|
if ( edgeQuery.indexA != B3_NULL_INDEX )
|
|
{
|
|
// When axes are aligned the edge separation can be garbage.
|
|
// If a face axis has positive separation there may be no points.
|
|
float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation );
|
|
|
|
if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) ||
|
|
( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) )
|
|
{
|
|
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 );
|
|
}
|
|
}
|
|
|
|
// Using the speculative distance means that sometimes there are no valid contact points from SAT.
|
|
// In this fall back to GJK. This is important to prevent tunneling in rare cases.
|
|
if ( manifold->pointCount == 0 )
|
|
{
|
|
b3Vec3 triangleB[] = { v1, v2, v3 };
|
|
b3DistanceInput input = { 0 };
|
|
input.proxyA = (b3ShapeProxy){
|
|
.points = triangleB,
|
|
.count = 3,
|
|
.radius = 0.0f,
|
|
};
|
|
input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullB->vertexCount, .radius = 0.0f };
|
|
input.transform = b3Transform_identity;
|
|
input.useRadii = false;
|
|
|
|
b3SimplexCache simplexCache = { 0 };
|
|
b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 );
|
|
|
|
if ( output.distance > 0.0f )
|
|
{
|
|
B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 );
|
|
|
|
manifold->pointCount = 1;
|
|
manifold->feature = b3GetTriangleFeature( &simplexCache );
|
|
manifold->normal = output.normal;
|
|
manifold->points[0].point = output.pointB;
|
|
manifold->points[0].separation = output.distance;
|
|
|
|
// This feature pair not accurate but maybe it doesn't matter
|
|
manifold->points[0].pair = b3FeaturePair_single;
|
|
}
|
|
|
|
// No way to cache this scenario
|
|
*cache = (b3SATCache){ 0 };
|
|
}
|
|
}
|