Add temporal coherence for convex vs triangle collision detection

src/collision/CollisionCallback.h

@@ -32,6 +32,8 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+class OverlappingPair;
+
 // Class CollisionCallback
 /**
  * This class can be used to register a callback for collision test queries.

src/collision/CollisionDetection.cpp

@@ -113,6 +113,9 @@ void CollisionDetection::computeMiddlePhase() {
         // Make all the contact manifolds and contact points of the pair obsolete
         pair->makeContactsObsolete();
 
+        // Make all the last frame collision info obsolete
+        pair->makeLastFrameCollisionInfosObsolete();
+
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
 
@@ -192,6 +195,9 @@ void CollisionDetection::computeMiddlePhase() {
                 // Not handled
                 continue;
             }
+
+            // Remove the obsolete last frame collision infos
+            pair->clearObsoleteLastFrameCollisionInfos();
         }
     }
 }
@@ -263,6 +269,8 @@ void CollisionDetection::computeNarrowPhase() {
         // If there is no collision algorithm between those two kinds of shapes, skip it
         if (narrowPhaseAlgorithm != nullptr) {
 
+            LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo();
+
             // Use the narrow-phase collision detection algorithm to check
             // if there really is a collision. If a collision occurs, the
             // notifyContact() callback method will be called.
@@ -271,14 +279,14 @@ void CollisionDetection::computeNarrowPhase() {
                 // Add the contact points as a potential contact manifold into the pair                
                 currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
 
-                currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
+                lastCollisionFrameInfo->wasColliding = true;
             }
             else {
-                currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false;
+                lastCollisionFrameInfo->wasColliding = false;
             }
 
             // The previous frame collision info is now valid
-            currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
+            lastCollisionFrameInfo->isValid = true;
         }
 
         currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
@@ -471,6 +479,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
 
     NarrowPhaseInfo* narrowPhaseInfo = nullptr;
 
+    pair->makeLastFrameCollisionInfosObsolete();
+
     // If both shapes are convex
     if ((isShape1Convex && isShape2Convex)) {
 
@@ -490,6 +500,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
         computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
     }
 
+    pair->clearObsoleteLastFrameCollisionInfos();
+
     return narrowPhaseInfo;
 }
 

src/collision/ContactManifold.h

@@ -410,5 +410,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint
 }
 
 }
+
 #endif
 

src/collision/ContactManifoldInfo.cpp

@@ -62,6 +62,9 @@ void ContactManifoldInfo::reset() {
         ContactPointInfo* elementToDelete = element;
         element = element->next;
 
+        // Call the constructor
+        elementToDelete->~ContactPointInfo();
+
         // Delete the current element
         mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
     }

src/collision/MiddlePhaseTriangleCallback.cpp

@@ -29,14 +29,12 @@
 using namespace reactphysics3d;
 
 // Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
-                                               const Vector3* verticesNormals) {
 
     // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
 	// destructor of the corresponding NarrowPhaseInfo.
     TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
-                                   TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+                                   TriangleShape(trianglePoints, verticesNormals, shapeId);
-                                                 verticesNormals, meshSubPart, triangleIndex);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/MiddlePhaseTriangleCallback.h

@@ -82,8 +82,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
         }
 
         /// Test collision between a triangle and the convex mesh shape
-        virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
+        virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
-                                  const Vector3* verticesNormals) override;
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/NarrowPhaseInfo.cpp

@@ -35,12 +35,15 @@ using namespace reactphysics3d;
 // Constructor
 NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
                 CollisionShape* shape2, const Transform& shape1Transform,
-                const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator)
+                const Transform& shape2Transform, void** cachedData1, void** cachedData2,
+                Allocator& shapeAllocator)
       : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
         shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
         contactPoints(nullptr), cachedCollisionData1(cachedData1),
-        cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) {
+        cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) {
 
+    // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
+    overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
 }
 
 // Destructor

src/collision/NarrowPhaseInfo.h

@@ -29,11 +29,12 @@
 // Libraries
 #include "shapes/CollisionShape.h"
 #include "collision/ContactManifoldInfo.h"
+#include "engine/OverlappingPair.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
 
-class OverlappingPair;
+struct LastFrameCollisionInfo;
 
 // Class NarrowPhaseInfo
 /**
@@ -70,12 +71,12 @@ struct NarrowPhaseInfo {
         // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
         void** cachedCollisionData2;
 
-		/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
-		Allocator& collisionShapeAllocator;
-
         /// Pointer to the next element in the linked list
         NarrowPhaseInfo* next;
 
+        /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
+        Allocator& collisionShapeAllocator;
+
         /// Constructor
         NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
                         CollisionShape* shape2, const Transform& shape1Transform,
@@ -93,8 +94,16 @@ struct NarrowPhaseInfo {
 
         /// Reset the remaining contact points
         void resetContactPoints();
+
+        /// Get the last collision frame info for temporal coherence
+        LastFrameCollisionInfo* getLastFrameCollisionInfo() const;
 };
 
+// Get the last collision frame info for temporal coherence
+inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const {
+    return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId());
+}
+
 }
 
 #endif

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -50,10 +50,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
 
 #endif
 
+    // Get the last frame collision info
+    LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
 
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+    lastFrameCollisionInfo->wasUsingGJK = true;
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+    lastFrameCollisionInfo->wasUsingSAT = false;
 
 	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
 		   narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -140,8 +143,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
             }
         }
 
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+        lastFrameCollisionInfo->wasUsingSAT = false;
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+        lastFrameCollisionInfo->wasUsingGJK = false;
 
         // Return true
         return true;
@@ -153,8 +156,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
         // Run the SAT algorithm to find the separating axis and compute contact point
         bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+        lastFrameCollisionInfo->wasUsingGJK = false;
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+        lastFrameCollisionInfo->wasUsingSAT = true;
 
         return isColliding;
     }

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -45,10 +45,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo*
 
 #endif
 
+    // Get the last frame collision info
+    LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
     bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+    lastFrameCollisionInfo->wasUsingSAT = true;
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+    lastFrameCollisionInfo->wasUsingGJK = false;
 
     return isColliding;
 }

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -88,11 +88,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
     // Create a simplex set
     VoronoiSimplex simplex;
 
+    // Get the last collision frame info
+    LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
     // Get the previous point V (last cached separating axis)
     Vector3 v;
-    LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+    if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
-    if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
+        v = lastFrameCollisionInfo->gjkSeparatingAxis;
-        v = lastFrameInfo.gjkSeparatingAxis;
         assert(v.lengthSquare() > decimal(0.000001));
     }
     else {
@@ -117,7 +119,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
         if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
                         
             // Cache the current separating axis for frame coherence
-            narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v;
+            lastFrameCollisionInfo->gjkSeparatingAxis = v;
             
             // No intersection, we return
             return GJKResult::SEPARATED;

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -74,7 +74,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
     decimal minPenetrationDepth = DECIMAL_LARGEST;
     uint minFaceIndex = 0;
 
-
     // For each face of the convex mesh
     for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
 
@@ -110,7 +109,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
                                                         narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
                                                         minPenetrationDepth, normalWorld);
 
-
         // Create the contact info object
         narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                          isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
@@ -468,156 +466,152 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
     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
+    LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
-    // frame collision data per triangle)
-    if (polyhedron1->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) {
 
-        // If the last frame collision info is valid and was also using SAT algorithm
+    // If the last frame collision info is valid and was also using SAT algorithm
-        if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+    if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
 
-            // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+        // 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
+        // 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.
+        // 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)
+        // If the previous separating axis (or axis with minimum penetration depth)
-            // was a face normal of polyhedron 1
+        // was a face normal of polyhedron 1
-            if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+        if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) {
 
-                decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
+            decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
-                                                                                         lastFrameInfo.satMinAxisFaceIndex);
+                                                 lastFrameCollisionInfo->satMinAxisFaceIndex);
-                // If the previous axis is a separating axis
+            // If the previous axis is a separating axis
-                if (penetrationDepth <= decimal(0.0)) {
+            if (penetrationDepth <= decimal(0.0)) {
 
-                    // Return no collision
+                // Return no collision
-                    return false;
+                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;
-
-                    // Compute the contact points between two faces of two convex polyhedra.
-                    // If contact points have been found, we report them without running the whole SAT algorithm
-                    if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
-                                                                      polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
-                                                                      narrowPhaseInfo, minPenetrationDepth)) {
-
-                        lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
-                        lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
-                        lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
-
-                        return true;
-                    }
-                    else {     // Contact points have not been found (the set of clipped points was empty)
-
-                        // Therefore, we need to run the whole SAT algorithm again
-                        isTemporalCoherenceValid = false;
-                    }
-                }
             }
-            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,
+            // The two shapes are overlapping as in the previous frame and on the same axis, therefore
-                                                                                         lastFrameInfo.satMinAxisFaceIndex);
+            // we will skip the entire SAT algorithm because the minimum separating axis did not change
-                // If the previous axis is a separating axis
+            isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
-                if (penetrationDepth <= decimal(0.0)) {
 
-                    // Return no collision
+            if (isTemporalCoherenceValid) {
-                    return false;
+
+                minPenetrationDepth = penetrationDepth;
+                minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
+                isMinPenetrationFaceNormal = true;
+                isMinPenetrationFaceNormalPolyhedron1 = true;
+
+                // Compute the contact points between two faces of two convex polyhedra.
+                // If contact points have been found, we report them without running the whole SAT algorithm
+                if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+                                          polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+                                          narrowPhaseInfo, minPenetrationDepth)) {
+
+                    lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+                    lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+                    lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
+
+                    return true;
                 }
+                else {     // Contact points have not been found (the set of clipped points was empty)
 
-                // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                    // Therefore, we need to run the whole SAT algorithm again
-                // 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;
-
-                    // Compute the contact points between two faces of two convex polyhedra.
-                    // If contact points have been found, we report them without running the whole SAT algorithm
-                    if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
-                                                                      polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
-                                                                      narrowPhaseInfo, minPenetrationDepth)) {
-
-                        lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
-                        lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
-                        lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
-
-                        return true;
-                    }
-                    else {     // Contact points have not been found (the set of clipped points was empty)
-
-                        // Therefore, we need to run the whole SAT algorithm again
-                        isTemporalCoherenceValid = 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);
-
-                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;
-
-                // Temporal coherence is valid only if the two edges build a minkowski
-                // face (and the cross product is therefore a candidate for separating axis
-                if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
                     isTemporalCoherenceValid = false;
                 }
+            }
+        }
+        else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
+                                   // was a face normal of polyhedron 2
 
-                if (isTemporalCoherenceValid) {
+            decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
+                                                 lastFrameCollisionInfo->satMinAxisFaceIndex);
 
-                    minPenetrationDepth = penetrationDepth;
+            // If the previous axis is a separating axis
-                    isMinPenetrationFaceNormal = false;
+            if (penetrationDepth <= decimal(0.0)) {
-                    isMinPenetrationFaceNormalPolyhedron1 = false;
+
-                    minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
+                // Return no collision
-                    minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
+                return false;
-                    separatingEdge1A = edge1A;
+            }
-                    separatingEdge1B = edge1B;
+
-                    separatingEdge2A = edge2A;
+            // The two shapes are overlapping as in the previous frame and on the same axis, therefore
-                    separatingEdge2B = edge2B;
+            // we will skip the entire SAT algorithm because the minimum separating axis did not change
-                    minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+            isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
+
+            if (isTemporalCoherenceValid) {
+
+                minPenetrationDepth = penetrationDepth;
+                minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
+                isMinPenetrationFaceNormal = true;
+                isMinPenetrationFaceNormalPolyhedron1 = false;
+
+                // Compute the contact points between two faces of two convex polyhedra.
+                // If contact points have been found, we report them without running the whole SAT algorithm
+                if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+                                          polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+                                          narrowPhaseInfo, minPenetrationDepth)) {
+
+                    lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+                    lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+                    lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
+
+                    return true;
                 }
+                else {     // Contact points have not been found (the set of clipped points was empty)
+
+                    // Therefore, we need to run the whole SAT algorithm again
+                    isTemporalCoherenceValid = 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(lastFrameCollisionInfo->satMinEdge1Index);
+            HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
+
+            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 = lastFrameCollisionInfo->wasColliding;
+
+            // Temporal coherence is valid only if the two edges build a minkowski
+            // face (and the cross product is therefore a candidate for separating axis
+            if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+                isTemporalCoherenceValid = false;
+            }
+
+            if (isTemporalCoherenceValid) {
+
+                minPenetrationDepth = penetrationDepth;
+                isMinPenetrationFaceNormal = false;
+                isMinPenetrationFaceNormalPolyhedron1 = false;
+                minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index;
+                minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index;
+                separatingEdge1A = edge1A;
+                separatingEdge1B = edge1B;
+                separatingEdge2A = edge2A;
+                separatingEdge2B = edge2B;
+                minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
             }
         }
     }
@@ -631,9 +625,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
         decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
         if (penetrationDepth <= decimal(0.0)) {
 
-            lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+            lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
-            lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+            lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
-            lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+            lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
 
             // We have found a separating axis
             return false;
@@ -649,9 +643,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
         penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
         if (penetrationDepth <= decimal(0.0)) {
 
-            lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+            lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
-            lastFrameInfo.satIsAxisFacePolyhedron2 = true;
+            lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
-            lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+            lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
 
             // We have found a separating axis
             return false;
@@ -694,10 +688,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
 
                     if (penetrationDepth <= decimal(0.0)) {
 
-                        lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+                        lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
-                        lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+                        lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
-                        lastFrameInfo.satMinEdge1Index = i;
+                        lastFrameCollisionInfo->satMinEdge1Index = i;
-                        lastFrameInfo.satMinEdge2Index = j;
+                        lastFrameCollisionInfo->satMinEdge2Index = j;
 
                         // We have found a separating axis
                         return false;
@@ -742,18 +736,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
 			// because of a numerical issue
 			if (!contactsFound) {
 				
-				lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+                lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
-				lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+                lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
-				lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+                lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
 
 				// Return no collision
 				return false;
 			}
         }
 
-        lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+        lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
-        lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+        lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
-        lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+        lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
     }
     else {    // If we have an edge vs edge contact
 
@@ -781,10 +775,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                                              closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
         }
 
-        lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+        lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
-        lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+        lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
-        lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
+        lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index;
-        lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
+        lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
     }
 
     return true;

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
         narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
 
+    // Get the last frame collision info
+    LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
 
@@ -52,8 +55,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
 
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
 
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+    lastFrameCollisionInfo->wasUsingGJK = true;
-    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+    lastFrameCollisionInfo->wasUsingSAT = false;
 
     // If we have found a contact point inside the margins (shallow penetration)
     if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@@ -76,8 +79,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
 
         bool isColliding =  satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+        lastFrameCollisionInfo->wasUsingGJK = false;
-        narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+        lastFrameCollisionInfo->wasUsingSAT = true;
 
         return isColliding;
     }

src/collision/shapes/CollisionShape.cpp

@@ -33,7 +33,7 @@ using namespace reactphysics3d;
 
 // Constructor
 CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
-               : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) {
+               : mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) {
     
 }
 

src/collision/shapes/CollisionShape.h

@@ -71,6 +71,9 @@ class CollisionShape {
         /// Scaling vector of the collision shape
         Vector3 mScaling;
 
+        /// Unique identifier of the shape inside an overlapping pair
+        uint mId;
+
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -126,6 +129,9 @@ class CollisionShape {
         /// Set the local scaling vector of the collision shape
         virtual void setLocalScaling(const Vector3& scaling);
 
+        /// Return the id of the shape
+        uint getId() const;
+
         /// Return the local inertia tensor of the collision shapes
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
 
@@ -171,6 +177,11 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
     mScaling = scaling;
 }
 
+// Return the id of the shape
+inline uint CollisionShape::getId() const {
+   return mId;
+}
+
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler

src/collision/shapes/ConcaveMeshShape.cpp

@@ -135,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
     return raycastCallback.getIsHit();
 }
 
+// Compute the shape Id for a given triangle of the mesh
+uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const {
+
+    uint shapeId = 0;
+
+    uint i=0;
+    while (i < subPart) {
+
+        shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles();
+
+        i++;
+    }
+
+    return shapeId + triangleIndex;
+}
+
 // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
 decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
 
@@ -162,9 +178,9 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
         // Get the vertices normals of the triangle
         Vector3 verticesNormals[3];
         mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
+
         // Create a triangle collision shape
-        TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+        TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
-                                    verticesNormals, data[0], data[1]);
         triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
 		
 #ifdef IS_PROFILING_ACTIVE
@@ -194,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
             smallestHitFraction = raycastInfo.hitFraction;
             mIsHit = true;
         }
+
     }
 }

src/collision/shapes/ConcaveMeshShape.h

@@ -155,6 +155,9 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Return the three vertex normals (in the array outVerticesNormals) of a triangle
         void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
 
+        /// Compute the shape Id for a given triangle of the mesh
+        uint computeTriangleShapeId(uint subPart, uint triangleIndex) const;
+
     public:
 
         /// Constructor
@@ -259,7 +262,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
     mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
 
     // Call the callback to test narrow-phase collision with this triangle
-    mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals);
+    mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
 }
 
 #ifdef IS_PROFILING_ACTIVE

src/collision/shapes/ConcaveShape.h

@@ -46,8 +46,7 @@ class TriangleCallback {
         virtual ~TriangleCallback() = default;
 
         /// Report a triangle
-        virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+        virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0;
-                                  const Vector3* trianglePoints, const Vector3* verticesNormals)=0;
 
 };
 

src/collision/shapes/HeightFieldShape.cpp

@@ -154,7 +154,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
            Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal};
 
            // Test collision against the first triangle
-           callback.testTriangle(0, 0, trianglePoints, verticesNormals1);
+           callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0));
 
            // Generate the second triangle for the current grid rectangle
            trianglePoints[0] = p3;
@@ -174,7 +174,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
            Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal};
 
            // Test collision against the second triangle
-           callback.testTriangle(0, 0, trianglePoints, verticesNormals2);
+           callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1));
        }
    }
 }
@@ -263,12 +263,10 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
 }
 
 // Raycast test between a ray and a triangle of the heightfield
-void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
-                                           const Vector3* verticesNormals) {
 
     // Create a triangle collision shape
-    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+    TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId);
-                                verticesNormals, meshSubPart, triangleIndex);
     triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
 
 #ifdef IS_PROFILING_ACTIVE

src/collision/shapes/HeightFieldShape.h

@@ -71,8 +71,7 @@ class TriangleOverlapCallback : public TriangleCallback {
         bool getIsHit() const {return mIsHit;}
 
         /// Raycast test between a ray and a triangle of the heightfield
-        virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+        virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
-                                  const Vector3* trianglePoints, const Vector3* verticesNormals) override;
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -169,6 +168,9 @@ class HeightFieldShape : public ConcaveShape {
         /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide
         void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
 
+        /// Compute the shape Id for a given triangle
+        uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const;
+
     public:
 
         /// Constructor
@@ -270,6 +272,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
                         0, 0, mass);
 }
 
+// Compute the shape Id for a given triangle
+inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const {
+
+    return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement;
+}
+
 }
 #endif
 

src/collision/shapes/SphereShape.cpp

@@ -35,7 +35,8 @@ using namespace reactphysics3d;
 /**
  * @param radius Radius of the sphere (in meters)
  */
-SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
+SphereShape::SphereShape(decimal radius)
+            : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
     assert(radius > decimal(0.0));
 }
 

src/collision/shapes/TriangleShape.cpp

@@ -43,16 +43,15 @@ using namespace reactphysics3d;
  * @param verticesNormals The three vertices normals for smooth mesh collision
  * @param margin The collision margin (in meters) around the collision shape
  */
-TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
+TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId)
-                             const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex)
+              : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) {
-              : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
 
-    mPoints[0] = point1;
+    mPoints[0] = vertices[0];
-    mPoints[1] = point2;
+    mPoints[1] = vertices[1];
-    mPoints[2] = point3;
+    mPoints[2] = vertices[2];
 
     // Compute the triangle normal
-    mNormal = (point2 - point1).cross(point3 - point1);
+    mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
     mNormal.normalize();
 
     mVerticesNormals[0] = verticesNormals[0];
@@ -60,6 +59,8 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
     mVerticesNormals[2] = verticesNormals[2];
 
     mRaycastTestType = TriangleRaycastSide::FRONT;
+
+    mId = shapeId;
 }
 
 // This method compute the smooth mesh contact with a triangle in case one of the two collision

src/collision/shapes/TriangleShape.h

@@ -72,12 +72,6 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Raycast test type for the triangle (front, back, front-back)
         TriangleRaycastSide mRaycastTestType;
 
-        /// Index of the mesh sub part in the original mesh
-        uint mMeshSubPart;
-
-        /// Triangle index of the triangle in the sub mesh
-        uint mTriangleIndex;
-
         // -------------------- Methods -------------------- //
 
         /// Return a local support point in a given direction without the object margin
@@ -95,6 +89,9 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
 
+        /// Generate the id of the shape (used for temporal coherence)
+        void generateId();
+
         // -------------------- Methods -------------------- //
 
         /// This method implements the technique described in Game Physics Pearl book
@@ -107,8 +104,7 @@ class TriangleShape : public ConvexPolyhedronShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
+        TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId);
-                      const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex);
 
         /// Destructor
         virtual ~TriangleShape() override = default;
@@ -164,12 +160,6 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Return the centroid of the polyhedron
         virtual Vector3 getCentroid() const override;
 
-        /// Return the index of the sub part mesh of the original mesh
-        uint getMeshSubPart() const;
-
-        /// Return the triangle index in the original mesh
-        uint getTriangleIndex() const;
-
         /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
         static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
                                                      Vector3& localContactPointShape1, Vector3& localContactPointShape2,
@@ -319,16 +309,6 @@ inline Vector3 TriangleShape::getCentroid() const {
     return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
 }
 
-// Return the index of the sub part mesh of the original mesh
-inline uint TriangleShape::getMeshSubPart() const {
-    return mMeshSubPart;
-}
-
-// Return the triangle index in the original mesh
-inline uint TriangleShape::getTriangleIndex() const {
-    return mTriangleIndex;
-}
-
 // Return the number of half-edges of the polyhedron
 inline uint TriangleShape::getNbHalfEdges() const {
     return 6;

src/constraint/ContactPoint.h

@@ -28,7 +28,6 @@
 
 // Libraries
 #include "body/CollisionBody.h"
-#include "collision/NarrowPhaseInfo.h"
 #include "collision/ContactPointInfo.h"
 #include "configuration.h"
 #include "mathematics/mathematics.h"
@@ -36,6 +35,8 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+struct NarrowPhaseInfo;
+
 // Class ContactPoint
 /**
  * This class represents a collision contact point between two

src/engine/CollisionWorld.h

@@ -259,4 +259,4 @@ inline void CollisionWorld::setProfilerName(std::string name) {
 
 }
 
- #endif
+#endif

src/engine/OverlappingPair.cpp

@@ -27,20 +27,31 @@
 #include <cassert>
 #include "OverlappingPair.h"
 #include "collision/ContactManifoldInfo.h"
+#include "collision/NarrowPhaseInfo.h"
 
 using namespace reactphysics3d;
 
 // Constructor
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                                 Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator)
+                                 Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator)
-                : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
+                : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr),
-                  mTempMemoryAllocator(temporaryMemoryAllocator) {
+                  mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) {
     
 }         
 
 // Destructor
 OverlappingPair::~OverlappingPair() {
 	assert(mPotentialContactManifolds == nullptr);
+
+    // Remove all the remaining last frame collision info
+    for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) {
+
+        // Call the constructor
+        it->second->~LastFrameCollisionInfo();
+
+        // Release memory
+        mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
+    }
 }
 
 // Create a new potential contact manifold using contact-points from narrow-phase
@@ -137,3 +148,59 @@ void OverlappingPair::reducePotentialContactManifolds() {
         manifold = manifold->getNext();
     }
 }
+
+
+// Add a new last frame collision info if it does not exist for the given shapes already
+void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) {
+
+    // Try to get the corresponding last frame collision info
+    auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2));
+
+    // If there is no collision info for those two shapes already
+    if (it == mLastFrameCollisionInfos.end()) {
+
+        // Create a new collision info
+        LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo)))
+                                                LastFrameCollisionInfo();
+
+        // Add it into the map of collision infos
+        std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it;
+        auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo));
+        assert(ret.second);
+    }
+    else {
+
+       // The existing collision info is not obsolete
+       it->second->isObsolete = false;
+    }
+}
+
+
+// Delete all the obsolete last frame collision info
+void OverlappingPair::clearObsoleteLastFrameCollisionInfos() {
+
+    // For each collision info
+    for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) {
+
+        // If the collision info is obsolete
+        if (it->second->isObsolete) {
+
+            // Delete it
+            it->second->~LastFrameCollisionInfo();
+            mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
+
+            mLastFrameCollisionInfos.erase(it++);
+        }
+        else {
+            ++it;
+        }
+    }
+}
+
+// Make all the last frame collision infos obsolete
+void OverlappingPair::makeLastFrameCollisionInfosObsolete() {
+
+    for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) {
+        it->second->isObsolete = true;
+    }
+}

src/engine/OverlappingPair.h

@@ -30,6 +30,7 @@
 #include "collision/ContactManifoldSet.h"
 #include "collision/ProxyShape.h"
 #include "collision/shapes/CollisionShape.h"
+#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -47,6 +48,9 @@ struct LastFrameCollisionInfo {
     /// True if we have information about the previous frame
     bool isValid;
 
+    /// True if the frame info is obsolete (the collision shape are not overlapping in middle phase)
+    bool isObsolete;
+
     /// True if the two shapes were colliding in the previous frame
     bool wasColliding;
 
@@ -72,6 +76,7 @@ struct LastFrameCollisionInfo {
     LastFrameCollisionInfo() {
 
         isValid = false;
+        isObsolete = false;
         wasColliding = false;
         wasUsingSAT = false;
         wasUsingGJK = false;
@@ -97,12 +102,19 @@ class OverlappingPair {
         /// Set of persistent contact manifolds
         ContactManifoldSet mContactManifoldSet;
 
-        /// Collision information about the last frame (for temporal coherence)
+        /// Temporal coherence collision data for each overlapping collision shapes of this pair.
-        LastFrameCollisionInfo mLastFrameCollisionInfo;
+        /// Temporal coherence data store collision information about the last frame.
+        /// If two convex shapes overlap, we have a single collision data but if one shape is concave,
+        /// we might have collision data for several overlapping triangles. The key in the map is the
+        /// shape Ids of the two collision shapes.
+        std::map<std::pair<uint, uint>, LastFrameCollisionInfo*> mLastFrameCollisionInfos;
 
         /// Linked-list of potential contact manifold
         ContactManifoldInfo* mPotentialContactManifolds;
 
+        /// Persistent memory allocator
+        Allocator& mPersistentAllocator;
+
         /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
         Allocator& mTempMemoryAllocator;
 
@@ -111,8 +123,8 @@ class OverlappingPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
+        OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,  Allocator& persistentMemoryAllocator,
-                        Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
+                        Allocator& temporaryMemoryAllocator);
 
         /// Destructor
         ~OverlappingPair();
@@ -130,7 +142,7 @@ class OverlappingPair {
         ProxyShape* getShape2() const;
 
         /// Return the last frame collision info
-        LastFrameCollisionInfo& getLastFrameCollisionInfo();
+        LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds);
 
         /// Return the a reference to the contact manifold set
         const ContactManifoldSet& getContactManifoldSet();
@@ -168,6 +180,18 @@ class OverlappingPair {
         /// Reduce the contact manifolds that have too many contact points
         void reduceContactManifolds();
 
+        /// Add a new last frame collision info if it does not exist for the given shapes already
+        void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2);
+
+        /// Return the last frame collision info for a given pair of shape ids
+        LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const;
+
+        /// Delete all the obsolete last frame collision info
+        void clearObsoleteLastFrameCollisionInfos();
+
+        /// Make all the last frame collision infos obsolete
+        void makeLastFrameCollisionInfosObsolete();
+
         /// Return the pair of bodies index
         static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
 
@@ -194,9 +218,14 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta
     mContactManifoldSet.addContactManifold(contactManifoldInfo);
 }
 
-// Return the last frame collision info
+// Return the last frame collision info for a given shape id or nullptr if none is found
-inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
+inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds) {
-    return mLastFrameCollisionInfo;
+    std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds);
+    if (it != mLastFrameCollisionInfos.end()) {
+        return it->second;
+    }
+
+    return nullptr;
 }
 
 // Return the contact manifold
@@ -265,6 +294,11 @@ inline void OverlappingPair::reduceContactManifolds() {
    mContactManifoldSet.reduce();
 }
 
+// Return the last frame collision info for a given pair of shape ids
+inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const {
+    return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2));
+}
+
 }
 
 #endif

test/tests/collision/TestRaycast.h

@@ -198,11 +198,12 @@ class TestRaycast : public Test {
             mSphereShape = new SphereShape(3);
             mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform);
 
-            const Vector3 triangleVertex1(100, 100, 0);
+            Vector3 triangleVertices[3];
-            const Vector3 triangleVertex2(105, 100, 0);
+            triangleVertices[0] = Vector3(100, 100, 0);
-            const Vector3 triangleVertex3(100, 103, 0);
+            triangleVertices[1] = Vector3(105, 100, 0);
+            triangleVertices[2] = Vector3(100, 103, 0);
             Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)};
-            mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0);
+            mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0);
             mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
 
             mCapsuleShape = new CapsuleShape(2, 5);