diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index b02abbf2..aed6977f 100644 --- a/src/body/RigidBody.cpp +++ b/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; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index a8012f9b..a1647ca9 100644 --- a/src/collision/CollisionDetection.cpp +++ b/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) { diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 17383def..e4b16417 100644 --- a/src/collision/CollisionDetection.h +++ b/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, diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index ff02fa38..f42e8934 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/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 diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 42d8ada2..53b5ca56 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/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); }; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 08d9e9f7..570345fe 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/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 diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index b4bd70e6..6e43e76b 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/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); }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 42d77256..d8d18876 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/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; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 4ed5e821..2d7afd4a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/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; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 48e5390b..b8fb779f 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/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 diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index e0d6ea0a..793dc48a 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/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); }; }