Working on temporal coherence in SAT algorithm

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -71,20 +71,56 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
     decimal minPenetrationDepth = DECIMAL_LARGEST;
     uint minFaceIndex = 0;
 
+    // True if the shapes were overlapping in the previous frame and are
+    // still overlapping on the same axis in this frame
+    bool isTemporalCoherenceValid = false;
+
+    LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+
+    // 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.
+
+        // Compute the penetration depth of the shapes along the face normal direction
+        decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+                                                                                 sphere, sphereCenter);
+
+        // 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;
+        }
+    }
+
+    // We the shapes are still overlapping in the same axis as in
+    // the previous frame, we skip the whole SAT algorithm
+    if (!isTemporalCoherenceValid) {
+
         // For each face of the convex mesh
         for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
 
-        // Get the face
+            // Compute the penetration depth of the shapes along the face normal direction
-        HalfEdgeStructure::Face face = polyhedron->getFace(f);
+            decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
-
-        // Get the face normal
-        const Vector3 faceNormal = polyhedron->getFaceNormal(f);
-
-        Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
-        decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
 
             // If the penetration depth is negative, we have found a separating axis
             if (penetrationDepth <= decimal(0.0)) {
+
+                lastFrameInfo.satMinAxisFaceIndex = f;
+
                 return false;
             }
 
@@ -94,6 +130,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
                 minFaceIndex = f;
             }
         }
+    }
 
     const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
     Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
@@ -109,9 +146,27 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
                                         isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
                                         isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
 
+    lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+
     return true;
 }
 
+// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
+decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
+                                                                    const SphereShape* sphere, const Vector3& sphereCenter) const {
+
+    // Get the face
+    HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex);
+
+    // Get the face normal
+    const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex);
+
+    Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
+    decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
+
+    return penetrationDepth;
+}
+
 // Test collision between a capsule and a convex mesh
 bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
@@ -131,6 +186,11 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
 
     const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
 
+    // Compute the end-points of the inner segment of the capsule
+    const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+    const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+    const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
+
     // Minimum penetration depth
     decimal minPenetrationDepth = DECIMAL_LARGEST;
     uint minFaceIndex = 0;
@@ -140,24 +200,108 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
     Vector3 separatingPolyhedronEdgeVertex1;
     Vector3 separatingPolyhedronEdgeVertex2;
 
+    // True if the shapes were overlapping in the previous frame and are
+    // still overlapping on the same axis in this frame
+    bool isTemporalCoherenceValid = false;
+
+    LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+
+    // 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 minimum separation axis was a face normal of the polyhedron
+        if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+
+            Vector3 outFaceNormalCapsuleSpace;
+
+            // Compute the penetration depth along the polyhedron face normal direction
+            const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+                                                                                            capsuleShape, polyhedronToCapsuleTransform,
+                                                                                            outFaceNormalCapsuleSpace);
+
+            // 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;
+                separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+            }
+        }
+        else {   // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
+
+            // Get an edge from the polyhedron (convert it into the capsule local-space)
+            HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+            const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+            const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+            const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+
+            Vector3 outAxis;
+
+            // Compute the penetration depth along this axis
+            const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+                                                                                              capsuleSegmentAxis, edgeVertex1,
+                                                                                              edgeDirectionCapsuleSpace,
+                                                                                              polyhedronToCapsuleTransform,
+                                                                                              outAxis);
+
+            // 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;
+                minEdgeIndex = lastFrameInfo.satMinEdge1Index;
+                isMinPenetrationFaceNormal = false;
+                separatingAxisCapsuleSpace = outAxis;
+                separatingPolyhedronEdgeVertex1 = edgeVertex1;
+                separatingPolyhedronEdgeVertex2 = edgeVertex2;
+            }
+        }
+    }
+
+    // We the shapes are still overlapping in the same axis as in
+    // the previous frame, we skip the whole SAT algorithm
+    if (!isTemporalCoherenceValid) {
+
         // For each face of the convex mesh
         for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
 
-        // Get the face
+            Vector3 outFaceNormalCapsuleSpace;
-        HalfEdgeStructure::Face face = polyhedron->getFace(f);
 
-        // Get the face normal
+            // Compute the penetration depth
-        const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+            const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape,
-
+                                                                                            polyhedronToCapsuleTransform,
-        // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
+                                                                                            outFaceNormalCapsuleSpace);
-        const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
-        const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr);
-        const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
-        const Vector3 capsuleSupportPointToFacePoint =  pointOnPolyhedronFace - capsuleSupportPoint;
-        const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
 
             // If the penetration depth is negative, we have found a separating axis
             if (penetrationDepth <= decimal(0.0)) {
+
+                lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+                lastFrameInfo.satMinAxisFaceIndex = f;
+
                 return false;
             }
 
@@ -166,15 +310,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
                 minPenetrationDepth = penetrationDepth;
                 minFaceIndex = f;
                 isMinPenetrationFaceNormal = true;
-            separatingAxisCapsuleSpace = faceNormalCapsuleSpace;
+                separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
             }
         }
 
-    // Compute the end-points of the inner segment of the capsule
-    const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
-    const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
-    const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
-
         // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
         for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
 
@@ -191,29 +330,21 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
             // Check using the Gauss Map if this edge cross product can be as separating axis
             if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
 
-            // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
+                Vector3 outAxis;
-            Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
-
-            // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
-            if (axis.lengthSquare() >= decimal(0.00001)) {
-
-                const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
-                const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
-
-                // Swap axis direction if necessary such that it points out of the polyhedron
-                if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
-                    axis = -axis;
-                }
-
-                axis.normalize();
 
                 // Compute the penetration depth
-                const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr);
+                const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
-                const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
+                                                                                                  capsuleSegmentAxis, edgeVertex1,
-                const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis);
+                                                                                                  edgeDirectionCapsuleSpace,
+                                                                                                  polyhedronToCapsuleTransform,
+                                                                                                  outAxis);
 
                 // If the penetration depth is negative, we have found a separating axis
                 if (penetrationDepth <= decimal(0.0)) {
+
+                    lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+                    lastFrameInfo.satMinEdge1Index = e;
+
                     return false;
                 }
 
@@ -222,12 +353,13 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
                     minPenetrationDepth = penetrationDepth;
                     minEdgeIndex = e;
                     isMinPenetrationFaceNormal = false;
-                    separatingAxisCapsuleSpace = axis;
+                    separatingAxisCapsuleSpace = outAxis;
                     separatingPolyhedronEdgeVertex1 = edgeVertex1;
                     separatingPolyhedronEdgeVertex2 = edgeVertex2;
                 }
             }
         }
+
     }
 
     // Convert the inner capsule segment points into the polyhedron local-space
@@ -246,6 +378,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
                                                   polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
                                                   capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
                                                   contactManifoldInfo, isCapsuleShape1);
+
+         lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+         lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
     }
     else {   // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
 
@@ -264,11 +399,68 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
         contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
                                             isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
                                             isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+
+         lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+         lastFrameInfo.satMinEdge1Index = minEdgeIndex;
     }
 
     return true;
 }
 
+// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
+decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
+                                                                       const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
+                                                                       const Vector3& edgeDirectionCapsuleSpace,
+                                                                       const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const {
+
+    decimal penetrationDepth = DECIMAL_LARGEST;
+
+    // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
+    outAxis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
+
+    // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
+    if (outAxis.lengthSquare() >= decimal(0.00001)) {
+
+        const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
+        const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
+
+        // Swap axis direction if necessary such that it points out of the polyhedron
+        if (outAxis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
+            outAxis = -outAxis;
+        }
+
+        outAxis.normalize();
+
+        // Compute the penetration depth
+        const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis, nullptr);
+        const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
+        penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis);
+    }
+
+    return penetrationDepth;
+}
+
+// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
+decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
+                                                                     const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
+                                                                     Vector3& outFaceNormalCapsuleSpace) const {
+
+    // Get the face
+    HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex);
+
+    // Get the face normal
+    const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex);
+
+    // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
+    outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+    const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr);
+    const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
+    const Vector3 capsuleSupportPointToFacePoint =  pointOnPolyhedronFace - capsuleSupportPoint;
+    const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
+
+    return penetrationDepth;
+}
+
 // Compute the two contact points between a polyhedron and a capsule when the separating
 // axis is a face normal of the polyhedron
 void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
@@ -424,10 +616,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
                 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);
@@ -468,11 +656,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
             }
         }
     }
-    }
 
     // We the shapes are still overlapping in the same axis as in
     // the previous frame, we skip the whole SAT algorithm
-    if (isTemporalCoherenceValid) {
+    if (!isTemporalCoherenceValid) {
 
         // Test all the face normals of the polyhedron 1 for separating axis
         uint faceIndex;

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -31,9 +31,13 @@
 #include "collision/NarrowPhaseInfo.h"
 #include "collision/shapes/ConvexPolyhedronShape.h"
 
+
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+class CapsuleShape;
+class SphereShape;
+
 // Class SATAlgorithm
 class SATAlgorithm {
 
@@ -77,6 +81,21 @@ class SATAlgorithm {
         decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
                                                         const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
 
+        /// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
+        decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
+                                                              const SphereShape* sphere, const Vector3& sphereCenter) const;
+
+        /// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
+        decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
+                                                               const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
+                                                               Vector3& outFaceNormalCapsuleSpace) const;
+
+        /// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
+        decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
+                                                                 const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
+                                                                 const Vector3& edgeDirectionCapsuleSpace,
+                                                                 const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
+
     public :
 
         // -------------------- Methods -------------------- //