Update CapsuleVsCapsuleAlgorithm.cpp

restored previous change

Author: robertocapuano

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -182,24 +182,19 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseI
                 if (closestPointsDistanceSquare > MACHINE_EPSILON) {
 
                     decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
-                    decimal penetrationDepth = sumRadius - closestPointsDistance;
+                    closestPointsSeg1ToSeg2 /= closestPointsDistance;
 
-                    // 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) {
+                    const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius);
+                    const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius;
 
-                        closestPointsSeg1ToSeg2 /= closestPointsDistance;
+                    const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
 
-                        const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius);
-                        const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius;
+                    decimal penetrationDepth = std::max(sumRadius - closestPointsDistance, MACHINE_EPSILON);
 
-                        const Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
-
-                        // Create the contact info object
-                        narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
-                    }
+                    // Create the contact info object
+                    narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
                 }
-                else if (sumRadius > 0){ // The segment are overlapping (degenerate case)
+                else { // The segment are overlapping (degenerate case)
 
                     // If the capsule segments are parralel
                     if (areCapsuleInnerSegmentsParralel) {