From aaa795e34f64035b5e827665a8a8f0e74f1282d3 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Fri, 10 Jul 2026 16:14:53 -0700 Subject: [PATCH] Edge edge optimization (#63) From #54 --- samples/sample_issues.cpp | 50 +- samples/sample_manifold.cpp | 12 +- samples/shaders/generated/capsule.glsl.h | 2 +- samples/shaders/generated/cube.glsl.h | 2 +- samples/shaders/generated/geom.glsl.h | 2 +- samples/shaders/generated/sphere.glsl.h | 2 +- src/convex_manifold.c | 260 ++--- src/manifold.c | 52 - src/manifold.h | 2 +- src/triangle_manifold.c | 227 +++-- test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_manifold.c | 1102 ++++++++++++++++++++++ 13 files changed, 1416 insertions(+), 300 deletions(-) create mode 100644 test/test_manifold.c diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index c8947b7..ec6edbd 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -590,7 +590,7 @@ public: { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; - bodyDef.position = { -m_walkRange, m_bodyHalfHeight + 0.1f, 0.0f }; + bodyDef.position = { -m_walkRangeX, m_bodyHalfHeight + 0.1f, 0.0f }; bodyDef.motionLocks.angularX = true; bodyDef.motionLocks.angularY = true; bodyDef.motionLocks.angularZ = true; @@ -613,8 +613,10 @@ public: b3CreateHullShape( m_characterId, &shapeDef, &box.base ); } - m_walkDirection = 1.0f; - m_walkSpeed = 350.0f * SRC; // s&box run speed + m_walkDirectionX = 1.0f; + m_walkDirectionZ = 1.0f; + m_walkSpeedX = 350.0f * SRC; // s&box run speed + m_walkSpeedZ = 20.0f * SRC; m_launchCount = 0; m_maxLaunchSpeed = 0.0f; m_launchMarkerCount = 0; @@ -850,18 +852,27 @@ public: // Drive the character with pure velocity control: keep the solver's vertical velocity, // set the horizontal velocity. This is how the s&box player controller moves. b3Pos position = b3Body_GetPosition( m_characterId ); - if ( position.x > m_walkRange ) + if ( position.x > m_walkRangeX ) { - m_walkDirection = -1.0f; + m_walkDirectionX = -1.0f; } - else if ( position.x < -m_walkRange ) + else if ( position.x < -m_walkRangeX ) { - m_walkDirection = 1.0f; + m_walkDirectionX = 1.0f; + } + + if ( position.z > m_walkRangeZ ) + { + m_walkDirectionZ = -1.0f; + } + else if ( position.z < -m_walkRangeZ ) + { + m_walkDirectionZ= 1.0f; } b3Vec3 velocity = b3Body_GetLinearVelocity( m_characterId ); - velocity.x = m_walkDirection * m_walkSpeed; - velocity.z = 0.0f; + velocity.x = m_walkDirectionX * m_walkSpeedX; + velocity.z = m_walkDirectionZ * m_walkSpeedZ; b3Body_SetLinearVelocity( m_characterId, velocity ); Sample::Step(); @@ -905,10 +916,16 @@ public: bool DrawControls() override { - float speedU = m_walkSpeed / SRC; - if ( ImGui::SliderFloat( "Walk Speed (inch/s)", &speedU, 100.0f, 400.0f, "%.0f" ) ) + float speedUX = m_walkSpeedX / SRC; + if ( ImGui::SliderFloat( "Walk Speed X (inch/s)", &speedUX, 100.0f, 400.0f, "%.0f" ) ) { - m_walkSpeed = speedU * SRC; + m_walkSpeedX = speedUX * SRC; + } + + float speedUZ = m_walkSpeedZ / SRC; + if ( ImGui::SliderFloat( "Walk Speed Z (inch/s)", &speedUZ, 10.0f, 100.0f, "%.0f" ) ) + { + m_walkSpeedZ = speedUZ * SRC; } if ( ImGui::Button( "Reset Counters" ) ) @@ -927,14 +944,17 @@ public: return new SBoxGhostCollisions( context ); } - static constexpr float m_walkRange = 3.5f; // turn around beyond +/- this x (meters) + static constexpr float m_walkRangeX = 3.5f; // turn around beyond +/- this x (meters) + static constexpr float m_walkRangeZ = 0.5f; // turn around beyond +/- this x (meters) static constexpr float m_launchThreshold = 0.5f; // upward m/s counted as a ghost launch (~20 inch/s) static constexpr int m_markerCapacity = 64; b3MeshData* m_chunkMesh[2] = {}; b3BodyId m_characterId; - float m_walkDirection; - float m_walkSpeed; + float m_walkDirectionX; + float m_walkDirectionZ; + float m_walkSpeedX; + float m_walkSpeedZ; int m_launchCount; float m_maxLaunchSpeed; bool m_wasLaunched; diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 08dbfee..475986b 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -244,7 +244,7 @@ public: DrawPoint( point, 10.0f, MakeColor( b3_colorYellow ) ); } - DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation ); + DrawString3D( point, MakeColor( b3_colorWhite ), " %.2f", 100.0f * manifoldPoint.separation ); b3FeaturePair pair = manifoldPoint.pair; DrawString3D( point + 0.025f * normal, MakeColor( b3_colorPapayaWhip ), " %X:%X %X:%X", pair.owner1, pair.index1, @@ -572,7 +572,7 @@ public: m_camera->SetView( 0.0f, 30.0f, 5.0f, b3Pos_zero ); } - m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.5f }; + m_capsule = { { -1.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f }, 0.15f }; m_hull = b3MakeBoxHull( 1.0f, 0.5f, 0.5f ); m_transformA = { { 0.0f, 0.0f, 0.0f }, b3Quat_identity }; @@ -636,17 +636,13 @@ public: m_camera->SetView( 0.0f, 30.0f, 10.0f, b3Pos_zero ); } - m_capsule = { { 0.0f, -0.2f, 0.0f }, { 0.0f, 0.2f, 0.0f }, 0.05f }; + m_capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.05f }; m_triangle[0] = { -4.0f, 0.0f, -4.0f }; m_triangle[1] = { -4.0f, 0.0f, 0.0f }; m_triangle[2] = { 0.0f, 0.0f, 0.0f }; m_transformA = b3WorldTransform_identity; - m_transformB = { { 0.0f, 0.5f, 0.0f }, b3Quat_identity }; - m_transformB.q = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.5f * B3_PI ); - - m_transformB = { { -0.500000000, 0.123778239, -0.500000000 }, - { { -0.157559350, 0.294042289, 0.821513653 }, -0.462417006 } }; + m_transformB = { { -1.0f, 0.0f, -1.0f }, b3Quat_identity }; } void Render() override diff --git a/samples/shaders/generated/capsule.glsl.h b/samples/shaders/generated/capsule.glsl.h index cd0efdd..3ed72bd 100644 --- a/samples/shaders/generated/capsule.glsl.h +++ b/samples/shaders/generated/capsule.glsl.h @@ -5,7 +5,7 @@ Generated by sokol-shdc (https://github.com/floooh/sokol-tools) Cmdline: - sokol-shdc --input shapes/capsule.glsl --output generated/capsule.glsl.h --slang hlsl5:metal_macos:glsl430 + sokol-shdc --input E:/box/box3d/samples/shaders/shapes/capsule.glsl --output E:/box/box3d/samples/shaders/generated/capsule.glsl.h --slang hlsl5:metal_macos:glsl430 Overview: ========= diff --git a/samples/shaders/generated/cube.glsl.h b/samples/shaders/generated/cube.glsl.h index 59d2fc6..6602a19 100644 --- a/samples/shaders/generated/cube.glsl.h +++ b/samples/shaders/generated/cube.glsl.h @@ -5,7 +5,7 @@ Generated by sokol-shdc (https://github.com/floooh/sokol-tools) Cmdline: - sokol-shdc --input shapes/cube.glsl --output generated/cube.glsl.h --slang hlsl5:metal_macos:glsl430 + sokol-shdc --input E:/box/box3d/samples/shaders/shapes/cube.glsl --output E:/box/box3d/samples/shaders/generated/cube.glsl.h --slang hlsl5:metal_macos:glsl430 Overview: ========= diff --git a/samples/shaders/generated/geom.glsl.h b/samples/shaders/generated/geom.glsl.h index 4b155d9..4792d60 100644 --- a/samples/shaders/generated/geom.glsl.h +++ b/samples/shaders/generated/geom.glsl.h @@ -5,7 +5,7 @@ Generated by sokol-shdc (https://github.com/floooh/sokol-tools) Cmdline: - sokol-shdc --input shapes/geom.glsl --output generated/geom.glsl.h --slang hlsl5:metal_macos:glsl430 + sokol-shdc --input E:/box/box3d/samples/shaders/shapes/geom.glsl --output E:/box/box3d/samples/shaders/generated/geom.glsl.h --slang hlsl5:metal_macos:glsl430 Overview: ========= diff --git a/samples/shaders/generated/sphere.glsl.h b/samples/shaders/generated/sphere.glsl.h index c234318..401709b 100644 --- a/samples/shaders/generated/sphere.glsl.h +++ b/samples/shaders/generated/sphere.glsl.h @@ -5,7 +5,7 @@ Generated by sokol-shdc (https://github.com/floooh/sokol-tools) Cmdline: - sokol-shdc --input shapes/sphere.glsl --output generated/sphere.glsl.h --slang hlsl5:metal_macos:glsl430 + sokol-shdc --input E:/box/box3d/samples/shaders/shapes/sphere.glsl --output E:/box/box3d/samples/shaders/generated/sphere.glsl.h --slang hlsl5:metal_macos:glsl430 Overview: ========= diff --git a/src/convex_manifold.c b/src/convex_manifold.c index 2422567..e97f58d 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -12,31 +12,6 @@ #include #include -static inline bool b3IsMinkowskiFaceIsolated( b3Vec3 a, b3Vec3 b, b3Vec3 n ) -{ - // An isolated edge (e.g. like in a capsule) defines a circle through the - // origin on the Gauss map. So testing for overlap between this circle and - // the arc AB simplifies to a simple plane test. - float an = b3Dot( a, n ); - float bn = b3Dot( b, n ); - - return an * bn <= 0.0f; -} - -// bxa = cross(b, a) and dxc = cross(d, c) -// but in practice we use the edge vector between the faces for robustness -static inline bool b3IsMinkowskiFace( b3Vec3 a, b3Vec3 b, b3Vec3 bxa, b3Vec3 c, b3Vec3 d, b3Vec3 dxc ) -{ - // Two edges build a face on the Minkowski sum if the associated arcs ab and cd intersect on the Gauss map. - // The associated arcs are defined by the adjacent face normals of each edge. - float cba = b3Dot( c, bxa ); - float dba = b3Dot( d, bxa ); - float adc = b3Dot( a, dxc ); - float bdc = b3Dot( b, dxc ); - - return cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; -} - static int b3ClipSegment( b3ClipVertex segment[2], b3Plane plane ) { int vertexCount = 0; @@ -178,18 +153,20 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c b3Transform capsuleTransform ) { // Find axis of minimum penetration + b3Vec3 maxNormal = b3Vec3_zero; float maxSeparation = -FLT_MAX; - int maxIndex1 = -1; - int maxIndex2 = -1; + int maxIndexA = B3_NULL_INDEX; + int maxIndexB = B3_NULL_INDEX; // We perform all computations in local space of the hull - b3Vec3 p1 = b3TransformPoint( capsuleTransform, capsule->center1 ); - b3Vec3 q1 = b3TransformPoint( capsuleTransform, capsule->center2 ); - b3Vec3 e1 = b3Sub( q1, p1 ); + b3Vec3 pA = b3TransformPoint( capsuleTransform, capsule->center1 ); + b3Vec3 qA = b3TransformPoint( capsuleTransform, capsule->center2 ); + b3Vec3 eA = b3Sub( qA, pA ); const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); const b3Plane* planes = b3GetHullPlanes( hull ); + float squaredTolerance = 0.005f * 0.005f; for ( int index = 0; index < hull->edgeCount; index += 2 ) { @@ -197,42 +174,70 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c const b3HullHalfEdge* twin = edges + index + 1; B3_ASSERT( edge->twin == index + 1 && twin->twin == index ); - b3Vec3 p2 = points[edge->origin]; - b3Vec3 q2 = points[twin->origin]; - b3Vec3 e2 = b3Sub( q2, p2 ); + b3Vec3 qB = points[twin->origin]; + b3Vec3 uB = planes[edge->face].normal; + b3Vec3 vB = planes[twin->face].normal; - b3Vec3 u2 = planes[edge->face].normal; - b3Vec3 v2 = planes[twin->face].normal; + // An isolated edge (e.g. like in a capsule) defines a circle through the + // origin on the Gauss map. So testing for overlap between this circle and + // the arc AB simplifies to a plane test. + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); - if ( b3IsMinkowskiFaceIsolated( u2, v2, e1 ) ) + if ( cba * dba < 0.0f ) { - // We can pass any point on the edge and choose - // the edge centers for better numerical precision. - b3Vec3 c1 = b3MulSV( 0.5f, b3Add( q1, p1 ) ); - b3Vec3 c2 = hull->center; - float separation = b3EdgeEdgeSeparation( q1, e1, c1, q2, e2, c2 ); + // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. + if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) ) + { + continue; + } + + // The intersection of the arcs on the Gauss map is the edge pair axis. Cast the + // arc of hull B (from uB to vB) against the plane containing the arc of hull A: + // dot(uB + t * (vB - uB), eA) == 0 + // then + // t = cba / (cba - dba) + // + // The signs of cba and dba differ (Minkowski test), so the division is safe. + // + // The axis generated points from B to A by construction since it lands between + // two face normals on B. This removes the need to orient the separation axis + // using the hull centers. + // + // The axis is perpendicular to both edges so I can use qA and qB as arbitrary + // points on edgeA and edgeB to measure the separation. + + float t = cba / ( cba - dba ); + b3Vec3 axis = b3Lerp( uB, vB, t ); + B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); + axis = b3Normalize( axis ); + float separation = b3Dot( axis, b3Sub( qA, qB ) ); + 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 = 0; - maxIndex2 = index; + maxIndexA = 0; + maxIndexB = index; } } } // Save result return (b3EdgeQuery){ + .normal = maxNormal, .separation = maxSeparation, - .indexA = (uint8_t)maxIndex1, - .indexB = (uint8_t)maxIndex2, + .indexA = maxIndexA, + .indexB = maxIndexB, }; } static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) { // Find axis of minimum penetration + b3Vec3 maxNormal = b3Vec3_zero; float maxSeparation = -FLT_MAX; int maxIndexA = B3_NULL_INDEX; int maxIndexB = B3_NULL_INDEX; @@ -247,6 +252,8 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD // Work in frame A b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + float squaredTolerance = 0.005f * 0.005f; + // Arranged to minimize transform operations for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) { @@ -272,27 +279,53 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD b3Vec3 uA = planesA[edgeA->face].normal; b3Vec3 vA = planesA[twinA->face].normal; - bool isMinkowski; - { - // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. - // The associated arcs are defined by the adjacent face normals of each edge. - float cba = b3Dot( uB, eA ); - float dba = b3Dot( vB, eA ); - float adc = -b3Dot( uA, eB ); - float bdc = -b3Dot( vA, eB ); + // See "Collision Detection of Convex Polyhedra Based on Duality Transformation" + // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. - isMinkowski = cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; - } + // These are signed volumes with an edge optimization to avoid cross products + // eA parallel to cross(vA, uA) + // eB parallel to cross(vB, uB) + // Since only signs are tested, length doesn't matter. - if ( isMinkowski ) + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) { - b3Vec3 centerA = hullA->center; - b3Vec3 centerB = b3TransformPoint( transformBtoA, hullB->center ); - float separation = b3EdgeEdgeSeparation( qA, eA, centerA, qB, eB, centerB ); + // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. + if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) ) + { + continue; + } + + // The intersection of the arcs on the Gauss map is the edge pair axis. Cast the + // arc of hull B (from uB to vB) against the plane containing the arc of hull A: + // dot(uB + t * (vB - uB), eA) == 0 + // then + // t = cba / (cba - dba) + // + // The signs of cba and dba differ (Minkowski test), so the division is safe. + // + // The axis generated points from B to A by construction since it lands between + // two face normals on B. This removes the need to orient the separation axis + // using the hull centers. + // + // The axis is perpendicular to both edges so I can use qA and qB as arbitrary + // points on edgeA and edgeB to measure the separation. + float t = cba / ( cba - dba ); + b3Vec3 axis = b3Lerp( uB, vB, t ); + B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); + axis = b3Normalize( axis ); + float separation = b3Dot( axis, b3Sub( qA, qB ) ); if ( separation > maxSeparation ) { // Continues to find the maximum separating axis + // Flip normal so it points from A to B + maxNormal = b3Neg( axis ); maxSeparation = separation; maxIndexA = indexA; maxIndexB = indexB; @@ -302,6 +335,7 @@ static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullD } return (b3EdgeQuery){ + .normal = maxNormal, .separation = maxSeparation, .indexA = maxIndexA, .indexB = maxIndexB, @@ -895,19 +929,11 @@ static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int cap const b3HullHalfEdge* edge2 = edges + query.indexB; const b3HullHalfEdge* twin2 = edges + edge2->twin; - b3Vec3 ch = hullA->center; b3Vec3 ph = points[edge2->origin]; b3Vec3 qh = points[twin2->origin]; b3Vec3 eh = b3Sub( qh, ph ); - b3Vec3 normal = b3Cross( ec, eh ); - normal = b3Normalize( normal ); - - // Normal should point outward from hull - if ( b3Dot( normal, b3Sub( ph, ch ) ) < 0.0f ) - { - normal = b3Neg( normal ); - } + b3Vec3 normal = query.normal; b3SegmentDistanceResult result = b3LineDistance( ph, eh, pc, ec ); @@ -918,7 +944,6 @@ static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int cap } b3Vec3 point = b3MulSV( 0.5f, b3Add( b3MulSub( result.point1, capsuleB->radius, normal ), result.point2 ) ); - float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); @@ -1235,8 +1260,8 @@ static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const return true; } -static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, - b3EdgeQuery query, b3SATCache* cache ) +static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3EdgeQuery query, b3SATCache* cache ) { // Work in shapeA coordinates const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); @@ -1249,7 +1274,6 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul const b3HullHalfEdge* edgeA = edgesA + query.indexA; const b3HullHalfEdge* twinA = edgesA + edgeA->twin; - b3Vec3 centerA = hullA->center; b3Vec3 pA = pointsA[edgeA->origin]; b3Vec3 qA = pointsA[twinA->origin]; b3Vec3 eA = b3Sub( qA, pA ); @@ -1260,14 +1284,7 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twinB->origin] ); b3Vec3 eB = b3Sub( qB, pB ); - b3Vec3 normal = b3Cross( eA, eB ); - normal = b3Normalize( normal ); - - if ( b3Dot( normal, b3Sub( pA, centerA ) ) < 0.0f ) - { - normal = b3Neg( normal ); - } - + b3Vec3 normal = query.normal; b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); if ( b3IsWithinSegments( &result ) == false ) @@ -1278,10 +1295,6 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul // This can slide off the end from caching float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); - - // todo I suspect this could trip if the cache becomes invalid - // B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); - b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); // Result in frame A @@ -1302,8 +1315,8 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul return true; } -void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, - b3SATCache* cache ) +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache ) { manifold->pointCount = 0; @@ -1405,55 +1418,64 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* case b3_edgePairAxis: { - int index1 = cache->indexA; - const b3HullHalfEdge* edge1 = edgesA + index1; - const b3HullHalfEdge* twin1 = edgesA + index1 + 1; - B3_ASSERT( edge1->twin == index1 + 1 && twin1->twin == index1 ); + int indexA = cache->indexA; + const b3HullHalfEdge* edge1 = edgesA + indexA; + const b3HullHalfEdge* twin1 = edgesA + indexA + 1; + B3_ASSERT( edge1->twin == indexA + 1 && twin1->twin == indexA ); - b3Vec3 p1 = pointsA[edge1->origin]; - b3Vec3 q1 = pointsA[twin1->origin]; - b3Vec3 e1 = b3Sub( q1, p1 ); + b3Vec3 pA = pointsA[edge1->origin]; + b3Vec3 qA = pointsA[twin1->origin]; + b3Vec3 eA = b3Sub( qA, pA ); - b3Vec3 u1 = planesA[edge1->face].normal; - b3Vec3 v1 = planesA[twin1->face].normal; + b3Vec3 uA = planesA[edge1->face].normal; + b3Vec3 vA = planesA[twin1->face].normal; - int index2 = cache->indexB; - const b3HullHalfEdge* edge2 = edgesB + index2; - const b3HullHalfEdge* twin2 = edgesB + index2 + 1; - B3_ASSERT( edge2->twin == index2 + 1 && twin2->twin == index2 ); + int indexB = cache->indexB; + const b3HullHalfEdge* edge2 = edgesB + indexB; + const b3HullHalfEdge* twin2 = edgesB + indexB + 1; + B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB ); - b3Vec3 p2 = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); - b3Vec3 q2 = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); - b3Vec3 e2 = b3Sub( q2, p2 ); + b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); + b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); + b3Vec3 eB = b3Sub( qB, pB ); - b3Vec3 u2 = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); - b3Vec3 v2 = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); + b3Vec3 uB = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); + b3Vec3 vB = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); // flipping the signs of u2 and v2 // cross(v2, u2) == cross(-v2, -u2) // so we still use -e2 // but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2) - bool isMinkowski = b3IsMinkowskiFace( u1, v1, e1, b3Neg( u2 ), b3Neg( v2 ), e2 ); - if ( isMinkowski == true ) + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) { - // Transform reference center of the first hull into local space of the second hull - b3Vec3 c1 = hullA->center; - b3Vec3 c2 = b3TransformPoint( transformBtoA, hullB->center ); - - float separation = b3EdgeEdgeSeparation( p1, e1, c1, p2, e2, c2 ); - if ( separation > speculativeDistance ) + // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. + float squaredTolerance = 0.005f * 0.005f; + if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) ) { - // Cache hit, shapes are separated - return; - } + // Transform reference center of the first hull into local space of the second hull + float t = cba / ( cba - dba ); + b3Vec3 axis = b3Lerp( uB, vB, t ); + B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); + axis = b3Normalize( axis ); + float separation = b3Dot( axis, b3Sub( qA, qB ) ); + + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } - // if ( cache->separation <= speculativeDistance ) - { // Try to rebuild contact from last features - b3EdgeQuery edgeQuery; + b3EdgeQuery edgeQuery = { 0 }; + edgeQuery.normal = b3Neg( axis ); + edgeQuery.separation = 0.0f; edgeQuery.indexA = cache->indexA; edgeQuery.indexB = cache->indexB; - edgeQuery.separation = 0.0f; b3SATCache localCache = { 0 }; bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); diff --git a/src/manifold.c b/src/manifold.c index 40e60ad..186d696 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -10,58 +10,6 @@ #include "box3d/math_functions.h" -// p1 : origin on edge 1 -// e1 : edge 1 -// c1 : shape 1 centroid -// p2 : origin on edge 2 -// e2 : edge 2 -// c2 : shape 2 centroid -float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ) -{ - // Build search direction - b3Vec3 u = b3Cross( e1, e2 ); - float length = b3Length( u ); - - // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| - const float kTolerance = 0.005f; - if ( length < kTolerance * sqrtf( b3LengthSquared( e1 ) * b3LengthSquared( e2 ) ) ) - { - return -FLT_MAX; - } - - if ( length * length < 1000.0f * FLT_MIN ) - { - return -FLT_MAX; - } - - b3Vec3 n = b3MulSV( 1.0f / length, u ); - - // Make sure normal points away from the first shape - // For a triangle, it is possible that N is aligned with the triangle normal and the sign - // value can be close to zero and flicker between small negative and positive values, leading to - // an incorrect separation value. So we assume the other hull has some volume and pick the most - // significant sign value to orient N. - float sign1 = b3Dot( n, b3Sub( p1, c1 ) ); - float sign2 = b3Dot( n, b3Sub( p2, c2 ) ); - if ( b3AbsFloat( sign1 ) > b3AbsFloat( sign2 ) ) - { - if ( sign1 < 0.0f ) - { - n = b3Neg( n ); - } - } - else - { - if ( sign2 > 0.0f ) - { - n = b3Neg( n ); - } - } - - // s = Dot(n, p2) - d = Dot(n, p2) - Dot(n, p1) = Dot(n, p2 - p1) - return b3Dot( n, b3Sub( p2, p1 ) ); -} - // This was extended to make the wedge shape get the correct incident face. // Instead of looking directly for the most anti-parallel face, we first find the closest vertex (passed in). // Then we look for all edges coming out of that vertex and look for the edge that is diff --git a/src/manifold.h b/src/manifold.h index 9e529ab..5ed0a5f 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -17,6 +17,7 @@ typedef struct b3FaceQuery typedef struct b3EdgeQuery { + b3Vec3 normal; float separation; int indexA; int indexB; @@ -35,7 +36,6 @@ typedef enum b3FeatureOwner b3_featureShapeB = 1 } b3FeatureOwner; -float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ); int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex ); b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ); diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index b87973a..b964ea6 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -18,7 +18,6 @@ typedef struct b3TriangleData { b3Vec3 v1, v2, v3; b3Vec3 e1, e2, e3; - b3Vec3 center; b3Plane plane; int flags; } b3TriangleData; @@ -178,35 +177,62 @@ static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule }; } -static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const b3Capsule* capsule ) +static b3EdgeQuery 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 ); - b3Vec3 capsuleCenter = b3Lerp( p1, p2, 0.5f ); - - b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vertices[0], b3Add( vertices[1], vertices[2] ) ) ); - // Find axis of minimum penetration + b3Vec3 maxNormal = b3Vec3_zero; float maxSeparation = -FLT_MAX; - int maxIndex1 = UINT8_MAX; - int maxIndex2 = UINT8_MAX; + int maxIndex1 = B3_NULL_INDEX; + int maxIndex2 = B3_NULL_INDEX; + float squaredTolerance = 0.005f * 0.005f; int edgeIndex = 2; 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 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. + if ( a * a + b * b < squaredTolerance * b3LengthSquared( capsuleEdge ) ) + { + 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 ) ); - float separation = b3EdgeEdgeSeparation( p1, capsuleEdge, capsuleCenter, v1, triangleEdge, triangleCenter ); 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; @@ -218,9 +244,10 @@ static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const // Save result return (b3EdgeQuery){ + .normal = maxNormal, .separation = maxSeparation, - .indexA = (uint8_t)maxIndex1, - .indexB = (uint8_t)maxIndex2, + .indexA = maxIndex1, + .indexB = maxIndex2, }; } @@ -272,8 +299,8 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con pt->pair = segment[1].pair; } -static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, const b3Capsule* capsule, - b3EdgeQuery query ) +static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule, b3EdgeQuery query ) { B3_ASSERT( 0 <= query.indexA && query.indexA < 3 ); @@ -283,20 +310,27 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con const b3Vec3* vs = triangle; - b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vs[0], b3Add( vs[1], vs[2] ) ) ); b3Vec3 v1 = vs[query.indexA]; b3Vec3 v2 = vs[( query.indexA + 1 ) % 3]; b3Vec3 triangleEdge = b3Sub( v2, v1 ); - b3Vec3 normal = b3Cross( capsuleEdge, triangleEdge ); - normal = b3Normalize( normal ); + b3Vec3 sideNormal = b3Normalize( b3Cross( triangleEdge, plane.normal ) ); - // Normal should point away from triangle center - if ( b3Dot( normal, b3Sub( v1, triangleCenter ) ) < 0.0f ) + // 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 ) ) { - normal = b3Neg( normal ); + 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 ) @@ -306,8 +340,7 @@ static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, con } b3Vec3 point = b3Lerp( b3MulSub( result.point1, capsule->radius, normal ), result.point2, 0.5f ); - - float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + float separation = b3Dot( normal, b3Sub( p1, v1 ) ); B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); manifold->normal = normal; @@ -430,7 +463,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const return; } - b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, capsuleA ); + b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, plane, capsuleA ); if ( edgeQuery.separation > radius ) { // Shapes are separated @@ -454,7 +487,7 @@ void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) { // Edge contact - b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, capsuleA, edgeQuery ); + b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, plane, capsuleA, edgeQuery ); } } @@ -527,9 +560,10 @@ static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3Hull }; } -static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3HullData* hull ) +static b3EdgeQuery b3QueryTriangleAndHullEdges( const b3TriangleData* triangle, const b3HullData* hull ) { b3EdgeQuery result = { + .normal = b3Vec3_zero, .separation = -FLT_MAX, .indexA = B3_NULL_INDEX, .indexB = B3_NULL_INDEX, @@ -552,6 +586,7 @@ static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3Hull 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 ) { @@ -577,14 +612,28 @@ static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3Hull continue; } - b3Vec3 triPoint = trianglePoints[j]; - float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangle->center, hullPoint, hullEdge, hull->center ); + // 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; @@ -822,12 +871,11 @@ static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity } static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, - b3Vec3 triangleCenter, const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache ) + const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache ) { B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); B3_ASSERT( query.indexA < 3 ); - b3Vec3 cA = triangleCenter; b3Vec3 pA = trianglePoint; b3Vec3 eA = triangleEdge; @@ -839,32 +887,6 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci b3Vec3 qB = pointsB[twinB->origin]; b3Vec3 eB = b3Sub( qB, pB ); - b3Vec3 normal = b3Cross( eA, eB ); - normal = b3Normalize( normal ); - - // Ensure normal points outward from triangle center - float outwardA = b3Dot( normal, b3Sub( pA, cA ) ); - - // Ensure normal points towards hull center - float outwardB = b3Dot( normal, b3Sub( hull->center, pB ) ); - - // Use the largest magnitude. The triangle outward value - // may be unreliable as some angles. - if ( b3AbsFloat( outwardA ) > b3AbsFloat( outwardB ) ) - { - if ( outwardA < 0.0f ) - { - normal = b3Neg( normal ); - } - } - else - { - if ( outwardB < 0.0f ) - { - normal = b3Neg( normal ); - } - } - // Get the closest points between the infinite edge lines b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); @@ -879,7 +901,7 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci } // This can slide off the end from caching - float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + float separation = b3Dot( query.normal, b3Sub( pB, pA ) ); B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); @@ -895,24 +917,13 @@ static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capaci cache->indexA = (uint8_t)query.indexA; cache->indexB = (uint8_t)query.indexB; - manifold->normal = normal; + manifold->normal = query.normal; manifold->pointCount = 1; b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; manifold->feature = edgesFeatures[query.indexA]; } -// See "Collision Detection of Convex Polyhedra Based on Duality Transformation" -// Simplified for triangle versus hull -static inline bool b3IsTriangleMinkowskiFace( b3Vec3 triNormal, b3Vec3 triEdge, b3Vec3 hullNormal1, b3Vec3 hullNormal2, - b3Vec3 hullEdge ) -{ - float cab = b3Dot( hullNormal1, triEdge ); - float dab = b3Dot( hullNormal2, triEdge ); - float bcd = b3Dot( triNormal, hullEdge ); - return cab * dab < 0.0f && cab * bcd > 0.0f; -} - b3AtomicInt b3_triangleConvexCalls; b3AtomicInt b3_triangleCacheHits; @@ -951,7 +962,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 return; } - b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); b3Vec3 trianglePoints[] = { v1, v2, v3 }; b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; @@ -962,7 +972,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 .e1 = triangleEdges[0], .e2 = triangleEdges[1], .e3 = triangleEdges[2], - .center = triangleCenter, .plane = trianglePlane, .flags = triangleFlags, }; @@ -1093,36 +1102,54 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal; b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal; - // Confirm the edge pair is still a Minkowski face - bool isMinkowski = b3IsTriangleMinkowskiFace( trianglePlane.normal, triEdge, hullNormal1, hullNormal2, hullEdge ); - if ( isMinkowski ) + // 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 ) { - // Transform reference center of the first hull into local space of the second hull - float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangleCenter, hullPoint, hullEdge, hullA->center ); - if ( separation > speculativeDistance ) + 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 ) ) { - // Cache hit, shapes are separated - return; - } - - if ( b3AbsFloat( cache->separation - separation ) < linearSlop ) - { - // Try to rebuild contact from last features - b3EdgeQuery edgeQuery; - edgeQuery.indexA = indexA; - edgeQuery.indexB = indexB; - edgeQuery.separation = separation; - - // Read cache but don't modify it - b3SATCache localCache = *cache; - b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, triangleCenter, hullA, edgeQuery, - &localCache ); - - if ( manifold->pointCount > 0 ) + // 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, contact point generated + // 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 + b3EdgeQuery edgeQuery; + edgeQuery.normal = b3Neg( axis ); + edgeQuery.indexA = indexA; + edgeQuery.indexB = indexB; + edgeQuery.separation = separation; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, hullA, edgeQuery, &localCache ); + + if ( manifold->pointCount > 0 ) + { + // Cache hit, contact point generated + return; + } + } } } @@ -1150,13 +1177,12 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 // This case is for testing case b3_manualEdgePairAxis: { - b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA ); if ( edgeQuery.indexA != B3_NULL_INDEX ) { b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; - b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, - cache ); + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache ); } return; } @@ -1192,7 +1218,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 return; } - b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + b3EdgeQuery edgeQuery = b3QueryTriangleAndHullEdges( &triangle, hullA ); if ( edgeQuery.separation > speculativeDistance ) { // Separating axis found @@ -1234,8 +1260,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; manifold->pointCount = 0; - b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, - cache ); + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, hullA, edgeQuery, cache ); } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ee0e9c1..92dcbb1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -17,6 +17,7 @@ set(BOX3D_TEST_FILES test_joint.c test_large_world.c test_macros.h + test_manifold.c test_math.c test_mover.c test_name_cache.c diff --git a/test/main.c b/test/main.c index 70f75ca..8edcf96 100644 --- a/test/main.c +++ b/test/main.c @@ -39,6 +39,7 @@ extern int HullTest( void ); extern int IdTest( void ); extern int JointTest( void ); extern int LargeWorldTest( void ); +extern int ManifoldTest( void ); extern int MathTest( void ); extern int MoverTest( void ); extern int NameCacheTest( void ); @@ -110,6 +111,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( IdTest ); MAYBE_RUN_TEST( JointTest ); MAYBE_RUN_TEST( LargeWorldTest ); + MAYBE_RUN_TEST( ManifoldTest ); MAYBE_RUN_TEST( MathTest ); MAYBE_RUN_TEST( MoverTest ); MAYBE_RUN_TEST( NameCacheTest ); diff --git a/test/test_manifold.c b/test/test_manifold.c new file mode 100644 index 0000000..702603f --- /dev/null +++ b/test/test_manifold.c @@ -0,0 +1,1102 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include + +static const float kRoot2 = 1.41421356f; +static const float kHalfRoot2 = 0.70710678f; + +static const b3Vec3 kAxisX = { 1.0f, 0.0f, 0.0f }; +static const b3Vec3 kAxisY = { 0.0f, 1.0f, 0.0f }; +static const b3Vec3 kAxisZ = { 0.0f, 0.0f, 1.0f }; + +// b3ComputeCosSin is a rational approximation good to about 1e-3. That is coarse enough to +// shift an edge off the position the analytic result expects, so these fixtures need libm. +static b3Quat ExactQuat( b3Vec3 axis, float radians ) +{ + float half = 0.5f * radians; + float s = sinf( half ); + return (b3Quat){ { s * axis.x, s * axis.y, s * axis.z }, cosf( half ) }; +} + +static b3Transform ExactRotation( b3Vec3 axis, float radians ) +{ + return (b3Transform){ b3Vec3_zero, ExactQuat( axis, radians ) }; +} + +// Cube A yawed 45 degrees presents an edge along y at x = +h*root2. +// Cube B rolled 45 degrees presents an edge along z at x = -h*root2. +// Sliding B along x makes those two edges the closest features, so the axis of minimum +// penetration is x, the separation is d - 2*h*root2 and the contact point sits at x = d/2. +// Both hulls are far from a face axis here, which keeps the edge query in charge. +static void MakeCrossedEdgeHulls( b3BoxHull* hullA, b3BoxHull* hullB, float halfWidth ) +{ + *hullA = b3MakeTransformedBoxHull( halfWidth, halfWidth, halfWidth, ExactRotation( kAxisY, 0.25f * B3_PI ) ); + *hullB = b3MakeTransformedBoxHull( halfWidth, halfWidth, halfWidth, ExactRotation( kAxisZ, 0.25f * B3_PI ) ); +} + +static b3Transform SlideX( float x ) +{ + return (b3Transform){ { x, 0.0f, 0.0f }, b3Quat_identity }; +} + +static float MinSeparation( const b3LocalManifold* manifold ) +{ + float minSeparation = FLT_MAX; + for ( int i = 0; i < manifold->pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + return minSeparation; +} + +// The edge pair axis is built by intersecting the two Gauss map arcs. Walk the crossed edges from +// speculative contact into deep overlap and check the axis, the separation and the point. +static int CrossedEdgeTest( void ) +{ + b3BoxHull hullA, hullB; + MakeCrossedEdgeHulls( &hullA, &hullB, 0.5f ); + + float distances[] = { 1.42f, kRoot2, 1.41f, 1.3f }; + + for ( int i = 0; i < ARRAY_COUNT( distances ); ++i ) + { + float d = distances[i]; + float expected = d - kRoot2; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( d ), &cache ); + + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( manifold.normal.x - 1.0f, 1e-6f ); + ENSURE_SMALL( manifold.normal.y, 1e-6f ); + ENSURE_SMALL( manifold.normal.z, 1e-6f ); + ENSURE_SMALL( manifold.points[0].separation - expected, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.x - 0.5f * d, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.y, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.z, 1e-5f ); + + // The forced edge query must agree with what the full solver chose + b3LocalManifold manual = { 0 }; + manual.points = points; + b3SATCache manualCache = { .type = b3_manualEdgePairAxis }; + b3CollideHulls( &manual, 8, &hullA.base, &hullB.base, SlideX( d ), &manualCache ); + + ENSURE( manual.pointCount == 1 ); + ENSURE_SMALL( manual.points[0].separation - expected, 1e-5f ); + } + + // Beyond the speculative distance the query reports the axis without building a contact. + // The axis carries its own orientation now, so a sign error here would read as deep overlap. + { + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( 1.5f ), &cache ); + + ENSURE( manifold.pointCount == 0 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( cache.separation - ( 1.5f - kRoot2 ), 1e-5f ); + } + + return 0; +} + +// The parallel edge rejection compares dot products against the edge length, so it is a sine +// threshold and must hold at any size. +static int EdgeAxisScaleTest( void ) +{ + float scales[] = { 100.0f, 1.0f, 0.2f }; + + for ( int i = 0; i < ARRAY_COUNT( scales ); ++i ) + { + float s = scales[i]; + b3BoxHull hullA, hullB; + MakeCrossedEdgeHulls( &hullA, &hullB, 0.5f * s ); + + float expected = -0.002f; + float d = s * kRoot2 + expected; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( d ), &cache ); + + // Differencing coordinates of magnitude d costs precision proportional to the scale + float tolerance = 1e-5f * s + 1e-6f; + + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( manifold.normal.x - 1.0f, 1e-6f ); + ENSURE_SMALL( manifold.points[0].separation - expected, tolerance ); + ENSURE_SMALL( manifold.points[0].point.x - 0.5f * d, tolerance ); + ENSURE_SMALL( manifold.points[0].point.y, tolerance ); + ENSURE_SMALL( manifold.points[0].point.z, tolerance ); + } + + return 0; +} + +// The cached edge pair rebuilds the axis without a fresh query. An untouched cache proves the +// cached branch answered rather than falling through to the full SAT. +static int EdgeCacheTest( void ) +{ + b3BoxHull hullA, hullB; + MakeCrossedEdgeHulls( &hullA, &hullB, 0.5f ); + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( 1.41f ), &cache ); + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + // Cached edges are the even half of each twin pair + ENSURE( ( cache.indexA & 1 ) == 0 && cache.indexA < hullA.base.edgeCount ); + ENSURE( ( cache.indexB & 1 ) == 0 && cache.indexB < hullB.base.edgeCount ); + + float seededSeparation = cache.separation; + ENSURE_SMALL( seededSeparation - ( 1.41f - kRoot2 ), 1e-5f ); + + // Small motion, the cached features still describe the contact + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( 1.4105f ), &cache ); + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.separation == seededSeparation ); + ENSURE_SMALL( manifold.points[0].separation - ( 1.4105f - kRoot2 ), 1e-5f ); + ENSURE_SMALL( manifold.normal.x - 1.0f, 1e-6f ); + + // Jump past the speculative distance. The cached axis alone must report the separation. + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, SlideX( 1.5f ), &cache ); + ENSURE( manifold.pointCount == 0 ); + ENSURE( cache.separation == seededSeparation ); + + return 0; +} + +// Sliding B along the direction of edge A walks the closest point off the end of the segment. +static int EdgeEndpointTest( void ) +{ + b3BoxHull hullA, hullB; + MakeCrossedEdgeHulls( &hullA, &hullB, 0.5f ); + + float d = 1.41f; + float expected = d - kRoot2; + + // Just inside the end of edge A + { + b3Transform transform = { { d, 0.49f, 0.0f }, b3Quat_identity }; + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { .type = b3_manualEdgePairAxis }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount == 1 ); + ENSURE_SMALL( manifold.points[0].separation - expected, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.y - 0.49f, 1e-5f ); + } + + // Off the end. The edge pair no longer describes a contact, so the builder rejects it and + // clears the cache rather than clamping to a point that is not on the hulls. + { + b3Transform transform = { { d, 0.55f, 0.0f }, b3Quat_identity }; + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { .type = b3_manualEdgePairAxis }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount == 0 ); + ENSURE( cache.type == b3_invalidAxis ); + + // The true gap is a vertex to edge distance well past the speculative distance + b3SATCache freshCache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &freshCache ); + ENSURE( manifold.pointCount == 0 ); + ENSURE( freshCache.separation > 0.0f ); + } + + return 0; +} + +static const b3Vec3 kTiltAxes[] = { + { 0.57735027f, 0.57735027f, 0.57735027f }, + { 0.70710678f, 0.0f, 0.70710678f }, + { 0.26726124f, 0.53452248f, 0.80178373f }, + { -0.48507125f, 0.72760688f, -0.48507125f }, +}; + +// Angles that straddle the 0.005 rejection threshold +static const float kTiltAngles[] = { 0.0f, 1e-7f, 1e-6f, 1e-5f, 1e-4f, 1e-3f, 0.004f, 0.005f, 0.006f, 0.01f, 0.05f }; + +// A cube corner is root3/2 from the center of rotation +static const float kHalfDiagonal = 0.87f; + +// Cubes stacked face to face and tipped by a hair. A third of the edge pairs are then nearly +// parallel, the angle between them is at the noise floor and the arc intersection carries no +// information. The face contact has to survive that untouched. +static int ParallelEdgeTest( void ) +{ + b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + float overlap = 0.01f; + + for ( int i = 0; i < ARRAY_COUNT( kTiltAxes ); ++i ) + { + for ( int j = 0; j < ARRAY_COUNT( kTiltAngles ); ++j ) + { + float angle = kTiltAngles[j]; + b3Transform transform = { { 0.0f, 1.0f - overlap, 0.0f }, ExactQuat( kTiltAxes[i], angle ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount == 4 ); + ENSURE( cache.type == b3_faceAxisA || cache.type == b3_faceAxisB ); + ENSURE( b3Dot( manifold.normal, kAxisY ) > 0.998f ); + + // The tilt can only lift or sink a face point by the length of the arc it sweeps + float bound = kHalfDiagonal * angle + 1e-5f; + for ( int k = 0; k < manifold.pointCount; ++k ) + { + ENSURE_SMALL( manifold.points[k].separation + overlap, bound ); + } + } + } + + return 0; +} + +// Same stack, but force the edge query to answer. With the edges exactly parallel no pair forms a +// Minkowski face at all, and once a pair does form its separation can never be positive because +// the hulls overlap. +static int ParallelEdgeManualTest( void ) +{ + b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + float overlap = 0.01f; + + for ( int i = 0; i < ARRAY_COUNT( kTiltAxes ); ++i ) + { + for ( int j = 0; j < ARRAY_COUNT( kTiltAngles ); ++j ) + { + float angle = kTiltAngles[j]; + b3Transform transform = { { 0.0f, 1.0f - overlap, 0.0f }, ExactQuat( kTiltAxes[i], angle ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { .type = b3_manualEdgePairAxis }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + if ( angle == 0.0f ) + { + // Every pair is parallel so the query finds nothing and leaves the cache alone + ENSURE( manifold.pointCount == 0 ); + ENSURE( cache.type == b3_manualEdgePairAxis ); + continue; + } + + // The closest points can fall off the ends of the segments + if ( manifold.pointCount == 0 ) + { + continue; + } + + ENSURE( manifold.pointCount == 1 ); + ENSURE( b3Dot( manifold.normal, kAxisY ) > 0.99f ); + + float separation = manifold.points[0].separation; + ENSURE( separation <= 0.0f ); + ENSURE( separation >= -overlap - kHalfDiagonal * angle - 1e-4f ); + } + } + + return 0; +} + +static uint32_t g_seed = 12345; + +static float NextFloat( float lower, float upper ) +{ + g_seed = 1664525u * g_seed + 1013904223u; + float t = (float)( g_seed >> 8 ) / (float)( 1 << 24 ); + return lower + t * ( upper - lower ); +} + +static b3Vec3 NextDirection( void ) +{ + b3Vec3 v = { NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ) }; + return b3Normalize( v ); +} + +// Overlapping hulls admit no separating axis, so an edge separation that comes back positive is +// always noise. It shows up as a manifold with no points, which the solver reads as no contact. +static int OverlapNeverEmptyTest( void ) +{ + b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BoxHull hullB = b3MakeBoxHull( 0.4f, 0.6f, 0.5f ); + + for ( int i = 0; i < 2000; ++i ) + { + b3Vec3 axis = NextDirection(); + + // Half the samples are nearly aligned, where the edge cross products are smallest + float angle = ( i & 1 ) ? NextFloat( -0.01f, 0.01f ) : NextFloat( -B3_PI, B3_PI ); + + // Shorter than the smallest half width, so the center of B is inside A + b3Vec3 offset = b3MulSV( 0.4f, NextDirection() ); + + b3Transform transform = { offset, ExactQuat( axis, angle ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount > 0 ); + ENSURE_SMALL( b3Length( manifold.normal ) - 1.0f, 1e-5f ); + + // Clipping keeps points that are separated, but the deepest one must penetrate + ENSURE( MinSeparation( &manifold ) < 0.0f ); + } + + return 0; +} + +// A cube pitched 45 degrees rests on an edge along x at y = -h*root2. The two faces meeting there +// have normals (0,-r,r) and (0,-r,-r), so the arc between them spans the whole lower quadrant. +// A triangle edge crossing under it at an angle picks out an interior point of that arc, which is +// where a wrong lerp parameter would show up. +static int TriangleEdgeTest( void ) +{ + float beta = 20.0f * B3_PI / 180.0f; + float gamma = 30.0f * B3_PI / 180.0f; + + b3BoxHull hull = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); + + // Tipping the triangle plane about z keeps its normal off the hull edge, which the Minkowski + // test needs. Tipping the edge within that plane moves the arc intersection off the midpoint. + b3Vec3 triNormal = { sinf( beta ), cosf( beta ), 0.0f }; + b3Vec3 triEdge = { sinf( gamma ) * cosf( beta ), -sinf( gamma ) * sinf( beta ), cosf( gamma ) }; + + // Perpendicular to both edges and pointing out of the hull + b3Vec3 axis = b3Normalize( b3Cross( kAxisX, triEdge ) ); + b3Vec3 hullPoint = { 0.0f, -kHalfRoot2, 0.0f }; + + float gaps[] = { 0.03f, 0.01f, 0.0f, -0.01f, -0.1f }; + + for ( int i = 0; i < ARRAY_COUNT( gaps ); ++i ) + { + float gap = gaps[i]; + b3Vec3 trianglePoint = b3MulAdd( hullPoint, gap, axis ); + + b3Vec3 v1 = b3MulAdd( trianglePoint, -1.0f, triEdge ); + b3Vec3 v2 = b3MulAdd( trianglePoint, 1.0f, triEdge ); + b3Vec3 v3 = b3MulAdd( v1, 1.5f, b3Cross( triNormal, triEdge ) ); + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { .type = b3_manualEdgePairAxis }; + b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + + b3Vec3 expectedNormal = b3Neg( axis ); + b3Vec3 expectedPoint = b3MulAdd( hullPoint, 0.5f * gap, axis ); + + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( manifold.normal.x - expectedNormal.x, 1e-5f ); + ENSURE_SMALL( manifold.normal.y - expectedNormal.y, 1e-5f ); + ENSURE_SMALL( manifold.normal.z - expectedNormal.z, 1e-5f ); + ENSURE_SMALL( manifold.points[0].separation - gap, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.x - expectedPoint.x, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.y - expectedPoint.y, 1e-5f ); + ENSURE_SMALL( manifold.points[0].point.z - expectedPoint.z, 1e-5f ); + } + + // The tipped triangle plane buries a corner of the hull, so neither face axis separates and + // the edge axis has to carry the speculative cull on its own. + float culled[] = { 0.03f, 0.05f }; + + for ( int i = 0; i < ARRAY_COUNT( culled ); ++i ) + { + float gap = culled[i]; + b3Vec3 trianglePoint = b3MulAdd( hullPoint, gap, axis ); + + b3Vec3 v1 = b3MulAdd( trianglePoint, -1.0f, triEdge ); + b3Vec3 v2 = b3MulAdd( trianglePoint, 1.0f, triEdge ); + b3Vec3 v3 = b3MulAdd( v1, 1.5f, b3Cross( triNormal, triEdge ) ); + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + + ENSURE( manifold.pointCount == 0 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( cache.separation - gap, 1e-5f ); + } + + return 0; +} + +// The same cube resting its bottom edge on a triangle whose first edge runs along x. Tipping the +// triangle takes that pair from exactly parallel through the rejection threshold. +static int TriangleParallelEdgeTest( void ) +{ + b3BoxHull hull = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); + + float overlap = 0.01f; + float y = -kHalfRoot2 + overlap; + + for ( int i = 0; i < ARRAY_COUNT( kTiltAxes ); ++i ) + { + for ( int j = 0; j < ARRAY_COUNT( kTiltAngles ); ++j ) + { + b3Quat q = ExactQuat( kTiltAxes[i], kTiltAngles[j] ); + + b3Vec3 v1 = b3RotateVector( q, ( b3Vec3 ){ -2.0f, y, -1.0f } ); + b3Vec3 v2 = b3RotateVector( q, ( b3Vec3 ){ 0.0f, y, 2.0f } ); + b3Vec3 v3 = b3RotateVector( q, ( b3Vec3 ){ 2.0f, y, -1.0f } ); + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + + ENSURE( manifold.pointCount == 4 ); + ENSURE( cache.type == b3_faceAxisA ); + ENSURE( b3Dot( manifold.normal, kAxisY ) > 0.99f ); + + // The tilt can only sink the contact by the length of the arc it sweeps + float bound = kHalfDiagonal * kTiltAngles[j] + 1e-5f; + ENSURE_SMALL( MinSeparation( &manifold ) + overlap, bound ); + } + } + + return 0; +} + +// Two long roof ridges laid across each other. Unlike the stacked cubes the axis of minimum +// penetration really is the edge pair, so the query has to hand the winning pair to the contact +// builder. Closing the crossing angle drives that pair toward parallel, where the cross product +// the builder needs to orient the contact loses its precision. +static int RidgeCrossingTest( void ) +{ + b3BoxHull hullA = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); + b3BoxHull hullB = b3MakeTransformedBoxHull( 1.5f, 0.1f, 0.1f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); + + float ridgeY = 0.1f * kRoot2; + float overlap = 0.01f; + float lift = 2.0f * ridgeY - overlap; + + // Well clear of the rejection threshold. The ridges cross over the origin and the axis is y. + float crossingAngles[] = { 0.02f, 0.1f, 0.5f }; + + for ( int i = 0; i < ARRAY_COUNT( crossingAngles ); ++i ) + { + b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, crossingAngles[i] ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount == 1 ); + ENSURE( cache.type == b3_edgePairAxis ); + ENSURE_SMALL( manifold.normal.x, 1e-5f ); + ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f ); + ENSURE_SMALL( manifold.normal.z, 1e-5f ); + ENSURE_SMALL( manifold.points[0].separation + overlap, 1e-4f ); + ENSURE_SMALL( manifold.points[0].point.y - ( ridgeY - 0.5f * overlap ), 1e-4f ); + + // Where the point lands along the ridges is ill conditioned at a shallow crossing since + // the closest point solve divides by the square of the sine of the angle. It only has to + // land near the crossing, not at the end of a three meter beam. + ENSURE_SMALL( manifold.points[0].point.x, 0.01f ); + ENSURE_SMALL( manifold.points[0].point.z, 0.01f ); + } + + // Inside the rejection threshold. A one point edge contact off a parallel pair would have a + // normal built from noise, so the query drops the pair and the roof faces carry the contact. + float shallowAngles[] = { 0.0f, 1e-4f, 1e-3f, 0.003f }; + + for ( int i = 0; i < ARRAY_COUNT( shallowAngles ); ++i ) + { + b3Transform transform = { { 0.0f, lift, 0.0f }, ExactQuat( kAxisY, shallowAngles[i] ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + ENSURE( manifold.pointCount == 4 ); + ENSURE( cache.type != b3_edgePairAxis ); + + // A roof face of one hull, so 45 degrees off the vertical + ENSURE_SMALL( manifold.normal.x, 1e-4f ); + ENSURE_SMALL( manifold.normal.y - kHalfRoot2, 1e-4f ); + ENSURE_SMALL( b3AbsFloat( manifold.normal.z ) - kHalfRoot2, 1e-4f ); + + float minSeparation = MinSeparation( &manifold ); + ENSURE( minSeparation < 0.0f ); + ENSURE( minSeparation > -2.0f * overlap ); + } + + return 0; +} + +// The edge pair axis produced by the arc intersection must match the classic edge cross product. +// Rebuild the axis, the separation and the contact point from the two contributing edges and the +// convex radius, then compare against the manifold. orientRef fixes the sign of the axis to match +// the manifold normal convention for the shape pair. e1 belongs to the shape whose contact point is +// pulled in by the radius (the hull or triangle when it meets a capsule), e2 to the other edge. +static int CheckEdgeContact( const b3LocalManifold* manifold, b3Vec3 p1, b3Vec3 e1, b3Vec3 p2, b3Vec3 e2, b3Vec3 orientRef, + float radius, float normalTol, float sepTol, float pointTol ) +{ + b3Vec3 axis = b3Normalize( b3Cross( e1, e2 ) ); + if ( b3Dot( axis, orientRef ) < 0.0f ) + { + axis = b3Neg( axis ); + } + + // Normal matches the cross product and is perpendicular to both edges + ENSURE_SMALL( manifold->normal.x - axis.x, normalTol ); + ENSURE_SMALL( manifold->normal.y - axis.y, normalTol ); + ENSURE_SMALL( manifold->normal.z - axis.z, normalTol ); + ENSURE_SMALL( b3Dot( manifold->normal, b3Normalize( e1 ) ), normalTol ); + ENSURE_SMALL( b3Dot( manifold->normal, b3Normalize( e2 ) ), normalTol ); + + b3SegmentDistanceResult closest = b3LineDistance( p1, e1, p2, e2 ); + + // Signed gap between the edge lines along the axis, less the capsule radius + float expectedSeparation = b3Dot( axis, b3Sub( closest.point2, closest.point1 ) ) - radius; + ENSURE_SMALL( manifold->points[0].separation - expectedSeparation, sepTol ); + + // Midpoint of the closest approach, pulling the first point in by the radius + b3Vec3 expectedPoint = b3MulSV( 0.5f, b3Add( b3MulSub( closest.point1, radius, axis ), closest.point2 ) ); + ENSURE_SMALL( manifold->points[0].point.x - expectedPoint.x, pointTol ); + ENSURE_SMALL( manifold->points[0].point.y - expectedPoint.y, pointTol ); + ENSURE_SMALL( manifold->points[0].point.z - expectedPoint.z, pointTol ); + + return 0; +} + +static void HullEdgeSegment( const b3HullData* hull, int edgeIndex, b3Transform transform, b3Vec3* point, b3Vec3* edge ) +{ + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + const b3HullHalfEdge* e = edges + edgeIndex; + b3Vec3 tail = b3TransformPoint( transform, points[e->origin] ); + b3Vec3 head = b3TransformPoint( transform, points[edges[e->twin].origin] ); + *point = tail; + *edge = b3Sub( head, tail ); +} + +// Two boxes crossing edge to edge. A holds a vertical edge, B is rolled to present a crossing edge +// and yawed so the arc intersection walks off the midpoint. For every configuration that resolves +// to an edge pair the recovered axis, separation and point must match the cross product oracle to +// tight tolerance. The oracle reads the edges the solver actually latched onto, so the check is +// exact regardless of which pair wins. +static int EdgeAxisOracleTest( void ) +{ + b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisY, 0.25f * B3_PI ) ); + + float rolls[] = { 0.18f * B3_PI, 0.25f * B3_PI, 0.32f * B3_PI }; + float yaws[] = { -0.35f, -0.15f, 0.0f, 0.15f, 0.35f }; + float distances[] = { 1.38f, 1.40f, kRoot2, 1.44f }; + + int edgeContacts = 0; + + for ( int i = 0; i < ARRAY_COUNT( rolls ); ++i ) + { + b3BoxHull hullB = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisZ, rolls[i] ) ); + + for ( int j = 0; j < ARRAY_COUNT( yaws ); ++j ) + { + for ( int k = 0; k < ARRAY_COUNT( distances ); ++k ) + { + b3Transform transform = { { distances[k], 0.0f, 0.0f }, ExactQuat( kAxisY, yaws[j] ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 ) + { + continue; + } + + b3Vec3 p1, e1, p2, e2; + HullEdgeSegment( &hullA.base, cache.indexA, b3Transform_identity, &p1, &e1 ); + HullEdgeSegment( &hullB.base, cache.indexB, transform, &p2, &e2 ); + + b3Vec3 centerA = hullA.base.center; + b3Vec3 centerB = b3TransformPoint( transform, hullB.base.center ); + b3Vec3 orientRef = b3Sub( centerB, centerA ); + + if ( CheckEdgeContact( &manifold, p1, e1, p2, e2, orientRef, 0.0f, 2e-4f, 2e-4f, 2e-3f ) != 0 ) + { + return 1; + } + + ++edgeContacts; + } + } + } + + // The sweep is only meaningful if it actually drove the edge path + ENSURE( edgeContacts >= 15 ); + + return 0; +} + +// The same oracle over randomly oriented box pairs. Whenever the solver reports an edge pair the +// recovered axis must be perpendicular to both edges and match the cross product. This casts a wide +// net over the arc that the structured sweep cannot reach. +static int EdgeAxisRandomOracleTest( void ) +{ + g_seed = 246813579u; + + int edgeContacts = 0; + + for ( int i = 0; i < 2000; ++i ) + { + float angleA = NextFloat( 0.2f, 0.5f ) * B3_PI; + float angleB = NextFloat( 0.2f, 0.5f ) * B3_PI; + b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( NextDirection(), angleA ) ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + float d = NextFloat( 1.2f, 1.55f ); + b3Transform transform = { b3MulSV( d, NextDirection() ), ExactQuat( NextDirection(), angleB ) }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, transform, &cache ); + + if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 ) + { + continue; + } + + b3Vec3 p1, e1, p2, e2; + HullEdgeSegment( &hullA.base, cache.indexA, b3Transform_identity, &p1, &e1 ); + HullEdgeSegment( &hullB.base, cache.indexB, transform, &p2, &e2 ); + + // Skip crossings near parallel where the closest point solve is ill conditioned. The + // parallel rejection itself is covered by ParallelEdgeTest. + float sine = b3Length( b3Cross( b3Normalize( e1 ), b3Normalize( e2 ) ) ); + if ( sine < 0.1f ) + { + continue; + } + + b3Vec3 orientRef = b3Sub( b3TransformPoint( transform, hullB.base.center ), hullA.base.center ); + + if ( CheckEdgeContact( &manifold, p1, e1, p2, e2, orientRef, 0.0f, 1e-3f, 1e-3f, 5e-3f ) != 0 ) + { + return 1; + } + + ++edgeContacts; + } + + ENSURE( edgeContacts >= 100 ); + + return 0; +} + +// A thin capsule stabbed through the +x +y edge of a box so the edge pair is the axis of minimum +// penetration. This drives the isolated edge axis (arc versus circle on the Gauss map) that a +// capsule presents. The edge is nearly parallel to a box face normal, exactly where the old center +// based orientation flickered, so the axis, the penetration and the point are all checked. +static int HullCapsuleEdgeDeepTest( void ) +{ + b3BoxHull hull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + // The +x +y edge runs along z between the +x and +y faces + b3Vec3 edgePoint = { 0.5f, 0.5f, 0.0f }; + b3Vec3 edgeDir = { 0.0f, 0.0f, 1.0f }; + b3Vec3 outward = b3Normalize( ( b3Vec3 ){ 1.0f, 1.0f, 0.0f } ); + + // Penetrate far enough that the core segment clearly overlaps the box so the deep path runs, + // but keep the radius small enough that the edge stays the axis of minimum penetration. + float depths[] = { 0.12f, 0.18f, 0.25f }; + float radii[] = { 0.05f, 0.1f, 0.2f }; + float tilts[] = { 0.0f, 0.25f, -0.25f }; + + int count = 0; + + for ( int i = 0; i < ARRAY_COUNT( depths ); ++i ) + { + for ( int j = 0; j < ARRAY_COUNT( radii ); ++j ) + { + for ( int k = 0; k < ARRAY_COUNT( tilts ); ++k ) + { + b3Vec3 capsuleDir = b3Normalize( ( b3Vec3 ){ 1.0f, -1.0f, tilts[k] } ); + b3Vec3 mid = b3MulAdd( edgePoint, -depths[i], outward ); + b3Vec3 c1 = b3MulAdd( mid, -0.5f, capsuleDir ); + b3Vec3 c2 = b3MulAdd( mid, 0.5f, capsuleDir ); + b3Capsule capsule = { c1, c2, radii[j] }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideHullAndCapsule( &manifold, 8, &hull.base, &capsule, b3Transform_identity, &cache ); + + ENSURE( manifold.pointCount == 1 ); + ENSURE( manifold.points[0].separation < 0.0f ); + + // Hull edge is e1, capsule axis is e2, normal points out of the hull + if ( CheckEdgeContact( &manifold, edgePoint, edgeDir, c1, b3Sub( c2, c1 ), outward, radii[j], 1e-4f, 1e-4f, + 1e-4f ) != 0 ) + { + return 1; + } + + ++count; + } + } + } + + ENSURE( count == ARRAY_COUNT( depths ) * ARRAY_COUNT( radii ) * ARRAY_COUNT( tilts ) ); + + return 0; +} + +// Force the triangle versus hull edge query over a broad sweep of crossing geometries. A cube tipped +// 45 degrees rests an edge along x at y = -h*root2. A triangle edge is laid across it at a range of +// yaws, plane tips and gaps so the arc intersection lands all over the arc. The manual axis hands +// the winning pair to the builder, and the recovered axis must match the cross product of the chosen +// triangle and hull edges and point from the triangle into the hull. +static int TriangleHullEdgeSweepTest( void ) +{ + b3BoxHull hull = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisX, 0.25f * B3_PI ) ); + b3Vec3 hullEdgePoint = { 0.0f, -kHalfRoot2, 0.0f }; + + // Degrees: triangle plane tip about z, and triangle edge yaw + float betas[] = { 8.0f, 20.0f, 32.0f }; + float gammas[] = { 20.0f, 35.0f, 50.0f, 70.0f }; + float gaps[] = { 0.02f, 0.0f, -0.03f, -0.08f }; + + int edgeContacts = 0; + + for ( int a = 0; a < ARRAY_COUNT( betas ); ++a ) + { + for ( int b = 0; b < ARRAY_COUNT( gammas ); ++b ) + { + for ( int c = 0; c < ARRAY_COUNT( gaps ); ++c ) + { + float beta = betas[a] * B3_PI / 180.0f; + float gamma = gammas[b] * B3_PI / 180.0f; + + // Tip the plane off the hull edge so the Minkowski test holds, then yaw the edge + b3Vec3 triNormal = { sinf( beta ), cosf( beta ), 0.0f }; + b3Vec3 triEdge = { sinf( gamma ) * cosf( beta ), -sinf( gamma ) * sinf( beta ), cosf( gamma ) }; + + // Perpendicular to both edges and pointing out of the hull + b3Vec3 axis = b3Normalize( b3Cross( kAxisX, triEdge ) ); + b3Vec3 trianglePoint = b3MulAdd( hullEdgePoint, gaps[c], axis ); + + b3Vec3 v1 = b3MulAdd( trianglePoint, -1.0f, triEdge ); + b3Vec3 v2 = b3MulAdd( trianglePoint, 1.0f, triEdge ); + b3Vec3 v3 = b3MulAdd( v1, 1.5f, b3Cross( triNormal, triEdge ) ); + + b3Vec3 triangleVerts[] = { v1, v2, v3 }; + b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { .type = b3_manualEdgePairAxis }; + b3CollideHullAndTriangle( &manifold, 8, &hull.base, v1, v2, v3, 0, &cache, true ); + + if ( cache.type != b3_edgePairAxis || manifold.pointCount != 1 ) + { + continue; + } + + b3Vec3 p1 = triangleVerts[cache.indexA]; + b3Vec3 e1 = triangleEdges[cache.indexA]; + + b3Vec3 p2, e2; + HullEdgeSegment( &hull.base, cache.indexB, b3Transform_identity, &p2, &e2 ); + + // Normal points from the triangle into the hull + b3Vec3 orientRef = b3Sub( hull.base.center, triangleCenter ); + + if ( CheckEdgeContact( &manifold, p1, e1, p2, e2, orientRef, 0.0f, 1e-4f, 1e-4f, 1e-3f ) != 0 ) + { + return 1; + } + + ++edgeContacts; + } + } + } + + ENSURE( edgeContacts >= 30 ); + + return 0; +} + +// A capsule laid nearly in a triangle plane and pushed across one edge so the edge pair drives the +// deep contact. This exercises the two sided triangle edge, where the side normal trick chooses +// which half of the arc holds the axis. Validate every edge contact the sweep produces. +static int CapsuleTriangleEdgeDeepTest( void ) +{ + // Triangle in the y = 0 plane. The v1 v2 edge runs along x at z = 0, the interior lies at z < 0. + b3Vec3 v1 = { -2.0f, 0.0f, 0.0f }; + b3Vec3 v2 = { 2.0f, 0.0f, 0.0f }; + b3Vec3 v3 = { 0.0f, 0.0f, -2.0f }; + b3Vec3 triangle[] = { v1, v2, v3 }; + b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); + + // A nearly in plane core crossing the edge at (0,0,z0) with a small out of plane tilt. The core + // pierces the triangle just inside the edge so the deep path runs and the tilted edge pair wins. + float z0s[] = { -0.05f, -0.03f, -0.01f }; + float tilts[] = { 0.2f, 0.3f, 0.4f }; + float yaws[] = { 0.4f, 0.6f, 0.8f }; + float radii[] = { 0.05f, 0.1f }; + + int edgeContacts = 0; + + for ( int i = 0; i < ARRAY_COUNT( z0s ); ++i ) + { + for ( int j = 0; j < ARRAY_COUNT( tilts ); ++j ) + { + for ( int y = 0; y < ARRAY_COUNT( yaws ); ++y ) + { + for ( int r = 0; r < ARRAY_COUNT( radii ); ++r ) + { + b3Vec3 capsuleDir = b3Normalize( ( b3Vec3 ){ sinf( yaws[y] ), tilts[j], cosf( yaws[y] ) } ); + b3Vec3 mid = { 0.0f, 0.0f, z0s[i] }; + b3Vec3 c1 = b3MulAdd( mid, -0.6f, capsuleDir ); + b3Vec3 c2 = b3MulAdd( mid, 0.6f, capsuleDir ); + b3Capsule capsule = { c1, c2, radii[r] }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideCapsuleAndTriangle( &manifold, 8, &capsule, triangle, &cache ); + + // Only the edge contacts exercise the new axis. Face contacts are handled elsewhere. + if ( manifold.pointCount != 1 || manifold.feature < b3_featureEdge1 || manifold.feature > b3_featureEdge3 ) + { + continue; + } + + int edgeIndex = manifold.feature - b3_featureEdge1; + b3Vec3 p1 = triangle[edgeIndex]; + b3Vec3 e1 = triangleEdges[edgeIndex]; + + // Normal points from the triangle toward the capsule + b3Vec3 capsuleEdge = b3Sub( c2, c1 ); + b3Vec3 capsuleCenter = b3Lerp( c1, c2, 0.5f ); + b3Vec3 orientRef = b3Sub( capsuleCenter, triangleCenter ); + + if ( CheckEdgeContact( &manifold, p1, e1, c1, capsuleEdge, orientRef, radii[r], 1e-4f, 1e-4f, 2e-4f ) != 0 ) + { + return 1; + } + + ++edgeContacts; + } + } + } + } + + // The sweep must actually reach the edge path + ENSURE( edgeContacts >= 15 ); + + return 0; +} + +// A sphere driven straight through a box face, from separated, across the surface where the collider +// switches from GJK closest points to the SAT face pick, and on into deep overlap. The separation +// must stay the analytic gap the whole way and the normal must not flip. A jump at the seam would +// read as a pop in the solver. The sweep is fine enough to land samples on both sides of the seam. +static int SphereHullSeamTest( void ) +{ + b3BoxHull hull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + float radius = 0.15f; + + float yStart = 0.5f + radius + 0.4f * B3_SPECULATIVE_DISTANCE; + float yEnd = 0.1f; + int steps = 400; + float dy = ( yStart - yEnd ) / steps; + + float previous = 0.0f; + int shallowSamples = 0; + int deepSamples = 0; + + for ( int i = 0; i <= steps; ++i ) + { + float y = yStart - i * dy; + b3Sphere sphere = { { 0.0f, y, 0.0f }, radius }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideHullAndSphere( &manifold, 8, &hull.base, &sphere, b3Transform_identity, &cache ); + + ENSURE( manifold.pointCount == 1 ); + + float separation = manifold.points[0].separation; + float expected = ( y - 0.5f ) - radius; + + // Separation is the analytic gap on both sides of the seam + ENSURE_SMALL( separation - expected, 1e-5f ); + + // Normal holds the face direction with no flip + ENSURE_SMALL( manifold.normal.x, 1e-5f ); + ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f ); + ENSURE_SMALL( manifold.normal.z, 1e-5f ); + + // No jump across the seam: consecutive separations track the step + if ( i > 0 ) + { + ENSURE_SMALL( ( previous - separation ) - dy, 1e-5f ); + } + previous = separation; + + if ( y > 0.5f ) + { + shallowSamples += 1; + } + else + { + deepSamples += 1; + } + } + + // The sweep must straddle the surface so both the GJK and the SAT branch run + ENSURE( shallowSamples > 0 && deepSamples > 0 ); + + return 0; +} + +// The same seam for a capsule laid parallel to the face. Above the surface the shallow path clips two +// points, in overlap the face path builds two, and every point must sit at the analytic gap through +// the transition with the normal fixed on the face. +static int CapsuleHullSeamTest( void ) +{ + b3BoxHull hull = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + float radius = 0.15f; + float halfLength = 0.3f; + + float yStart = 0.5f + radius + 0.4f * B3_SPECULATIVE_DISTANCE; + float yEnd = 0.1f; + int steps = 400; + float dy = ( yStart - yEnd ) / steps; + + float previous = 0.0f; + int shallowSamples = 0; + int deepSamples = 0; + + for ( int i = 0; i <= steps; ++i ) + { + float y = yStart - i * dy; + b3Capsule capsule = { { -halfLength, y, 0.0f }, { halfLength, y, 0.0f }, radius }; + + b3LocalManifoldPoint points[8]; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideHullAndCapsule( &manifold, 8, &hull.base, &capsule, b3Transform_identity, &cache ); + + ENSURE( manifold.pointCount >= 1 ); + + float expected = ( y - 0.5f ) - radius; + + // Every point sits at the analytic gap + for ( int k = 0; k < manifold.pointCount; ++k ) + { + ENSURE_SMALL( manifold.points[k].separation - expected, 1e-5f ); + } + + // Normal holds the face direction with no flip + ENSURE_SMALL( manifold.normal.x, 1e-5f ); + ENSURE_SMALL( manifold.normal.y - 1.0f, 1e-5f ); + ENSURE_SMALL( manifold.normal.z, 1e-5f ); + + // No jump across the seam + float minSeparation = MinSeparation( &manifold ); + if ( i > 0 ) + { + ENSURE_SMALL( ( previous - minSeparation ) - dy, 1e-5f ); + } + previous = minSeparation; + + if ( y > 0.5f ) + { + shallowSamples += 1; + } + else + { + deepSamples += 1; + } + } + + ENSURE( shallowSamples > 0 && deepSamples > 0 ); + + return 0; +} + +int ManifoldTest( void ) +{ + RUN_SUBTEST( CrossedEdgeTest ); + RUN_SUBTEST( EdgeAxisScaleTest ); + RUN_SUBTEST( EdgeCacheTest ); + RUN_SUBTEST( EdgeEndpointTest ); + RUN_SUBTEST( ParallelEdgeTest ); + RUN_SUBTEST( ParallelEdgeManualTest ); + RUN_SUBTEST( OverlapNeverEmptyTest ); + RUN_SUBTEST( RidgeCrossingTest ); + RUN_SUBTEST( TriangleEdgeTest ); + RUN_SUBTEST( TriangleParallelEdgeTest ); + RUN_SUBTEST( EdgeAxisOracleTest ); + RUN_SUBTEST( EdgeAxisRandomOracleTest ); + RUN_SUBTEST( HullCapsuleEdgeDeepTest ); + RUN_SUBTEST( TriangleHullEdgeSweepTest ); + RUN_SUBTEST( CapsuleTriangleEdgeDeepTest ); + RUN_SUBTEST( SphereHullSeamTest ); + RUN_SUBTEST( CapsuleHullSeamTest ); + + return 0; +}