From ad0f805f53be29b0f3c7e17b5fdc1986c51a759c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 1 Nov 2017 23:09:02 +0100
Subject: [PATCH] Fix robustness issue in SAT Algorithm (convex polyhedron vs
 capsule)

---
 .../CapsuleVsConvexPolyhedronAlgorithm.cpp          |  5 ++++-
 src/collision/narrowphase/SAT/SATAlgorithm.cpp      | 13 +++++++++----
 src/collision/narrowphase/SAT/SATAlgorithm.h        |  2 +-
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 89336c4e..e895eef7 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -119,10 +119,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
                     }
 
                     // Compute and create two contact points
-                    satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
+                    bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
                                                               polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
                                                               capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
                                                               narrowPhaseInfo, isCapsuleShape1);
+                    if (!contactsFound) {
+                        return false;
+                    }
 
                     break;
                 }
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 1ba5143b..e6d09dd1 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -256,7 +256,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
 
         if (reportContacts) {
 
-            computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
+            return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
                                                       polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
                                                       capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
                                                       narrowPhaseInfo, isCapsuleShape1);
@@ -349,7 +349,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
 
 // Compute the two contact points between a polyhedron and a capsule when the separating
 // axis is a face normal of the polyhedron
-void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
                                                              decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
                                                              Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                              const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
@@ -391,13 +391,14 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
 
     // First we clip the inner segment of the capsule with the four planes of the adjacent faces
     std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
-    assert(clipSegment.size() == 2);
 
 	// Project the two clipped points into the polyhedron face
 	const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
 
+    bool contactFound = false;
+
 	// For each of the two clipped points
-    for (uint i = 0; i<2; i++) {
+    for (uint i = 0; i<clipSegment.size(); i++) {
 
 		// Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth)
 		const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
@@ -405,6 +406,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
 		// If the clipped point is one that produce this penetration depth, we keep it
 		if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
 
+            contactFound = true;
+
             Vector3 contactPointPolyhedron = clipSegment[i] + delta;
 
 			// Project the clipped point into the capsule bounds
@@ -426,6 +429,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
                                              isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
 		}
 	}
+
+    return contactFound;
 }
 
 // This method returns true if an edge of a polyhedron and a capsule forms a
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 37e4f7d2..5c721a00 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -126,7 +126,7 @@ class SATAlgorithm {
         bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
 
         /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
-        void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+        bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
                                                        decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
                                                        Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                        const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,