From 44e1b12aaf257479611c2560281dcd529d4775f7 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 10 Oct 2020 00:10:05 +0200
Subject: [PATCH] =?UTF-8?q?Fix=20robustness=20issue=20with=20SphereShape?=
 =?UTF-8?q?=20vs=C2=A0SphereShape=20collision=20detection?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../narrowphase/SphereVsSphereAlgorithm.cpp   | 62 ++++++++++---------
 1 file changed, 34 insertions(+), 28 deletions(-)

diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 78f817db..1d488ad5 100755
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -56,50 +56,56 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf
         const decimal sphere2Radius = sphereShape2->getRadius();
 
         // Compute the sum of the radius
-        decimal sumRadiuses = sphere1Radius + sphere2Radius;
+        const decimal sumRadiuses = sphere1Radius + sphere2Radius;
 
         // Compute the product of the sum of the radius
-        decimal sumRadiusesProducts = sumRadiuses * sumRadiuses;
+        const decimal sumRadiusesProducts = sumRadiuses * sumRadiuses;
 
         // If the sphere collision shapes intersect
         if (squaredDistanceBetweenCenters < sumRadiusesProducts) {
 
-            // If we need to report contacts
-            if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
+            const decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
 
-                const Transform transform1Inverse = transform1.getInverse();
-                const Transform transform2Inverse = transform2.getInverse();
+            // Make sure the penetration depth is not zero (even if the previous condition test was true the penetration depth can still be
+            // zero because of precision issue of the computation at the previous line)
+            if (penetrationDepth > 0) {
 
-                decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
-                Vector3 intersectionOnBody1;
-                Vector3 intersectionOnBody2;
-                Vector3 normal;
+                // If we need to report contacts
+                if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
 
-                // If the two sphere centers are not at the same position
-                if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
+                    const Transform transform1Inverse = transform1.getInverse();
+                    const Transform transform2Inverse = transform2.getInverse();
 
-                    Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
-                    Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
+                    Vector3 intersectionOnBody1;
+                    Vector3 intersectionOnBody2;
+                    Vector3 normal;
 
-                    intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit();
-                    intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit();
-                    normal = vectorBetweenCenters.getUnit();
-                }
-                else {    // If the sphere centers are at the same position (degenerate case)
+                    // If the two sphere centers are not at the same position
+                    if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
 
-                    // Take any contact normal direction
-                    normal.setAllValues(0, 1, 0);
+                        const Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
+                        const Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
 
-                    intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal);
-                    intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal);
+                        intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit();
+                        intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit();
+                        normal = vectorBetweenCenters.getUnit();
+                    }
+                    else {    // If the sphere centers are at the same position (degenerate case)
+
+                        // Take any contact normal direction
+                        normal.setAllValues(0, 1, 0);
+
+                        intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal);
+                        intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal);
+                    }
+
+                    // Create the contact info object
+                    narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
                 }
 
-                // Create the contact info object
-                narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
+                narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
+                isCollisionFound = true;
             }
-
-            narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
-            isCollisionFound = true;
         }
     }