From abfa55a269e378cd217d6300f60e7572f319f621 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 5 Feb 2024 22:32:08 +0100
Subject: [PATCH] Fix issue with missing collision between capsule and triangle
 edge if edge is parallel to capsule segment

---
 CHANGELOG.md                                  |  1 +
 .../CapsuleVsConvexPolyhedronAlgorithm.cpp    | 23 ++++---------------
 .../narrowphase/SAT/SATAlgorithm.cpp          |  4 ++++
 3 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7f3a0e07..fdc3c99e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -61,6 +61,7 @@
 - Issue [#237](https://github.com/DanielChappuis/reactphysics3d/issues/237) Wrong assert has been removed 
 - Issue [#239](https://github.com/DanielChappuis/reactphysics3d/issues/239) Memory allocation alignment
 - Issue [#240](https://github.com/DanielChappuis/reactphysics3d/issues/240) Uninitialized variable
+- Issue [#347](https://github.com/DanielChappuis/reactphysics3d/issues/347) Missing collision between capsule and triangle edge in some case
 - Issue with edge vs edge collision detection for BoxShape, ConvexMeshShape, ConcaveMeshShape and HeightFieldShape (SAT algorithm)
 - Compilation error on Clang 19
 
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 347eae65..c550fb1a 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -82,10 +82,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
             // If we need to report contacts
             if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
 
-                bool noContact = false;
-
-                // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
-                // capsule inner segment and parallel to the contact point normal, we would like to create
+                // GJK has found a shallow contact. If the normal of face of the polyhedron mesh is orthogonal to the
+                // capsule inner segment (the face normal is parallel to the contact point normal), we would like to create
                 // two contact points instead of a single one (as in the deep contact case with SAT algorithm)
 
                 // Get the contact point created by GJK
@@ -116,13 +114,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
                     bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint.normal) > decimal(0.0);
                     bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
 
-                    // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
+                    // If the polyhedron face normal is orthogonal to the capsule inner segment (the face normal is parallel to the contact point normal) and the face normal
                     // is in direction of the contact normal (from the polyhedron point of view).
                     if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
                         && areParallelVectors(faceNormalWorld, contactPoint.normal)) {
 
                         // Remove the previous contact point computed by GJK
-                        narrowPhaseInfoBatch.resetContactPoints(batchIndex);
+                        //narrowPhaseInfoBatch.resetContactPoints(batchIndex);
 
                         const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
                         const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@@ -147,24 +145,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
                                                                   polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
                                                                   capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
                                                                   narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
-                        if (!contactsFound) {
-                            noContact = true;
-                            narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = false;
+                        if (contactsFound) {
                             break;
                         }
-
-                        break;
                     }
                 }
-
-                if (noContact) {
-                    continue;
-                }
             }
 
-            lastFrameCollisionInfo->wasUsingSAT = false;
-            lastFrameCollisionInfo->wasUsingGJK = false;
-
             // Colision found
             narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
             isCollisionFound = true;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 48cc3589..5da41fe4 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -438,6 +438,10 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint32 referenceFac
 		// If the clipped point is one that produce this penetration depth, we keep it
 		if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
 
+            if (!contactFound) {
+                narrowPhaseInfoBatch.resetContactPoints(batchIndex);
+            }
+
             contactFound = true;
 
             Vector3 contactPointPolyhedron = clipSegment[i] + delta;