Working on temporal coherence in SAT (polyhedron vs polyhedron)

CMakeLists.txt

@@ -88,6 +88,8 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
     "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
     "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
+    "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
+    "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
     "src/collision/shapes/AABB.h"
     "src/collision/shapes/AABB.cpp"
     "src/collision/shapes/ConvexShape.h"

src/collision/CollisionDetection.cpp

@@ -268,7 +268,15 @@ void CollisionDetection::computeNarrowPhase() {
 
                 // Trigger a callback event for the new contact
                 if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
+
+                currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
             }
+            else {
+                currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false;
+            }
+
+            // The previous frame collision info is now valid
+            currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
         }
 
         currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
@@ -834,6 +842,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
                         // Report the contact to the user
                         callback->notifyContact(collisionInfo);
                     }
+
+                    // The previous frame collision info is now valid
+                    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
                 }
 
                 NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -44,6 +44,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
     SATAlgorithm satAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
 
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+
     // If we have found a contact point inside the margins (shallow penetration)
     if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
 
@@ -104,9 +107,11 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
 
                 break;
             }
-
         }
 
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+
         // Return true
         return true;
     }
@@ -115,7 +120,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
     if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
         // Run the SAT algorithm to find the separating axis and compute contact point
-        return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+        bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+
+        return isColliding;
     }
 
     return false;

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -0,0 +1,48 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
+#include "GJK/GJKAlgorithm.h"
+#include "SAT/SATAlgorithm.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Compute the narrow-phase collision detection between two convex polyhedra
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+                                                                ContactManifoldInfo& contactManifoldInfo) {
+
+    // Run the SAT algorithm to find the separating axis and compute contact point
+    SATAlgorithm satAlgorithm;
+    bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+
+    return isColliding;
+}

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h

@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+#define	REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two convex polyhedra.
+ */
+class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
+
+    protected :
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default;
+
+        /// Destructor
+        virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default;
+
+        /// Deleted copy-constructor
+        ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+        /// Deleted assignment operator
+        ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+        /// Compute the narrow-phase collision detection between two convex polyhedra
+        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+                                   ContactManifoldInfo& contactManifoldInfo) override;
+};
+
+}
+
+#endif
+

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -56,6 +56,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
     if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
         return &mSphereVsConvexPolyhedronAlgorithm;
     }
+    // Capsule vs Convex Polyhedron algorithm
+    if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+        return &mCapsuleVsConvexPolyhedronAlgorithm;
+    }
+    // Convex Polyhedron vs Convex Polyhedron algorithm
+    if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON &&
+        shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+        return &mConvexPolyhedronVsConvexPolyhedronAlgorithm;
+    }
 
     return nullptr;
 }

src/collision/narrowphase/DefaultCollisionDispatch.h

@@ -33,6 +33,8 @@
 #include "SphereVsConvexPolyhedronAlgorithm.h"
 #include "SphereVsCapsuleAlgorithm.h"
 #include "CapsuleVsCapsuleAlgorithm.h"
+#include "CapsuleVsConvexPolyhedronAlgorithm.h"
+#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
 #include "GJK/GJKAlgorithm.h"
 
 namespace reactphysics3d {
@@ -50,14 +52,20 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         /// Sphere vs Sphere collision algorithm
         SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
 
-        /// Sphere vs Convex Mesh collision algorithm
-        SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
+        /// Capsule vs Capsule collision algorithm
+        CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
 
         /// Sphere vs Capsule collision algorithm
         SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
 
-        /// Capsule vs Capsule collision algorithm
-        CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
+        /// Sphere vs Convex Polyhedron collision algorithm
+        SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
+
+        /// Capsule vs Convex Polyhedron collision algorithm
+        CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm;
+
+        /// Convex Polyhedron vs Convex Polyhedron collision algorithm
+        ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm;
 
     public:
 

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -93,7 +93,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
     VoronoiSimplex simplex;
 
     // Get the previous point V (last cached separating axis)
-    Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis();
+    Vector3 v;
+    LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+    if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
+        v = lastFrameInfo.gjkSeparatingAxis;
+    }
+    else {
+        v.setAllValues(0, 1, 0);
+    }
 
     // Initialize the upper bound for the square distance
     decimal distSquare = DECIMAL_LARGEST;
@@ -113,7 +120,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
         if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
                         
             // Cache the current separating axis for frame coherence
-            narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
+            narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v;
             
             // No intersection, we return
             return GJKResult::SEPARATED;

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -29,6 +29,7 @@
 #include "collision/PolyhedronMesh.h"
 #include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/SphereShape.h"
+#include "engine/OverlappingPair.h"
 #include "configuration.h"
 #include "engine/Profiler.h"
 #include <algorithm>
@@ -329,7 +330,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
 }
 
 // Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
+                                                                   ContactManifoldInfo& contactManifoldInfo) const {
 
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -344,15 +346,141 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
     uint minFaceIndex = 0;
     bool isMinPenetrationFaceNormal = false;
     bool isMinPenetrationFaceNormalPolyhedron1 = false;
+    uint minSeparatingEdge1Index, minSeparatingEdge2Index;
     Vector3 separatingEdge1A, separatingEdge1B;
     Vector3 separatingEdge2A, separatingEdge2B;
     Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
 
+    LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+
+    // True if the shapes were overlapping in the previous frame and are
+    // still overlapping on the same axis in this frame
+    bool isTemporalCoherenceValid = false;
+
+    // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
+    // frame collision data per triangle)
+    if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) {
+
+        // If the last frame collision info is valid and was also using SAT algorithm
+        if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+
+            // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+            // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+            // the shapes are still separated along this axis, we directly exit with no collision.
+
+            // If the previous separating axis (or axis with minimum penetration depth)
+            // was a face normal of polyhedron 1
+            if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+
+                decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
+                                                                                         lastFrameInfo.satMinAxisFaceIndex);
+                // If the previous axis is a separating axis
+                if (penetrationDepth <= decimal(0.0)) {
+
+                    // Return no collision
+                    return false;
+                }
+
+                // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+                if (isTemporalCoherenceValid) {
+
+                    minPenetrationDepth = penetrationDepth;
+                    minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+                    isMinPenetrationFaceNormal = true;
+                    isMinPenetrationFaceNormalPolyhedron1 = true;
+                }
+            }
+            else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
+                                                               // was a face normal of polyhedron 2
+
+                decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
+                                                                                         lastFrameInfo.satMinAxisFaceIndex);
+                // If the previous axis is a separating axis
+                if (penetrationDepth <= decimal(0.0)) {
+
+                    // Return no collision
+                    return false;
+                }
+
+                // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+                if (isTemporalCoherenceValid) {
+
+                    minPenetrationDepth = penetrationDepth;
+                    minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+                    isMinPenetrationFaceNormal = true;
+                    isMinPenetrationFaceNormalPolyhedron1 = false;
+                }
+            }
+            else {   // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
+
+                HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+                HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index);
+
+                // If the two edges build a minkowski face (and the cross product is
+                // therefore a candidate for separating axis
+                if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+
+                    Vector3 separatingAxisPolyhedron2Space;
+
+                    const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+                    const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+                    const Vector3 edge1Direction = edge1B - edge1A;
+                    const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+                    const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+                    const Vector3 edge2Direction = edge2B - edge2A;
+
+                    // Compute the penetration depth
+                    decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+                                                                           edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+
+                    // If the previous axis is a separating axis
+                    if (penetrationDepth <= decimal(0.0)) {
+
+                        // Return no collision
+                        return false;
+                    }
+
+                    // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                    // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                    isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+                    if (isTemporalCoherenceValid) {
+
+                        minPenetrationDepth = penetrationDepth;
+                        isMinPenetrationFaceNormal = false;
+                        isMinPenetrationFaceNormalPolyhedron1 = false;
+                        minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
+                        minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
+                        separatingEdge1A = edge1A;
+                        separatingEdge1B = edge1B;
+                        separatingEdge2A = edge2A;
+                        separatingEdge2B = edge2B;
+                        minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+                    }
+                }
+            }
+        }
+    }
+
+    // We the shapes are still overlapping in the same axis as in
+    // the previous frame, we skip the whole SAT algorithm
+    if (isTemporalCoherenceValid) {
+
         // Test all the face normals of the polyhedron 1 for separating axis
         uint faceIndex;
-    decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
+        decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
         if (penetrationDepth <= decimal(0.0)) {
 
+            lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+            lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+            lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+
             // We have found a separating axis
             return false;
         }
@@ -364,9 +492,13 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
         }
 
         // Test all the face normals of the polyhedron 2 for separating axis
-    penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
+        penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
         if (penetrationDepth <= decimal(0.0)) {
 
+            lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+            lastFrameInfo.satIsAxisFacePolyhedron2 = true;
+            lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+
             // We have found a separating axis
             return false;
         }
@@ -408,6 +540,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
 
                     if (penetrationDepth <= decimal(0.0)) {
 
+                        lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+                        lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+                        lastFrameInfo.satMinEdge1Index = i;
+                        lastFrameInfo.satMinEdge2Index = j;
+
                         // We have found a separating axis
                         return false;
                     }
@@ -417,6 +554,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
                         minPenetrationDepth = penetrationDepth;
                         isMinPenetrationFaceNormalPolyhedron1 = false;
                         isMinPenetrationFaceNormal = false;
+                        minSeparatingEdge1Index = i;
+                        minSeparatingEdge2Index = j;
                         separatingEdge1A = edge1A;
                         separatingEdge1B = edge1B;
                         separatingEdge2A = edge2A;
@@ -427,11 +566,15 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
                 }
             }
         }
+    }
+
+    // Here we know the shapes are overlapping on a given minimum separating axis.
+    // Now, we will clip the shapes along this axis to find the contact points
 
     assert(minPenetrationDepth > decimal(0.0));
     assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
 
-    // If the separation axis is a face normal
+    // If the minimum separating axis is a face normal
     if (isMinPenetrationFaceNormal) {
 
         const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
@@ -439,6 +582,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
         const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
         const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
 
+        assert(minPenetrationDepth > decimal(0.0));
+
         const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
         const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
 
@@ -491,8 +636,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
 
         } while (currentEdgeIndex != firstEdgeIndex);
 
+        assert(planesNormals.size() > 0);
+        assert(planesNormals.size() == planesPoints.size());
+
         // Clip the reference faces with the adjacent planes of the reference face
         std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+        assert(clipPolygonVertices.size() > 0);
 
         // We only keep the clipped points that are below the reference face
         const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
@@ -514,6 +663,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
                                                     isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
             }
         }
+
+        lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+        lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+        lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
     }
     else {    // If we have an edge vs edge contact
 
@@ -531,6 +684,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
         // Create the contact point
         contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
                                             closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+
+        lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+        lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+        lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
+        lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
     }
 
     return true;
@@ -583,21 +741,16 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
     return -axis.dot(edge2A - edge1A);
 }
 
-// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
-decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
+decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
                                                                     const ConvexPolyhedronShape* polyhedron2,
                                                                     const Transform& polyhedron1ToPolyhedron2,
-                                                              uint& minFaceIndex) const {
+                                                                    uint faceIndex) const {
 
-    decimal minPenetrationDepth = DECIMAL_LARGEST;
-
-    // For each face of the first polyhedron
-    for (uint f = 0; f < polyhedron1->getNbFaces(); f++) {
-
-        HalfEdgeStructure::Face face = polyhedron1->getFace(f);
+    HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
 
     // Get the face normal
-        const Vector3 faceNormal = polyhedron1->getFaceNormal(f);
+    const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex);
 
     // Convert the face normal into the local-space of polyhedron 2
     const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
@@ -609,8 +762,26 @@ decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhe
     const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
     decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
 
+    return penetrationDepth;
+}
+
+// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
+decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+                                                               const ConvexPolyhedronShape* polyhedron2,
+                                                               const Transform& polyhedron1ToPolyhedron2,
+                                                               uint& minFaceIndex) const {
+
+    decimal minPenetrationDepth = DECIMAL_LARGEST;
+
+    // For each face of the first polyhedron
+    for (uint f = 0; f < polyhedron1->getNbFaces(); f++) {
+
+        decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2,
+                                                                                 polyhedron1ToPolyhedron2, f);
+
         // If the penetration depth is negative, we have found a separating axis
         if (penetrationDepth <= decimal(0.0)) {
+            minFaceIndex = f;
             return penetrationDepth;
         }
 

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -66,6 +66,17 @@ class SATAlgorithm {
                                             const Vector3& edge1Direction, const Vector3& edge2Direction,
                                             Vector3& outSeparatingAxis) const;
 
+        /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
+        decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+                                                              const ConvexPolyhedronShape* polyhedron2,
+                                                              const Transform& polyhedron1ToPolyhedron2,
+                                                              uint faceIndex) const;
+
+
+        /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
+        decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
+                                                        const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -101,10 +112,6 @@ class SATAlgorithm {
 
         /// Test collision between two convex meshes
         bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
-        /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
-        decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
-                                                        const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
 };
 
 }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -31,7 +31,7 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
-// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
 bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
@@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
     GJKAlgorithm gjkAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
 
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+
     // If we have found a contact point inside the margins (shallow penetration)
     if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
 
@@ -53,7 +56,12 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
 
         // Run the SAT algorithm to find the separating axis and compute contact point
         SATAlgorithm satAlgorithm;
-        return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+        bool isColliding =  satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+
+        return isColliding;
     }
 
     return false;

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
-        /// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+        /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                    ContactManifoldInfo& contactManifoldInfo) override;
 };

src/engine/OverlappingPair.cpp

@@ -32,7 +32,6 @@ using namespace reactphysics3d;
 // Constructor
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
-                : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
-                  mCachedSeparatingAxis(0.0, 1.0, 0.0) {
+                : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
     
 }                               

src/engine/OverlappingPair.h

@@ -37,6 +37,51 @@ namespace reactphysics3d {
 // Type for the overlapping pair ID
 using overlappingpairid = std::pair<uint, uint>;
 
+// Structure LastFrameCollisionInfo
+/**
+ * This structure contains collision info about the last frame.
+ * This is used for temporal coherence between frames.
+ */
+struct LastFrameCollisionInfo {
+
+    /// True if we have information about the previous frame
+    bool isValid;
+
+    /// True if the two shapes were colliding in the previous frame
+    bool wasColliding;
+
+    /// True if we were using GJK algorithm to check for collision in the previous frame
+    bool wasUsingGJK;
+
+    /// True if we were using SAT algorithm to check for collision in the previous frame
+    bool wasUsingSAT;
+
+    /// True if there was a narrow-phase collision
+    /// in the previous frame
+    bool wasCollidingLastFrame;
+
+    // ----- GJK Algorithm -----
+
+    /// Previous separating axis
+    Vector3 gjkSeparatingAxis;
+
+    // SAT Algorithm
+    bool satIsAxisFacePolyhedron1;
+    bool satIsAxisFacePolyhedron2;
+    uint satMinAxisFaceIndex;
+    uint satMinEdge1Index;
+    uint satMinEdge2Index;
+
+    /// Constructor
+    LastFrameCollisionInfo() {
+
+        isValid = false;
+        wasColliding = false;
+        wasUsingSAT = false;
+        wasUsingGJK = false;
+    }
+};
+
 // Class OverlappingPair
 /**
  * This class represents a pair of two proxy collision shapes that are overlapping
@@ -54,8 +99,8 @@ class OverlappingPair {
         /// Set of persistent contact manifolds
         ContactManifoldSet mContactManifoldSet;
 
-        /// Cached previous separating axis
-        Vector3 mCachedSeparatingAxis;
+        /// Collision information about the last frame (for temporal coherence)
+        LastFrameCollisionInfo mLastFrameCollisionInfo;
 
     public:
 
@@ -83,11 +128,8 @@ class OverlappingPair {
         /// Add a contact manifold
         void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
 
-        /// Return the cached separating axis
-        Vector3 getCachedSeparatingAxis() const;
-
-        /// Set the cached separating axis
-        void setCachedSeparatingAxis(const Vector3& axis);
+        /// Return the last frame collision info
+        LastFrameCollisionInfo& getLastFrameCollisionInfo();
 
         /// Return the number of contacts in the cache
         uint getNbContactPoints() const;
@@ -124,17 +166,11 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& conta
     mContactManifoldSet.addContactManifold(contactManifoldInfo);
 }
 
-// Return the cached separating axis
-inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
-    return mCachedSeparatingAxis;
+// Return the last frame collision info
+inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
+    return mLastFrameCollisionInfo;
 }
 
-// Set the cached separating axis
-inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
-    mCachedSeparatingAxis = axis;
-}
-
-
 // Return the number of contact points in the contact manifold
 inline uint OverlappingPair::getNbContactPoints() const {
     return mContactManifoldSet.getTotalNbContactPoints();