Disable previous axis clipping (temporal coherence) in SAT algorithm for testCollision() methods

src/body/RigidBody.cpp

@@ -156,7 +156,7 @@ decimal RigidBody::getMass() const {
  * @param force The force to apply on the body
  * @param point The point where the force is applied (in world-space coordinates)
  */
-inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
+void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
 
     // If it is not a dynamic body, we do nothing
     if (mType != BodyType::DYNAMIC) return;
@@ -208,7 +208,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
 /**
  * @param force The external force to apply on the center of mass of the body
  */
-inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
+void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
 
     // If it is not a dynamic body, we do nothing
     if (mType != BodyType::DYNAMIC) return;
@@ -719,7 +719,7 @@ bool RigidBody::isGravityEnabled() const {
 /**
  * @param torque The external torque to apply on the body
  */
-inline void RigidBody::applyTorque(const Vector3& torque) {
+void RigidBody::applyTorque(const Vector3& torque) {
 
     // If it is not a dynamic body, we do nothing
     if (mType != BodyType::DYNAMIC) return;

src/collision/CollisionDetection.cpp

@@ -335,7 +335,8 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
 }
 
 // Execute the narrow-phase collision detection algorithm on batches
-bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, MemoryAllocator& allocator) {
+bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts,
+                                                  bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator) {
 
     bool contactFound = false;
 
@@ -366,13 +367,13 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI
         contactFound |= capsuleVsCapsuleAlgo->testCollision(capsuleVsCapsuleBatch, 0, capsuleVsCapsuleBatch.getNbObjects(), reportContacts, allocator);
     }
     if (sphereVsConvexPolyhedronBatch.getNbObjects() > 0) {
-        contactFound |= sphereVsConvexPolyAlgo->testCollision(sphereVsConvexPolyhedronBatch, 0, sphereVsConvexPolyhedronBatch.getNbObjects(), reportContacts, allocator);
+        contactFound |= sphereVsConvexPolyAlgo->testCollision(sphereVsConvexPolyhedronBatch, 0, sphereVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
     }
     if (capsuleVsConvexPolyhedronBatch.getNbObjects() > 0) {
-        contactFound |= capsuleVsConvexPolyAlgo->testCollision(capsuleVsConvexPolyhedronBatch, 0, capsuleVsConvexPolyhedronBatch.getNbObjects(), reportContacts, allocator);
+        contactFound |= capsuleVsConvexPolyAlgo->testCollision(capsuleVsConvexPolyhedronBatch, 0, capsuleVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
     }
     if (convexPolyhedronVsConvexPolyhedronBatch.getNbObjects() > 0) {
-        contactFound |= convexPolyVsConvexPolyAlgo->testCollision(convexPolyhedronVsConvexPolyhedronBatch, 0, convexPolyhedronVsConvexPolyhedronBatch.getNbObjects(), reportContacts, allocator);
+        contactFound |= convexPolyVsConvexPolyAlgo->testCollision(convexPolyhedronVsConvexPolyhedronBatch, 0, convexPolyhedronVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
     }
 
     return contactFound;
@@ -423,7 +424,7 @@ void CollisionDetection::computeNarrowPhase() {
     swapPreviousAndCurrentContacts();
 
     // Test the narrow-phase collision detection on the batches to be tested
-    testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator);
+    testNarrowPhaseCollision(mNarrowPhaseInput, true, true, allocator);
 
     // Process all the potential contacts after narrow-phase collision
     processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex,
@@ -449,7 +450,7 @@ bool CollisionDetection::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& nar
     MemoryAllocator& allocator = mMemoryManager.getPoolAllocator();
 
     // Test the narrow-phase collision detection on the batches to be tested
-    bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, false, allocator);
+    bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, false, false, allocator);
     if (collisionFound && callback != nullptr) {
 
         // Compute the overlapping bodies
@@ -503,6 +504,7 @@ void CollisionDetection::computeSnapshotContactPairs(NarrowPhaseInfoBatch& narro
         narrowPhaseInfoBatch.resetContactPoints(i);
     }
 }
+
 // Compute the narrow-phase collision detection for the testCollision() methods.
 // This method returns true if contacts are found.
 bool CollisionDetection::computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& narrowPhaseInput, CollisionCallback& callback) {
@@ -512,7 +514,7 @@ bool CollisionDetection::computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& n
     MemoryAllocator& allocator = mMemoryManager.getPoolAllocator();
 
     // Test the narrow-phase collision detection on the batches to be tested
-    bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, true, allocator);
+    bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, true, false, allocator);
 
     // If collision has been found, create contacts
     if (collisionFound) {

src/collision/CollisionDetection.h

@@ -205,7 +205,7 @@ class CollisionDetection {
         void removeNonOverlappingPairs();
 
         /// Execute the narrow-phase collision detection algorithm on batches
-        bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, MemoryAllocator& allocator);
+        bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator);
 
         /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
         void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -41,13 +41,14 @@ using namespace reactphysics3d;
 // by Dirk Gregorius.
 bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                        uint batchStartIndex, uint batchNbItems,
-                                                       bool reportContacts, MemoryAllocator& memoryAllocator) {
+                                                       bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
+                                                       MemoryAllocator& memoryAllocator) {
 
     bool isCollisionFound = false;
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
-    SATAlgorithm satAlgorithm(memoryAllocator);
+    SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h

@@ -71,7 +71,8 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute the narrow-phase collision detection between a capsule and a polyhedron
         bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                           uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
+                           uint batchNbItems, bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
+                           MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -37,10 +37,10 @@ using namespace reactphysics3d;
 // by Dirk Gregorius.
 bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                                 uint batchStartIndex, uint batchNbItems,
-                                                                bool reportContacts,  MemoryAllocator& memoryAllocator) {
+                                                                bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
 
     // Run the SAT algorithm to find the separating axis and compute contact point
-    SATAlgorithm satAlgorithm(memoryAllocator);
+    SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h

@@ -66,7 +66,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
 
         /// Compute the narrow-phase collision detection between two convex polyhedra
         bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
-                           MemoryAllocator& memoryAllocator);
+                           bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -43,7 +43,8 @@ using namespace reactphysics3d;
 const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
 
 // Constructor
-SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator)
+             : mClipWithPreviousAxisIfStillColliding(clipWithPreviousAxisIfStillColliding), mMemoryAllocator(memoryAllocator) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -525,7 +526,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                 }
 
                 // The two shapes were overlapping in the previous frame and still seem to overlap in this one
-                if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                     minPenetrationDepth = penetrationDepth;
                     minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -565,7 +566,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                 }
 
                 // The two shapes were overlapping in the previous frame and still seem to overlap in this one
-                if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                     minPenetrationDepth = penetrationDepth;
                     minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -623,7 +624,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                     }
 
                     // If the shapes were overlapping on the previous axis and still seem to overlap in this frame
-                    if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                    if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                         // Compute the closest points between the two edges (in the local-space of poylhedron 2)
                         Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -60,6 +60,20 @@ class SATAlgorithm {
         /// make sure the contact manifold does not change too much between frames.
         static const decimal SAME_SEPARATING_AXIS_BIAS;
 
+        /// True means that if two shapes were colliding last time (previous frame) and are still colliding
+        /// we use the previous (minimum penetration depth) axis to clip the colliding features and we don't
+        /// recompute a new (minimum penetration depth) axis. This value must be true for a dynamic simulation
+        /// because it uses temporal coherence and clip the colliding features with the previous
+        /// axis (this is good for stability). However, when we use the testCollision() methods, the penetration
+        /// depths might be very large and we always want the current true axis with minimum penetration depth.
+        /// In this case, this value must be set to false. Consider the following situation. Two shapes start overlaping
+        /// with "x" being the axis of minimum penetration depth. Then, if the shapes move but are still penetrating,
+        /// it is possible that the axis of minimum penetration depth changes for axis "y" for instance. If this value
+        /// if true, we will always use the axis of the previous collision test and therefore always report that the
+        /// penetrating axis is "x" even if it has changed to axis "y" during the collision. This is not what we want
+        /// when we call the testCollision() methods.
+        bool mClipWithPreviousAxisIfStillColliding;
+
         /// Memory allocator
         MemoryAllocator& mMemoryAllocator;
 
@@ -126,7 +140,7 @@ class SATAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        SATAlgorithm(MemoryAllocator& memoryAllocator);
+        SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
 
         /// Destructor
         ~SATAlgorithm() = default;

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
 bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
-                                                      bool reportContacts, MemoryAllocator& memoryAllocator) {
+                                                      bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
@@ -80,7 +80,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
         if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
             // Run the SAT algorithm to find the separating axis and compute contact point
-            SATAlgorithm satAlgorithm(memoryAllocator);
+            SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -72,7 +72,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
         bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
-                           MemoryAllocator& memoryAllocator);
+                           bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
 };
 
 }