From 8b82c4ac819998ea2af634a8a5858fd0cada31df Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 30 Jul 2017 22:14:46 +0200 Subject: [PATCH] Refactor the way to create the contact manifolds and contact points --- CMakeLists.txt | 4 + src/body/CollisionBody.cpp | 6 +- src/collision/CollisionCallback.cpp | 79 ++++ src/collision/CollisionCallback.h | 89 ++++ src/collision/CollisionDetection.cpp | 425 ++++++++++++------ src/collision/CollisionDetection.h | 17 +- src/collision/ContactManifold.cpp | 82 ++-- src/collision/ContactManifold.h | 248 ++++++---- src/collision/ContactManifoldInfo.cpp | 60 +-- src/collision/ContactManifoldInfo.h | 45 +- src/collision/ContactManifoldSet.cpp | 278 +++++++----- src/collision/ContactManifoldSet.h | 84 ++-- src/collision/NarrowPhaseInfo.cpp | 97 ++++ src/collision/NarrowPhaseInfo.h | 22 +- src/collision/OverlapCallback.h | 57 +++ .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 8 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 3 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +- .../CapsuleVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 178 ++++---- .../narrowphase/ConcaveVsConvexAlgorithm.h | 26 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +- ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 5 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +- .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 22 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 8 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 4 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 3 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +- .../SphereVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 5 +- .../narrowphase/SphereVsSphereAlgorithm.h | 3 +- src/collision/shapes/BoxShape.h | 2 + src/collision/shapes/CollisionShape.h | 17 - src/constraint/ContactPoint.cpp | 9 +- src/constraint/ContactPoint.h | 70 ++- src/engine/CollisionWorld.h | 58 --- src/engine/ContactSolver.cpp | 8 +- src/engine/DynamicsWorld.cpp | 11 +- src/engine/EventListener.h | 10 +- src/engine/OverlappingPair.cpp | 100 ++++- src/engine/OverlappingPair.h | 69 ++- src/memory/Allocator.h | 3 + src/memory/PoolAllocator.h | 8 + src/memory/SingleFrameAllocator.h | 9 +- src/reactphysics3d.h | 2 + .../CollisionDetectionScene.h | 40 +- testbed/src/SceneDemo.cpp | 6 +- 50 files changed, 1558 insertions(+), 767 deletions(-) create mode 100644 src/collision/CollisionCallback.cpp create mode 100644 src/collision/CollisionCallback.h create mode 100644 src/collision/NarrowPhaseInfo.cpp create mode 100644 src/collision/OverlapCallback.h diff --git a/CMakeLists.txt b/CMakeLists.txt index cba141df..1e1d2fcf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" "src/collision/NarrowPhaseInfo.h" + "src/collision/NarrowPhaseInfo.cpp" "src/collision/ContactManifold.h" "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.h" @@ -167,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/Profiler.cpp" "src/engine/Timer.h" "src/engine/Timer.cpp" + "src/collision/CollisionCallback.h" + "src/collision/CollisionCallback.cpp" + "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" "src/mathematics/mathematics_functions.cpp" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index db22f606..39d47de4 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -177,7 +177,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the linked list of contact manifolds of that body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - ContactManifoldListElement* nextElement = currentElement->next; + ContactManifoldListElement* nextElement = currentElement->getNext(); // Delete the current element currentElement->~ContactManifoldListElement(); @@ -272,8 +272,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { // this body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - currentElement->contactManifold->mIsAlreadyInIsland = false; - currentElement = currentElement->next; + currentElement->getContactManifold()->mIsAlreadyInIsland = false; + currentElement = currentElement->getNext(); nbManifolds++; } diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp new file mode 100644 index 00000000..f470763d --- /dev/null +++ b/src/collision/CollisionCallback.cpp @@ -0,0 +1,79 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "collision/CollisionCallback.h" +#include "engine/OverlappingPair.h" +#include "memory/Allocator.h" +#include "collision/ContactManifold.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) : + contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()), + body2(pair->getShape2()->getBody()), + proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()), + mMemoryAllocator(allocator) { + + assert(pair != nullptr); + + const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); + + // For each contact manifold in the set of manifolds in the pair + ContactManifold* contactManifold = manifoldSet.getContactManifolds(); + while (contactManifold != nullptr) { + + assert(contactManifold->getNbContactPoints() > 0); + + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement))) + ContactManifoldListElement(contactManifold, + contactManifoldElements); + contactManifoldElements = element; + + contactManifold = contactManifold->getNext(); + } +} + +// Destructor +CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() { + + // Release memory allocator for the contact manifold list elements + ContactManifoldListElement* element = contactManifoldElements; + while (element != nullptr) { + + ContactManifoldListElement* nextElement = element->getNext(); + + // Delete and release memory + element->~ContactManifoldListElement(); + mMemoryAllocator.release(element, sizeof(ContactManifoldListElement)); + + element = nextElement; + } +} + diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h new file mode 100644 index 00000000..f860b42d --- /dev/null +++ b/src/collision/CollisionCallback.h @@ -0,0 +1,89 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H +#define REACTPHYSICS3D_COLLISION_CALLBACK_H + +// Libraries +#include "collision/ContactManifold.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class CollisionCallback +/** + * This class can be used to register a callback for collision test queries. + * You should implement your own class inherited from this one and implement + * the notifyContact() method. This method will called each time a contact + * point is reported. + */ +class CollisionCallback { + + public: + + // Structure CollisionCallbackInfo + /** + * This structure represents the contact information between two collision + * shapes of two bodies + */ + struct CollisionCallbackInfo { + + public: + + /// Pointer to the first element of the linked-list that contains contact manifolds + ContactManifoldListElement* contactManifoldElements; + + /// Pointer to the first body of the contact + CollisionBody* body1; + + /// Pointer to the second body of the contact + CollisionBody* body2; + + /// Pointer to the proxy shape of first body + const ProxyShape* proxyShape1; + + /// Pointer to the proxy shape of second body + const ProxyShape* proxyShape2; + + /// Memory allocator + Allocator& mMemoryAllocator; + + // Constructor + CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator); + + // Destructor + ~CollisionCallbackInfo(); + }; + + /// Destructor + virtual ~CollisionCallback() = default; + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; +}; + +} + +#endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 0e7c8078..57360706 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -26,10 +26,13 @@ // Libraries #include "CollisionDetection.h" #include "engine/CollisionWorld.h" +#include "collision/OverlapCallback.h" #include "body/Body.h" #include "collision/shapes/BoxShape.h" #include "body/RigidBody.h" #include "configuration.h" +#include "collision/CollisionCallback.h" +#include "collision/OverlapCallback.h" #include #include #include @@ -43,7 +46,7 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator) - : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), + : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { @@ -83,7 +86,7 @@ void CollisionDetection::computeBroadPhase() { // Ask the broad-phase to recompute the overlapping pairs of collision // shapes. This call can only add new overlapping pairs in the collision // detection. - mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator); + mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator); } } @@ -101,25 +104,24 @@ void CollisionDetection::computeMiddlePhase() { OverlappingPair* pair = it->second; + // Make all the contact manifolds and contact points of the pair obselete + pair->makeContactsObselete(); + ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the + // Check if the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { std::map::iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; @@ -128,56 +130,61 @@ void CollisionDetection::computeMiddlePhase() { ++it; } - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); + // Check if the collision filtering allows collision between the two shapes + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) { - // Check that at least one body is awake and not static - bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; - bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; - if (!isBody1Active && !isBody2Active) continue; + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); - // Check if the bodies are in the set of bodies that cannot collide between each other - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; + // Check that at least one body is awake and not static + bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; + bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; + if (!isBody1Active && !isBody2Active) continue; - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + // Check if the bodies are in the set of bodies that cannot collide between each other + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(pair, shape1->getCollisionShape(), - shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); - mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; + // If both shapes are convex + if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { - } - // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), + shape2->getCachedCollisionData()); + mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; - NarrowPhaseInfo* narrowPhaseInfo = nullptr; - computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); - - // Add all the narrow-phase info object reported by the callback into the - // list of all the narrow-phase info object - while (narrowPhaseInfo != nullptr) { - NarrowPhaseInfo* next = narrowPhaseInfo->next; - narrowPhaseInfo->next = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = narrowPhaseInfo; - - narrowPhaseInfo = next; } - } - // Concave vs Concave shape - else { - // Not handled - continue; + // Concave vs Convex algorithm + else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || + (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + while (narrowPhaseInfo != nullptr) { + NarrowPhaseInfo* next = narrowPhaseInfo->next; + narrowPhaseInfo->next = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = narrowPhaseInfo; + + narrowPhaseInfo = next; + } + } + // Concave vs Concave shape + else { + // Not handled + continue; + } } } } @@ -231,7 +238,7 @@ void CollisionDetection::computeNarrowPhase() { PROFILE("CollisionDetection::computeNarrowPhase()"); - const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; + NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; while (currentNarrowPhaseInfo != nullptr) { // Select the narrow phase algorithm to use according to the two collision shapes @@ -245,30 +252,16 @@ void CollisionDetection::computeNarrowPhase() { // 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. - ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator); - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform); - - // If it is the first contact since the pairs are overlapping - if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { - - // Trigger a callback event - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo); - } - - // Add the contact manifold to the corresponding overlapping pair - currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo); + // Add the contact points as a potential contact manifold into the pair + currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); // Add the overlapping pair into the set of pairs in contact during narrow-phase overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), currentNarrowPhaseInfo->overlappingPair->getShape2()); mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; - // Trigger a callback event for the new contact - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo); - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; } else { @@ -282,8 +275,14 @@ void CollisionDetection::computeNarrowPhase() { currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; } + // Convert the potential contact into actual contacts + processAllPotentialContacts(); + // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); + + // Report contacts to the user + reportAllContacts(); } // Allow the broadphase to notify the collision detection about an overlapping pair. @@ -302,13 +301,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Check if the overlapping pair already exists if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; - // Compute the maximum number of contact manifolds for this pair - int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), - shape2->getCollisionShape()->getType()); - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); + OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator); assert(newPair != nullptr); #ifndef NDEBUG @@ -372,40 +367,173 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); // For each contact manifold in the set of manifolds in the pair - for (int i=0; igetNbContactPoints() > 0); // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement1 = new (allocatedMemory1) + ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); body1->mContactManifoldsList = listElement1; // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement2 = new (allocatedMemory2) + ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); body2->mContactManifoldsList = listElement2; + + contactManifold = contactManifold->getNext(); } } -// Delete all the contact points in the currently overlapping pairs -void CollisionDetection::clearContactPoints() { +/// Convert the potential contact into actual contacts +void CollisionDetection::processAllPotentialContacts() { - // For each overlapping pair + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - it->second->clearContactPoints(); + for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + + // Process the potential contacts of the overlapping pair + processPotentialContacts(it->second); } } +// Process the potential contact manifold of a pair to create actual contact manifold +void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { + + // Reduce the number of contact points of the manifold + pair->reducePotentialContactManifolds(); + + // If there is a concave mesh shape in the pair + if (pair->hasConcaveShape()) { + + processSmoothMeshContacts(pair); + } + else { // If both collision shapes are convex + + // Add all the potential contact manifolds as actual contact manifolds to the pair + ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); + while (potentialManifold != nullptr) { + + pair->addContactManifold(potentialManifold); + + potentialManifold = potentialManifold->mNext; + } + } + + // Clear the obselete contact manifolds and contact points + pair->clearObseleteManifoldsAndContactPoints(); + + // Reset the potential contacts of the pair + pair->clearPotentialContactManifolds(); +} + +// Report contacts for all the colliding overlapping pairs +void CollisionDetection::reportAllContacts() { + + // For each overlapping pairs in contact during the narrow-phase + std::map::iterator it; + for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + + // If there is a user callback + if (mWorld->mEventListener != nullptr) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator); + + // Trigger a callback event to report the new contact to the user + mWorld->mEventListener->newContact(collisionInfo); + } + } +} + +// Process the potential contacts where one collion is a concave shape. +// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described +// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision +// issue with some internal edges. +void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) { + +// // Set with the triangle vertices already processed to void further contacts with same triangle +// std::unordered_multimap processTriangleVertices; + +// std::vector smoothContactPoints; + +// // If the collision shape 1 is the triangle +// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE; +// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE); +// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE); + +// const TriangleShape* triangleShape = nullptr; +// if (isFirstShapeTriangle) { +// triangleShape = static_cast(pair->getShape1()->getCollisionShape()); +// } +// else { +// triangleShape = static_cast(pair->getShape2()->getCollisionShape()); +// } +// assert(triangleShape != nullptr); + +// // Get the temporary memory allocator +// Allocator& allocator = pair->getTemporaryAllocator(); + +// // For each potential contact manifold of the pair +// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); +// while (potentialManifold != nullptr) { + +// // For each contact point of the potential manifold +// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo(); +// while (contactPointInfo != nullptr) { + +// // Compute the barycentric coordinates of the point in the triangle +// decimal u, v, w; +// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0), +// triangleShape->getVertexPosition(1), +// triangleShape->getVertexPosition(2), +// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2, +// u, v, w); +// int nbZeros = 0; +// bool isUZero = approxEqual(u, 0, 0.0001); +// bool isVZero = approxEqual(v, 0, 0.0001); +// bool isWZero = approxEqual(w, 0, 0.0001); +// if (isUZero) nbZeros++; +// if (isVZero) nbZeros++; +// if (isWZero) nbZeros++; + +// // If the triangle contact point is on a triangle vertex of a triangle edge +// if (nbZeros == 1 || nbZeros == 2) { + + +// // Create a smooth mesh contact info +// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo))) +// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle, +// triangleShape->getVertexPosition(0), +// triangleShape->getVertexPosition(1), +// triangleShape->getVertexPosition(2)); + +// smoothContactPoints.push_back(smoothContactInfo); + +// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we +// // will put the contact point back in the manifold. +// ... +// } + +// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle +// // from the contact manifold because we know we will keep to contact points. We only remove the vertices +// // and edges contact points for the moment. If those points will be kept in the future, we will have to +// // put them back again in the contact manifold +// } + +// potentialManifold = potentialManifold->mNext; +// } + +// // Sort the list of narrow-phase contacts according to their penetration depth +// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare()); + +// ... +} + // Compute the middle-phase collision detection between two proxy shapes NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { @@ -424,7 +552,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), + narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), shape2->getCachedCollisionData()); @@ -436,7 +564,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo); } return narrowPhaseInfo; @@ -450,7 +578,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over std::unordered_set reportedBodies; // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); // For each overlaping proxy shape @@ -504,7 +632,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -526,16 +654,18 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // 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. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -570,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -595,7 +725,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -617,16 +747,18 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // 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. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -673,7 +805,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body if (aabb1.testCollision(aabb2)) { // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -693,26 +825,29 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // 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. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); - - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2, - body1ProxyShape, body2ProxyShape); - - // Report the contact to the user - collisionCallback->notifyContact(collisionInfo); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + collisionCallback->notifyContact(collisionInfo); } // Go to the next proxy shape @@ -737,7 +872,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -760,7 +895,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -777,17 +912,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // 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. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body, - proxyShape->getBody(), bodyProxyShape, - proxyShape); - - // Report the contact to the user + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); callback->notifyContact(collisionInfo); } } @@ -795,9 +926,15 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); } } @@ -822,10 +959,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; + OverlappingPair* originalPair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); + // Create a new overlapping pair so that we do not work on the original one + OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator, + mPoolAllocator); + + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. @@ -834,14 +975,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); - - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -851,29 +992,29 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // 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. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(), - shape2->getBody(), shape1, shape2); - - // Report the contact to the user + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); callback->notifyContact(collisionInfo); } - - // The previous frame collision info is now valid - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); } } } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 3fe88812..49f0d56b 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -72,7 +72,7 @@ class CollisionDetection { NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + PoolAllocator& mPoolAllocator; /// Reference to the single frame memory allocator SingleFrameAllocator& mSingleFrameAllocator; @@ -118,9 +118,6 @@ class CollisionDetection { /// involed in the corresponding contact. void addContactManifoldToBody(OverlappingPair* pair); - /// Delete all the contact points in the currently overlapping pairs - void clearContactPoints(); - /// Fill-in the collision detection matrix void fillInCollisionMatrix(); @@ -137,6 +134,18 @@ class CollisionDetection { /// Compute the middle-phase collision detection between two proxy shapes NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair); + + /// Convert the potential contact into actual contacts + void processAllPotentialContacts(); + + /// Process the potential contact manifold of a pair to create actual contact manifold + void processPotentialContacts(OverlappingPair* pair); + + /// Report contacts for all the colliding overlapping pairs + void reportAllContacts(); + + /// Process the potential contacts where one collion is a concave shape + void processSmoothMeshContacts(OverlappingPair* pair); public : diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 36103642..846a418a 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -30,15 +30,14 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short normalDirectionId) - : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), +ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator) + : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) { // For each contact point info in the manifold - const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo(); + const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); while(pointInfo != nullptr) { // Create the new contact point @@ -46,7 +45,8 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); // Add the new contact point into the manifold - mContactPoints[mNbContactPoints] = contact; + contact->setNext(mContactPoints); + mContactPoints = contact; mNbContactPoints++; pointInfo = pointInfo->next; @@ -58,49 +58,69 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS // Destructor ContactManifold::~ContactManifold() { - clear(); -} -// Clear the contact manifold -void ContactManifold::clear() { - for (uint i=0; i~ContactPoint(); - mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); + // Delete all the contact points + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + // TODO : Delete this + bool test = mMemoryAllocator.isReleaseNeeded(); + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + contactPoint = nextContactPoint; } - mNbContactPoints = 0; } // Add a contact point void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) { - assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD); - // Create the new contact point ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); // Add the new contact point into the manifold - mContactPoints[mNbContactPoints] = contactPoint; - mNbContactPoints++; + contactPoint->setNext(mContactPoints); + mContactPoints = contactPoint; + mNbContactPoints++; } -// Remove a contact point -void ContactManifold::removeContactPoint(int index) { +// Clear the obselete contact points +void ContactManifold::clearObseleteContactPoints() { - assert(mNbContactPoints > 0); - assert(index >= 0 && index < mNbContactPoints); + ContactPoint* contactPoint = mContactPoints; + ContactPoint* previousContactPoint = nullptr; + while (contactPoint != nullptr) { - // Delete the contact - mContactPoints[index]->~ContactPoint(); - mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); + ContactPoint* nextContactPoint = contactPoint->getNext(); - for (int i=index; (i+1) < mNbContactPoints; i++) { - mContactPoints[i] = mContactPoints[i+1]; + if (contactPoint->getIsObselete()) { + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + if (previousContactPoint != nullptr) { + previousContactPoint->setNext(nextContactPoint); + } + else { + mContactPoints = nextContactPoint; + } + + mNbContactPoints--; + } + else { + previousContactPoint = contactPoint; + } + + contactPoint = nextContactPoint; } - mNbContactPoints--; + assert(mNbContactPoints >= 0); + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 893a84a7..0704beed 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -46,40 +46,43 @@ class ContactManifold; */ struct ContactManifoldListElement { - public: + private: // -------------------- Attributes -------------------- // - /// Pointer to the actual contact manifold - ContactManifold* contactManifold; + /// Pointer to a contact manifold with contact points + ContactManifold* mContactManifold; /// Next element of the list - ContactManifoldListElement* next; + ContactManifoldListElement* mNext; + + public: // -------------------- Methods -------------------- // /// Constructor - ContactManifoldListElement(ContactManifold* initContactManifold, - ContactManifoldListElement* initNext) - :contactManifold(initContactManifold), next(initNext) { + ContactManifoldListElement(ContactManifold* contactManifold, + ContactManifoldListElement* next) + :mContactManifold(contactManifold), mNext(next) { } + + /// Return the contact manifold + ContactManifold* getContactManifold() { + return mContactManifold; + } + + /// Return the next element in the linked-list + ContactManifoldListElement* getNext() { + return mNext; + } }; // Class ContactManifold /** * This class represents the set of contact points between two bodies. * The contact manifold is implemented in a way to cache the contact - * points among the frames for better stability following the - * "Contact Generation" presentation of Erwin Coumans at GDC 2010 - * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). - * Some code of this class is based on the implementation of the - * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). - * The contacts between two bodies are added one after the other in the cache. - * When the cache is full, we have to remove one point. The idea is to keep - * the point with the deepest penetration depth and also to keep the - * points producing the larger area (for a more stable contact manifold). - * The new added point is always kept. + * points among the frames for better stability. */ class ContactManifold { @@ -94,10 +97,10 @@ class ContactManifold { ProxyShape* mShape2; /// Contact points in the manifold - ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + ContactPoint* mContactPoints; /// Normal direction Id (Unique Id representing the normal direction) - short int mNormalDirectionId; + short int mContactNormalId; /// Number of contacts in the cache int8 mNbContactPoints; @@ -124,20 +127,86 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + Allocator& mMemoryAllocator; + + /// Pointer to the next contact manifold in the linked-list + ContactManifold* mNext; + + /// Pointer to the previous contact manifold in linked-list + ContactManifold* mPrevious; + + /// True if the contact manifold is obselete + bool mIsObselete; // -------------------- Methods -------------------- // /// Return true if the contact manifold has already been added into an island bool isAlreadyInIsland() const; + + /// Set the pointer to the next element in the linked-list + void setNext(ContactManifold* nextManifold); + + /// Return true if the manifold is obselete + bool getIsObselete() const; + + /// Set to true to make the manifold obselete + void setIsObselete(bool isObselete, bool setContactPoints); + + /// Clear the obselete contact points + void clearObseleteContactPoints(); + + /// Return the contact normal direction Id of the manifold + short getContactNormalId() const; + + /// Return the largest depth of all the contact points + decimal getLargestContactDepth() const; + + /// set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + /// set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + /// Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + /// Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + /// Add a contact point + void addContactPoint(const ContactPointInfo* contactPointInfo); + + /// Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + /// Set the accumulated rolling resistance impulse + void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); + + /// Set the pointer to the previous element in the linked-list + void setPrevious(ContactManifold* previousManifold); + + /// Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + /// Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + /// Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + /// Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + /// Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; public: // -------------------- Methods -------------------- // /// Constructor - ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short int normalDirectionId); + ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, + Allocator& memoryAllocator); /// Destructor ~ContactManifold(); @@ -160,68 +229,25 @@ class ContactManifold { /// Return a pointer to the second body of the contact manifold CollisionBody* getBody2() const; - /// Return the normal direction Id - short int getNormalDirectionId() const; - - /// Clear the contact manifold - void clear(); - /// Return the number of contact points in the manifold int8 getNbContactPoints() const; - /// Return the first friction vector at the center of the contact manifold - const Vector3& getFrictionVector1() const; + /// Return a pointer to the first contact point of the manifold + ContactPoint* getContactPoints() const; - /// set the first friction vector at the center of the contact manifold - void setFrictionVector1(const Vector3& mFrictionVector1); + /// Return a pointer to the previous element in the linked-list + ContactManifold* getPrevious() const; - /// Return the second friction vector at the center of the contact manifold - const Vector3& getFrictionVector2() const; - - /// set the second friction vector at the center of the contact manifold - void setFrictionVector2(const Vector3& mFrictionVector2); - - /// Return the first friction accumulated impulse - decimal getFrictionImpulse1() const; - - /// Set the first friction accumulated impulse - void setFrictionImpulse1(decimal frictionImpulse1); - - /// Return the second friction accumulated impulse - decimal getFrictionImpulse2() const; - - /// Set the second friction accumulated impulse - void setFrictionImpulse2(decimal frictionImpulse2); - - /// Return the friction twist accumulated impulse - decimal getFrictionTwistImpulse() const; - - /// Set the friction twist accumulated impulse - void setFrictionTwistImpulse(decimal frictionTwistImpulse); - - /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); - - /// Return a contact point of the manifold - ContactPoint* getContactPoint(uint index) const; - - /// Return the normalized averaged normal vector - Vector3 getAverageContactNormal() const; - - /// Return the largest depth of all the contact points - decimal getLargestContactDepth() const; - - /// Add a contact point - void addContactPoint(const ContactPointInfo* contactPointInfo); - - /// Remove a contact point - void removeContactPoint(int index); + /// Return a pointer to the next element in the linked-list + ContactManifold* getNext() const; // -------------------- Friendship -------------------- // friend class DynamicsWorld; friend class Island; friend class CollisionBody; + friend class ContactManifoldSet; + friend class ContactSolver; }; // Return a pointer to the first proxy shape of the contact @@ -244,11 +270,6 @@ inline CollisionBody* ContactManifold::getBody2() const { return mShape2->getBody(); } -// Return the normal direction Id -inline short int ContactManifold::getNormalDirectionId() const { - return mNormalDirectionId; -} - // Return the number of contact points in the manifold inline int8 ContactManifold::getNbContactPoints() const { return mNbContactPoints; @@ -309,10 +330,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR mRollingResistanceImpulse = rollingResistanceImpulse; } -// Return a contact point of the manifold -inline ContactPoint* ContactManifold::getContactPoint(uint index) const { - assert(index < mNbContactPoints); - return mContactPoints[index]; +// Return a pointer to the first contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoints() const { + return mContactPoints; } // Return true if the contact manifold has already been added into an island @@ -320,31 +340,69 @@ inline bool ContactManifold::isAlreadyInIsland() const { return mIsAlreadyInIsland; } -// Return the normalized averaged normal vector -inline Vector3 ContactManifold::getAverageContactNormal() const { - Vector3 averageNormal; - - for (uint i=0; igetNormal(); - } - - return averageNormal.getUnit(); -} - // Return the largest depth of all the contact points inline decimal ContactManifold::getLargestContactDepth() const { decimal largestDepth = 0.0f; - for (uint i=0; igetPenetrationDepth(); + assert(mNbContactPoints > 0); + + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr){ + decimal depth = contactPoint->getPenetrationDepth(); if (depth > largestDepth) { largestDepth = depth; } + + contactPoint = contactPoint->getNext(); } return largestDepth; } +// Return a pointer to the previous element in the linked-list +inline ContactManifold* ContactManifold::getPrevious() const { + return mPrevious; +} + +// Set the pointer to the previous element in the linked-list +inline void ContactManifold::setPrevious(ContactManifold* previousManifold) { + mPrevious = previousManifold; +} + +// Return a pointer to the next element in the linked-list +inline ContactManifold* ContactManifold::getNext() const { + return mNext; +} + +// Set the pointer to the next element in the linked-list +inline void ContactManifold::setNext(ContactManifold* nextManifold) { + mNext = nextManifold; +} + +// Return true if the manifold is obselete +inline bool ContactManifold::getIsObselete() const { + return mIsObselete; +} + +// Set to true to make the manifold obselete +inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) { + mIsObselete = isObselete; + + if (setContactPoints) { + ContactPoint* contactPoint = mContactPoints; + while (contactPoint != nullptr) { + contactPoint->setIsObselete(isObselete); + + contactPoint = contactPoint->getNext(); + } + } +} + +// Return the contact normal direction Id of the manifold +inline short ContactManifold::getContactNormalId() const { + return mContactNormalId; +} + } #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index c06785ea..a6b8c6df 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -30,7 +30,8 @@ using namespace reactphysics3d; // Constructor ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator) - : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {} + : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator), + mContactNormalId(-1) {} // Destructor ContactManifoldInfo::~ContactManifoldInfo() { @@ -40,14 +41,13 @@ ContactManifoldInfo::~ContactManifoldInfo() { } // Add a new contact point into the manifold -void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { +void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) { - assert(penDepth > decimal(0.0)); + assert(contactPointInfo->penetrationDepth > decimal(0.0)); + assert(contactNormalId >= 0); + assert(mContactNormalId == -1 || contactNormalId == mContactNormalId); - // Create the contact point info - ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo))) - ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + mContactNormalId = contactNormalId; // Add it into the linked list of contact points contactPointInfo->next = mContactPointsList; @@ -73,15 +73,31 @@ void ContactManifoldInfo::reset() { mNbContactPoints = 0; } -// Reduce the number of points in the contact manifold +// Return the largest penetration depth among its contact points +decimal ContactManifoldInfo::getLargestPenetrationDepth() const { + + ContactPointInfo* contactPoint = mContactPointsList; + assert(contactPoint != nullptr); + decimal maxDepth = decimal(0.0); + while (contactPoint != nullptr) { + + if (contactPoint->penetrationDepth > maxDepth) { + maxDepth = contactPoint->penetrationDepth; + } + + contactPoint = contactPoint->next; + } + + return maxDepth; +} + +// Reduce the number of contact points of the currently computed manifold // This is based on the technique described by Dirk Gregorius in his // "Contacts Creation" GDC presentation void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { assert(mContactPointsList != nullptr); - // TODO : Implement this (do not forget to deallocate removed points) - // The following algorithm only works to reduce to 4 contact points assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); @@ -221,9 +237,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { largestArea = area; pointsToKeep[3] = element; } - - element = element->next; } + + element = element->next; } assert(pointsToKeep[3] != nullptr); @@ -250,6 +266,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { } element = element->next; + // Call the destructor + elementToDelete->~ContactPointInfo(); + // Delete the current element mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); } @@ -258,21 +277,4 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { } } -/// Return the largest penetration depth among the contact points -decimal ContactManifoldInfo::getLargestPenetrationDepth() const { - - decimal maxDepth = decimal(0.0); - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - - if (element->penetrationDepth > maxDepth) { - maxDepth = element->penetrationDepth; - } - - element = element->next; - } - - return maxDepth; -} - diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index a7e84a98..5851c375 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Constants -const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold +const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold // Class ContactManifoldInfo /** @@ -50,12 +50,18 @@ class ContactManifoldInfo { /// Linked list with all the contact points ContactPointInfo* mContactPointsList; - /// Memory allocator used to allocate contact points - Allocator& mAllocator; - /// Number of contact points in the manifold uint mNbContactPoints; + /// Next element in the linked-list of contact manifold info + ContactManifoldInfo* mNext; + + /// Reference the the memory allocator where the contact point infos have been allocated + Allocator& mAllocator; + + /// Contact normal direction Id (Identify the contact normal direction of points in manifold) + short mContactNormalId; + public: // -------------------- Methods -------------------- // @@ -66,15 +72,14 @@ class ContactManifoldInfo { /// Destructor ~ContactManifoldInfo(); - /// Deleted copy-constructor + /// Deleted Copy-constructor ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; /// Deleted assignment operator ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; /// Add a new contact point into the manifold - void addContactPoint(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2); + void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId); /// Remove all the contact points void reset(); @@ -82,11 +87,21 @@ class ContactManifoldInfo { /// Get the first contact point info of the linked list of contact points ContactPointInfo* getFirstContactPointInfo() const; - /// Reduce the number of points in the contact manifold + /// Return the largest penetration depth among its contact points + decimal getLargestPenetrationDepth() const; + + /// Return the pointer to the next manifold info in the linked-list + ContactManifoldInfo* getNext(); + + /// Return the contact normal Id + short getContactNormalId() const; + + /// Reduce the number of contact points of the currently computed manifold void reduce(const Transform& shape1ToWorldTransform); - /// Return the largest penetration depth among the contact points - decimal getLargestPenetrationDepth() const; + // Friendship + friend class OverlappingPair; + friend class CollisionDetection; }; // Get the first contact point info of the linked list of contact points @@ -94,6 +109,16 @@ inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const { return mContactPointsList; } +// Return the pointer to the next manifold info in the linked-list +inline ContactManifoldInfo* ContactManifoldInfo::getNext() { + return mNext; +} + +// Return the contact normal Id +inline short ContactManifoldInfo::getContactNormalId() const { + return mContactNormalId; +} + } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index c9b0d5bb..b9b7f3e8 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,10 +30,12 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds) - : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), - mShape2(shape2), mMemoryAllocator(memoryAllocator) { + Allocator& memoryAllocator) + : mNbManifolds(0), mShape1(shape1), + mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) { + // Compute the maximum number of manifolds allowed between the two shapes + mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape()); } // Destructor @@ -43,167 +45,131 @@ ContactManifoldSet::~ContactManifoldSet() { clear(); } -void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { +void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { - assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr); + assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr); - // If there is no contact manifold yet - if (mNbManifolds == 0) { + // Try to find an existing contact manifold with similar contact normal + ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId()); - // If the maximum number of manifold is 1 - if (mNbMaxManifolds == 1) { - createManifold(contactManifoldInfo, 0); - } - else { + // If a similar manifold has been found + if (similarManifold != nullptr) { - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); - - createManifold(contactManifoldInfo, normalDirectionId); - } + // Update the old manifold with the new one + updateManifoldWithNewOne(similarManifold, contactManifoldInfo); } - else { // If there is already at least one contact manifold in the set + else { - // If the maximum number of manifold is 1 - if (mNbMaxManifolds == 1) { + // If there are too much contact manifolds in the set + if (mNbManifolds >= mNbMaxManifolds) { - // Replace the old manifold with the new one - updateManifoldWithNewOne(0, contactManifoldInfo); - } - else { + // We need to remove a manifold from the set. + // We seach for the one with the smallest maximum penetration depth among its contact points + ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth()); - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); + // If the manifold with the minimum penetration depth is an existing one + if (minDepthManifold != nullptr) { - // Select the manifold with the most similar normal (if exists) - int similarManifoldIndex = 0; - if (mNbMaxManifolds > 1) { - similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); - } + // Remove the manifold + removeManifold(minDepthManifold); - // If a similar manifold has been found - if (similarManifoldIndex != -1) { - - // Replace the old manifold with the new one - updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo); - } - else { - - // If we have not reach the maximum number of manifolds - if (mNbManifolds < mNbMaxManifolds) { - - // Create the new contact manifold - createManifold(contactManifoldInfo, normalDirectionId); - } - else { - - decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth(); - - // We have reached the maximum number of manifold, we do not - // want to keep the manifold with the smallest penetration detph - int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth); - - // If the manifold with the smallest penetration depth is not the new one, - // we have to keep the new manifold and remove the one with the smallest depth - if (smallestPenDepthManifoldIndex >= 0) { - removeManifold(smallestPenDepthManifoldIndex); - createManifold(contactManifoldInfo, normalDirectionId); - } - } + // Create a new contact manifold + createManifold(contactManifoldInfo); } } + + // Create a new contact manifold + createManifold(contactManifoldInfo); } } // Update a previous similar manifold with a new one -void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) { +void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) { - // For each contact point of the previous manifold - for (int i=0; igetNbContactPoints(); ) { + assert(oldManifold != nullptr); + assert(newManifold != nullptr); - ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i); + // For each contact point of the new manifold + ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo(); + assert(contactPointInfo != nullptr); + while (contactPointInfo != nullptr) { - // For each contact point in the new manifold - ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo(); - bool needToRemovePoint = true; - while (newPoint != nullptr) { + // For each contact point in the old manifold + bool isSimilarPointFound = false; + ContactPoint* oldContactPoint = oldManifold->getContactPoints(); + while (oldContactPoint != nullptr) { + + assert(oldContactPoint != nullptr); // If the new contact point is similar (very close) to the old contact point - if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) { + if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { // Replace (update) the old contact point with the new one - contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(), - mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform()); - needToRemovePoint = false; - newPoint->isUsed = true; + oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + isSimilarPointFound = true; break; } - newPoint = newPoint->next; + oldContactPoint = oldContactPoint->getNext(); } - // If no new contact point is similar to the old one - if (needToRemovePoint) { + // If we have not found a similar contact point + if (!isSimilarPointFound) { - // Remove the old contact point - mManifolds[oldManifoldIndex]->removeContactPoint(i); - } - else { - i++; + // Add the contact point to the manifold + oldManifold->addContactPoint(contactPointInfo); } + + contactPointInfo = contactPointInfo->next; } - // For each point of the new manifold that have not been used yet (to update - // an existing point in the previous manifold), add it into the previous manifold - const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo(); - while (newPointInfo != nullptr) { - - if (!newPointInfo->isUsed) { - mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo); - } - - newPointInfo = newPointInfo->next; - } + // The old manifold is no longer obselete + oldManifold->setIsObselete(false, false); } -// Return the manifold with the smallest contact penetration depth -int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { +// Return the manifold with the smallest contact penetration depth among its points +ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { - // The contact point will be in a new contact manifold, we now have too much - // manifolds condidates. We need to remove one. We choose to remove the manifold - // with the smallest contact depth among their points - int smallestDepthManifoldIndex = -1; - decimal minDepth = initDepth; assert(mNbManifolds == mNbMaxManifolds); - for (int i=0; igetLargestContactDepth(); + + ContactManifold* minDepthManifold = nullptr; + decimal minDepth = initDepth; + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + decimal depth = manifold->getLargestContactDepth(); if (depth < minDepth) { minDepth = depth; - smallestDepthManifoldIndex = i; + minDepthManifold = manifold; } + + manifold = manifold->getNext(); } - return smallestDepthManifoldIndex; + return minDepthManifold; } -// Return the index of the contact manifold with a similar average normal. -// If no manifold has close enough average normal, it returns -1 -int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { +// Return the contact manifold with a similar average normal. +// If no manifold has close enough average normal, it returns nullptr +ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { + + ContactManifold* manifold = mManifolds; // Return the Id of the manifold with the same normal direction id (if exists) - for (int i=0; igetNormalDirectionId()) { - return i; + while (manifold != nullptr) { + if (normalDirectionId == manifold->getContactNormalId()) { + return manifold; } + + manifold = manifold->getNext(); } - return -1; + return nullptr; } // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the // normal vector into of the of the bucket and returns a unique Id for the bucket -short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { +short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) { assert(normal.lengthSquare() > MACHINE_EPSILON); @@ -240,36 +206,100 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons // Clear the contact manifold set void ContactManifoldSet::clear() { - // Destroy all the contact manifolds - for (int i=mNbManifolds-1; i>=0; i--) { - removeManifold(i); + ContactManifold* manifold = mManifolds; + while(manifold != nullptr) { + + ContactManifold* nextManifold = manifold->getNext(); + + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); + + manifold = nextManifold; + + mNbManifolds--; } assert(mNbManifolds == 0); } // Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) { +void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) { assert(mNbManifolds < mNbMaxManifolds); - mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId); + ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator); + manifold->setPrevious(nullptr); + manifold->setNext(mManifolds); + mManifolds = manifold; + mNbManifolds++; } // Remove a contact manifold from the set -void ContactManifoldSet::removeManifold(int index) { +void ContactManifoldSet::removeManifold(ContactManifold* manifold) { assert(mNbManifolds > 0); - assert(index >= 0 && index < mNbManifolds); - // Delete the new contact - mManifolds[index]->~ContactManifold(); - mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); + ContactManifold* previous = manifold->getPrevious(); + ContactManifold* next = manifold->getNext(); - for (int i=index; (i+1) < mNbManifolds; i++) { - mManifolds[i] = mManifolds[i+1]; + if (previous != nullptr) { + previous->setNext(manifold->getNext()); } + if (next != nullptr) { + next->setPrevious(manifold->getPrevious()); + } + + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); mNbManifolds--; } + +// Make all the contact manifolds and contact points obselete +void ContactManifoldSet::makeContactsObselete() { + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + + manifold->setIsObselete(true, true); + + manifold = manifold->getNext(); + } +} + +// Clear the obselete contact manifolds and contact points +void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() { + + ContactManifold* manifold = mManifolds; + ContactManifold* previousManifold = nullptr; + while (manifold != nullptr) { + ContactManifold* nextManifold = manifold->getNext(); + + if (manifold->getIsObselete()) { + + if (previousManifold != nullptr) { + previousManifold->setNext(nextManifold); + + if (nextManifold != nullptr) { + nextManifold->setPrevious(previousManifold); + } + } + else { + mManifolds = nextManifold; + } + + // Delete the contact manifold + removeManifold(manifold); + + } + else { + manifold->clearObseleteContactPoints(); + previousManifold = manifold; + } + + manifold = nextManifold; + } +} diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 1a2bac97..0990277c 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -60,33 +60,34 @@ class ContactManifoldSet { /// Pointer to the second proxy shape of the contact ProxyShape* mShape2; - /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + /// Reference to the memory allocator for the contact manifolds + Allocator& mMemoryAllocator; /// Contact manifolds of the set - ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; + ContactManifold* mManifolds; // -------------------- Methods -------------------- // /// Create a new contact manifold and add it to the set - void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId); + void createManifold(const ContactManifoldInfo* manifoldInfo); - /// Remove a contact manifold from the set - void removeManifold(int index); - - // Return the index of the contact manifold with a similar average normal. - int selectManifoldWithSimilarNormal(short int normalDirectionId) const; - - // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) - // Each face of the cube is divided into 4x4 buckets. This method maps the - // normal vector into of the of the bucket and returns a unique Id for the bucket - short int computeCubemapNormalId(const Vector3& normal) const; + // Return the contact manifold with a similar average normal. + ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const; /// Return the manifold with the smallest contact penetration depth - int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; + ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; /// Update a previous similar manifold with a new one - void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold); + void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); + + /// Return the maximum number of contact manifolds allowed between to collision shapes + int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2); + + /// Clear the contact manifold set + void clear(); + + /// Delete a contact manifold + void removeManifold(ContactManifold* manifold); public: @@ -94,13 +95,13 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds); + Allocator& memoryAllocator); /// Destructor ~ContactManifoldSet(); /// Add a contact manifold in the set - void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); /// Return the first proxy shape ProxyShape* getShape1() const; @@ -108,17 +109,25 @@ class ContactManifoldSet { /// Return the second proxy shape ProxyShape* getShape2() const; - /// Clear the contact manifold set - void clear(); - /// Return the number of manifolds in the set int getNbContactManifolds() const; - /// Return a given contact manifold - ContactManifold* getContactManifold(int index) const; + /// Return a pointer to the first element of the linked-list of contact manifolds + ContactManifold* getContactManifolds() const; + + /// Make all the contact manifolds and contact points obselete + void makeContactsObselete(); /// Return the total number of contact points in the set of manifolds int getTotalNbContactPoints() const; + + /// Clear the obselete contact manifolds and contact points + void clearObseleteManifoldsAndContactPoints(); + + // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) + // Each face of the cube is divided into 4x4 buckets. This method maps the + // normal vector into of the of the bucket and returns a unique Id for the bucket + static short int computeCubemapNormalId(const Vector3& normal); }; // Return the first proxy shape @@ -136,21 +145,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const { return mNbManifolds; } -// Return a given contact manifold -inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { - assert(index >= 0 && index < mNbManifolds); - return mManifolds[index]; +// Return a pointer to the first element of the linked-list of contact manifolds +inline ContactManifold* ContactManifoldSet::getContactManifolds() const { + return mManifolds; } // Return the total number of contact points in the set of manifolds inline int ContactManifoldSet::getTotalNbContactPoints() const { int nbPoints = 0; - for (int i=0; igetNbContactPoints(); + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + nbPoints += manifold->getNbContactPoints(); + + manifold = manifold->getNext(); } + return nbPoints; } +// Return the maximum number of contact manifolds allowed between to collision shapes +inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) { + + // If both shapes are convex + if (shape1->isConvex() && shape2->isConvex()) { + return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + + } // If there is at least one concave shape + else { + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } +} + } #endif diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp new file mode 100644 index 00000000..22ee6666 --- /dev/null +++ b/src/collision/NarrowPhaseInfo.cpp @@ -0,0 +1,97 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include +#include "NarrowPhaseInfo.h" +#include "ContactPointInfo.h" +#include "engine/OverlappingPair.h" + +using namespace reactphysics3d; + +// Constructor +NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, + const CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, void** cachedData1, void** cachedData2) + : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), + shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), + contactPoints(nullptr), cachedCollisionData1(cachedData1), + cachedCollisionData2(cachedData2), next(nullptr) { + +} + +// Destructor +NarrowPhaseInfo::~NarrowPhaseInfo() { + + assert(contactPoints == nullptr); +} + +// Add a new contact point +void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + // Get the memory allocator + Allocator& allocator = overlappingPair->getTemporaryAllocator(); + + // Create the contact point info + ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) + ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + + // Add it into the linked list of contact points + contactPointInfo->next = contactPoints; + contactPoints = contactPointInfo; +} + +/// Take all the generated contact points and create a new potential +/// contact manifold into the overlapping pair +void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() { + overlappingPair->addPotentialContactPoints(this); +} + +// Reset the remaining contact points +void NarrowPhaseInfo::resetContactPoints() { + + // Get the memory allocator + Allocator& allocator = overlappingPair->getTemporaryAllocator(); + + // For each remaining contact point info + ContactPointInfo* element = contactPoints; + while(element != nullptr) { + + ContactPointInfo* elementToDelete = element; + + element = element->next; + + // Call the destructor + elementToDelete->~ContactPointInfo(); + + // Delete the current element + allocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + contactPoints = nullptr; +} diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index b97a71be..29bc6c9d 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -28,6 +28,7 @@ // Libraries #include "shapes/CollisionShape.h" +#include "collision/ContactManifoldInfo.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -58,6 +59,9 @@ struct NarrowPhaseInfo { /// Transform that maps from collision shape 2 local-space to world-space Transform shape2ToWorldTransform; + /// Linked-list of contact points created during the narrow-phase + ContactPointInfo* contactPoints; + /// Cached collision data of the proxy shape // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 void** cachedCollisionData1; @@ -72,12 +76,20 @@ struct NarrowPhaseInfo { /// Constructor NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, const CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2) - : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), - shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), - cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) { + const Transform& shape2Transform, void** cachedData1, void** cachedData2); - } + /// Destructor + ~NarrowPhaseInfo(); + + /// Add a new contact point + void addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2); + + /// Create a new potential contact manifold into the overlapping pair using current contact points + void addContactPointsAsPotentialContactManifold(); + + /// Reset the remaining contact points + void resetContactPoints(); }; } diff --git a/src/collision/OverlapCallback.h b/src/collision/OverlapCallback.h new file mode 100644 index 00000000..bbba73d1 --- /dev/null +++ b/src/collision/OverlapCallback.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H +#define REACTPHYSICS3D_OVERLAP_CALLBACK_H + +// Libraries +#include "body/CollisionBody.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class OverlapCallback +/** + * This class can be used to register a callback for collision overlap queries. + * You should implement your own class inherited from this one and implement + * the notifyOverlap() method. This method will called each time a contact + * point is reported. + */ +class OverlapCallback { + + public: + + /// Destructor + virtual ~OverlapCallback() { + + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody)=0; +}; + +} + +#endif diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 17008622..f98e4d20 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); @@ -116,8 +116,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - segmentsDistance; // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); return true; } @@ -148,7 +148,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - closestPointsDistance; // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); return true; } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 9d8393c9..0b19338f 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -61,8 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index b538dfa7..88bd9ace 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -37,13 +37,12 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a capsule and a polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; SATAlgorithm satAlgorithm; - GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; @@ -61,8 +60,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK - ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); - assert(contactPoint != nullptr); + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + assert(contactPoint != nullptr); bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; @@ -96,7 +95,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK - contactManifoldInfo.reset(); + narrowPhaseInfo->resetContactPoints(); const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; @@ -116,7 +115,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - contactManifoldInfo, isCapsuleShape1); + narrowPhaseInfo, isCapsuleShape1); break; } @@ -133,7 +132,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 6b3f504f..c0dd46d5 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index c0da7611..7f5f439b 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -141,114 +141,114 @@ void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI // Process the concave triangle mesh collision using the smooth mesh collision algorithm described // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision // issue with some internal edges. -void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { +//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, +// std::vector contactPoints, +// NarrowPhaseCallback* narrowPhaseCallback) { - // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; +// // Set with the triangle vertices already processed to void further contacts with same triangle +// std::unordered_multimap processTriangleVertices; - // Sort the list of narrow-phase contacts according to their penetration depth - std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); +// // Sort the list of narrow-phase contacts according to their penetration depth +// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); - // For each contact point (from smaller penetration depth to larger) - std::vector::const_iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { +// // For each contact point (from smaller penetration depth to larger) +// std::vector::const_iterator it; +// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - const SmoothMeshContactInfo info = *it; - const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; +// const SmoothMeshContactInfo info = *it; +// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], - info.triangleVertices[1], - info.triangleVertices[2], - contactPoint, u, v, w); - int nbZeros = 0; - bool isUZero = approxEqual(u, 0, 0.0001); - bool isVZero = approxEqual(v, 0, 0.0001); - bool isWZero = approxEqual(w, 0, 0.0001); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; +// // Compute the barycentric coordinates of the point in the triangle +// decimal u, v, w; +// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], +// info.triangleVertices[1], +// info.triangleVertices[2], +// contactPoint, u, v, w); +// int nbZeros = 0; +// bool isUZero = approxEqual(u, 0, 0.0001); +// bool isVZero = approxEqual(v, 0, 0.0001); +// bool isWZero = approxEqual(w, 0, 0.0001); +// if (isUZero) nbZeros++; +// if (isVZero) nbZeros++; +// if (isWZero) nbZeros++; - // If it is a vertex contact - if (nbZeros == 2) { +// // If it is a vertex contact +// if (nbZeros == 2) { - Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); +// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - // Check that this triangle vertex has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { +// // Check that this triangle vertex has not been processed yet +// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } - } - else if (nbZeros == 1) { // If it is an edge contact +// // Keep the contact as it is and report it +// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); +// } +// } +// else if (nbZeros == 1) { // If it is an edge contact - Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); +// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); +// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); - // Check that this triangle edge has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && - !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { +// // Check that this triangle edge has not been processed yet +// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && +// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } +// // Keep the contact as it is and report it +// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); +// } - } - else { // If it is a face contact +// } +// else { // If it is a face contact - ContactPointInfo newContactInfo(info.contactInfo); +// ContactPointInfo newContactInfo(info.contactInfo); - ProxyShape* firstShape; - ProxyShape* secondShape; - if (info.isFirstShapeTriangle) { - firstShape = overlappingPair->getShape1(); - secondShape = overlappingPair->getShape2(); - } - else { - firstShape = overlappingPair->getShape2(); - secondShape = overlappingPair->getShape1(); - } +// ProxyShape* firstShape; +// ProxyShape* secondShape; +// if (info.isFirstShapeTriangle) { +// firstShape = overlappingPair->getShape1(); +// secondShape = overlappingPair->getShape2(); +// } +// else { +// firstShape = overlappingPair->getShape2(); +// secondShape = overlappingPair->getShape1(); +// } - // We use the triangle normal as the contact normal - Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; - Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; - Vector3 localNormal = a.cross(b); - newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; - newContactInfo.normal.normalize(); - if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { - newContactInfo.normal = -newContactInfo.normal; - } +// // We use the triangle normal as the contact normal +// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; +// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; +// Vector3 localNormal = a.cross(b); +// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; +// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; +// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; +// newContactInfo.normal.normalize(); +// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { +// newContactInfo.normal = -newContactInfo.normal; +// } - // We recompute the contact point on the second body with the new normal as described in - // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and - // Dirk Gregorius) to avoid adding torque - Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); - if (info.isFirstShapeTriangle) { - Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; - newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; - } - else { - Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; - newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; - } +// // We recompute the contact point on the second body with the new normal as described in +// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and +// // Dirk Gregorius) to avoid adding torque +// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); +// if (info.isFirstShapeTriangle) { +// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; +// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; +// } +// else { +// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; +// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; +// } - // Report the contact - narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); - } +// // Report the contact +// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); +// } - // Add the three vertices of the triangle to the set of processed - // triangle vertices - addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); - } -} +// // Add the three vertices of the triangle to the set of processed +// // triangle vertices +// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); +// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); +// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); +// } +//} // Return true if the vertex is in the set of already processed vertices bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h index 531615da..4619e480 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -83,25 +83,35 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { // Class SmoothMeshContactInfo /** - * This class is used to store data about a contact with a triangle for the smooth - * mesh algorithm. + * Contains data for of potential smooth contact during the smooth mesh + * contacts computation. */ -class SmoothMeshContactInfo { +struct SmoothMeshContactInfo { public: - ContactPointInfo contactInfo; + ContactManifoldInfo* contactManifoldInfo; + ContactPointInfo* contactInfo; bool isFirstShapeTriangle; Vector3 triangleVertices[3]; + bool isUVWZero[3]; /// Constructor - SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, - const Vector3& trianglePoint2, const Vector3& trianglePoint3) - : contactInfo(contact) { + SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo, + bool firstShapeTriangle, + const Vector3& trianglePoint1, const Vector3& trianglePoint2, + const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero) + : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) { + isFirstShapeTriangle = firstShapeTriangle; + triangleVertices[0] = trianglePoint1; triangleVertices[1] = trianglePoint2; triangleVertices[2] = trianglePoint3; + + isUVWZero[0] = isUZero; + isUVWZero[1] = isVZero; + isUVWZero[2] = isWZero; } }; @@ -109,7 +119,7 @@ class SmoothMeshContactInfo { struct ContactsDepthCompare { bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) { - return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; + return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth; } }; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 16b9edf5..c6297fb2 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -34,12 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index ed76d6fc..1e0c9748 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 4f7e1177..ff81782f 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -46,8 +46,7 @@ using namespace reactphysics3d; /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. -GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { PROFILE("GJKAlgorithm::testCollision()"); @@ -209,7 +208,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro } // Add a new contact point - contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB); + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); return GJKResult::COLLIDE_IN_MARGIN; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index f40592c4..b8c7319c 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -87,16 +87,13 @@ class GJKAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volumes collide. - GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo); + GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); - - }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index f665181b..035bd8e5 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -83,8 +83,10 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; + // TODO : Use the following reportContacts variable in all narrow-phase algorithms + /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index c838d8ab..4974d367 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -44,7 +44,7 @@ using namespace reactphysics3d; const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Test collision between a sphere and a convex mesh -bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()"); @@ -145,7 +145,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); // Create the contact info object - contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); @@ -171,7 +171,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd } // Test collision between a capsule and a convex mesh -bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()"); @@ -387,7 +387,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - contactManifoldInfo, isCapsuleShape1); + narrowPhaseInfo, isCapsuleShape1); lastFrameInfo.satIsAxisFacePolyhedron1 = true; lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; @@ -406,7 +406,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; // Create the contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); @@ -477,7 +477,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, - ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const { + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); uint firstEdgeIndex = face.edgeIndex; @@ -523,7 +523,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; // Create the contact point - contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, + narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); } @@ -543,8 +543,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, + bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); @@ -869,7 +869,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); // Create a new contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } @@ -893,7 +893,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // Create the contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); lastFrameInfo.satIsAxisFacePolyhedron1 = false; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 7a002fce..49cd2626 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -113,24 +113,24 @@ class SATAlgorithm { SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; /// Test collision between a sphere and a convex mesh - bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; /// Test collision between a capsule and a convex mesh - bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, - ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const; + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const; // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, const Vector3& edgeAdjacentFace2Normal) const; /// Test collision between two convex meshes - bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; }; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 86255c2b..e291fd83 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -34,7 +34,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; @@ -86,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI } // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 154f6e4c..bb9e9866 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index d90a29b3..049ce449 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -34,12 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -61,7 +60,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index f2f8d6e5..2d9fa801 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 54e51e52..94211ac0 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -30,8 +30,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); @@ -62,7 +61,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, + narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1, intersectionOnBody2); return true; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index b70befd2..1fd885a5 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d2b92dcc..5a07b0cb 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -237,6 +237,8 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { case 4: return Vector3(0, -1, 0); case 5: return Vector3(0, 1, 0); } + + assert(false); } // Return the centroid of the box diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 2b1fa576..78b82ae0 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -118,10 +118,6 @@ class CollisionShape { /// Return true if the collision shape type is a convex shape static bool isConvex(CollisionShapeType shapeType); - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types - static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2); - // -------------------- Friendship -------------------- // friend class ProxyShape; @@ -151,19 +147,6 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } -// Return the maximum number of contact manifolds allowed in an overlapping -// pair wit the given two collision shape types -inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2) { - // If both shapes are convex - if (isConvex(shapeType1) && isConvex(shapeType2)) { - return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; - } // If there is at least one concave shape - else { - return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; - } -} - } #endif diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index f420bbff..fa6838ec 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -37,19 +37,20 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), - mIsRestingContact(false) { + mIsRestingContact(false), mIsObselete(false), mNext(nullptr) { assert(mPenetrationDepth > decimal(0.0)); // Compute the world position of the contact points mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; + + mIsObselete = false; } // Update the contact point with a new one that is similar (very close) /// The idea is to keep the cache impulse (for warm starting the contact solver) -void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform) { +void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) { assert(isSimilarWithContactPoint(contactInfo)); assert(contactInfo->penetrationDepth > decimal(0.0)); @@ -62,4 +63,6 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& // Compute the world position of the contact points mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; + + mIsObselete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index ec4c6556..41cc35e4 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -71,6 +71,37 @@ class ContactPoint { /// Cached penetration impulse decimal mPenetrationImpulse; + /// True if the contact point is obselete + bool mIsObselete; + + /// Pointer to the next contact point in the linked-list + ContactPoint* mNext; + + // -------------------- Methods -------------------- // + + /// Update the contact point with a new one that is similar (very close) + void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform); + + /// Return true if the contact point is similar (close enougth) to another given contact point + bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; + + /// Set the cached penetration impulse + void setPenetrationImpulse(decimal impulse); + + + /// Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); + + /// Set to true to make the contact point obselete + void setIsObselete(bool isObselete); + + /// Set the next contact point in the linked list + void setNext(ContactPoint* next); + + /// Return true if the contact point is obselete + bool getIsObselete() const; + public : // -------------------- Methods -------------------- // @@ -88,10 +119,6 @@ class ContactPoint { /// Deleted assignment operator ContactPoint& operator=(const ContactPoint& contact) = delete; - /// Update the contact point with a new one that is similar (very close) - void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform); - /// Return the normal vector of the contact Vector3 getNormal() const; @@ -110,23 +137,22 @@ class ContactPoint { /// Return the cached penetration impulse decimal getPenetrationImpulse() const; - /// Return true if the contact point is similar (close enougth) to another given contact point - bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; - - /// Set the cached penetration impulse - void setPenetrationImpulse(decimal impulse); - /// Return true if the contact is a resting contact bool getIsRestingContact() const; - /// Set the mIsRestingContact variable - void setIsRestingContact(bool isRestingContact); + /// Return the next contact point in the linked list + ContactPoint* getNext() const; /// Return the penetration depth decimal getPenetrationDepth() const; /// Return the number of bytes used by the contact point size_t getSizeInBytes() const; + + // Friendship + friend class ContactManifold; + friend class ContactManifoldSet; + friend class ContactSolver; }; // Return the normal vector of the contact @@ -180,6 +206,26 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } +// Return true if the contact point is obselete +inline bool ContactPoint::getIsObselete() const { + return mIsObselete; +} + +// Set to true to make the contact point obselete +inline void ContactPoint::setIsObselete(bool isObselete) { + mIsObselete = isObselete; +} + +// Return the next contact point in the linked list +inline ContactPoint* ContactPoint::getNext() const { + return mNext; +} + +// Set the next contact point in the linked list +inline void ContactPoint::setNext(ContactPoint* next) { + mNext = next; +} + // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 0e24f0c8..82d2601f 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -233,64 +233,6 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits); } -// Class CollisionCallback -/** - * This class can be used to register a callback for collision test queries. - * You should implement your own class inherited from this one and implement - * the notifyContact() method. This method will called each time a contact - * point is reported. - */ -class CollisionCallback { - - public: - - struct CollisionCallbackInfo { - - public: - const ContactManifoldInfo& contactManifold; - CollisionBody* body1; - CollisionBody* body2; - const ProxyShape* proxyShape1; - const ProxyShape* proxyShape2; - - // Constructor - CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2, - const ProxyShape* proxShape1, const ProxyShape* proxShape2) : - contactManifold(manifold), body1(b1), body2(b2), - proxyShape1(proxShape1), proxyShape2(proxShape2) { - - } - }; - - /// Destructor - virtual ~CollisionCallback() { - - } - - /// This method will be called for each reported contact point - virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; -}; - -// Class OverlapCallback -/** - * This class can be used to register a callback for collision overlap queries. - * You should implement your own class inherited from this one and implement - * the notifyOverlap() method. This method will called each time a contact - * point is reported. - */ -class OverlapCallback { - - public: - - /// Destructor - virtual ~OverlapCallback() { - - } - - /// This method will be called for each reported overlapping bodies - virtual void notifyOverlap(CollisionBody* collisionBody)=0; -}; - } #endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d5825d5c..94a82b14 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -148,10 +148,8 @@ void ContactSolver::initializeForIsland(Island* island) { const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { - - // Get a contact point - ContactPoint* externalContact = externalManifold->getContactPoint(c); + ContactPoint* externalContact = externalManifold->getContactPoints(); + while (externalContact != nullptr) { // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); @@ -200,6 +198,8 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; mNbContactPoints++; + + externalContact = externalContact->getNext(); } mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 4f7bd15a..136d34f6 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -683,9 +683,9 @@ void DynamicsWorld::computeIslands() { // For each contact manifold in which the current body is involded ContactManifoldListElement* contactElement; for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr; - contactElement = contactElement->next) { + contactElement = contactElement->getNext()) { - ContactManifold* contactManifold = contactElement->contactManifold; + ContactManifold* contactManifold = contactElement->getContactManifold(); assert(contactManifold->getNbContactPoints() > 0); @@ -842,12 +842,13 @@ std::vector DynamicsWorld::getContactsList() const { // For each contact manifold of the pair const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int i=0; igetNext(); } } diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h index e8594255..061724a0 100644 --- a/src/engine/EventListener.h +++ b/src/engine/EventListener.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_EVENT_LISTENER_H // Libraries -#include "collision/ContactManifoldInfo.h" +#include "collision/CollisionCallback.h" namespace reactphysics3d { @@ -49,17 +49,11 @@ class EventListener { /// Destructor virtual ~EventListener() = default; - /// Called when a new contact point is found between two bodies that were separated before - /** - * @param contact Information about the contact - */ - virtual void beginContact(const ContactManifoldInfo& contactManifold) {} - /// Called when a new contact point is found between two bodies /** * @param contact Information about the contact */ - virtual void newContact(const ContactManifoldInfo& contactManifold) {} + virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {} /// Called at the beginning of an internal tick of the simulation step. /// Each time the DynamicsWorld::update() method is called, the physics diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index ce628b7d..a23857e3 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -26,12 +26,108 @@ // Libraries #include #include "OverlappingPair.h" +#include "collision/ContactManifoldInfo.h" using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) { + Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator) + : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr), + mTempMemoryAllocator(temporaryMemoryAllocator) { } + +// Create a new potential contact manifold using contact-points from narrow-phase +void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { + + assert(narrowPhaseInfo->contactPoints != nullptr); + + // For each potential contact point to add + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + while (contactPoint != nullptr) { + + ContactPointInfo* nextContactPoint = contactPoint->next; + + // Compute the contact normal id + short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal); + + // Look if the contact point correspond to an existing potential manifold + // (if the contact point normal is similar to the normal of an existing manifold) + ContactManifoldInfo* manifold = mPotentialContactManifolds; + bool similarManifoldFound = false; + while(manifold != nullptr) { + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (manifold->getContactNormalId() == contactNormalId) { + + // Add the contact point to the manifold + manifold->addContactPoint(contactPoint, contactNormalId); + + similarManifoldFound = true; + + break; + } + + manifold = manifold->getNext(); + } + + // If we have not found an existing manifold with a similar contact normal + if (!similarManifoldFound) { + + // Create a new potential contact manifold + ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo))) + ContactManifoldInfo(mTempMemoryAllocator); + + // Add the manifold into the linked-list of potential contact manifolds + manifoldInfo->mNext = mPotentialContactManifolds; + mPotentialContactManifolds = manifoldInfo; + + // Add the contact point to the manifold + manifoldInfo->addContactPoint(contactPoint, contactNormalId); + } + + contactPoint = nextContactPoint; + } + + // All the contact point info of the narrow-phase info have been moved + // into the potential contacts of the overlapping pair + narrowPhaseInfo->contactPoints = nullptr; +} + +// Clear all the potential contact manifolds +void OverlappingPair::clearPotentialContactManifolds() { + + // Do we need to release memory + if (mTempMemoryAllocator.isReleaseNeeded()) { + + ContactManifoldInfo* element = mPotentialContactManifolds; + while(element != nullptr) { + + // Remove the proxy collision shape + ContactManifoldInfo* elementToRemove = element; + element = element->getNext(); + + // Delete the element + elementToRemove->~ContactManifoldInfo(); + mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); + } + } + + mPotentialContactManifolds = nullptr; +} + +// Reduce the number of contact points of all the potential contact manifolds +void OverlappingPair::reducePotentialContactManifolds() { + + // For each potential contact manifold + ContactManifoldInfo* manifold = mPotentialContactManifolds; + while (manifold != nullptr) { + + // Reduce the number of contact points of the manifold + manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform()); + + manifold = manifold->getNext(); + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index ce028f9c..a55df081 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -102,13 +102,19 @@ class OverlappingPair { /// Collision information about the last frame (for temporal coherence) LastFrameCollisionInfo mLastFrameCollisionInfo; + /// Linked-list of potential contact manifold + ContactManifoldInfo* mPotentialContactManifolds; + + /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo + Allocator& mTempMemoryAllocator; + public: // -------------------- Methods -------------------- // /// Constructor OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator); + Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator); /// Destructor ~OverlappingPair() = default; @@ -125,9 +131,6 @@ class OverlappingPair { /// Return the pointer to second body ProxyShape* getShape2() const; - /// Add a contact manifold - void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); - /// Return the last frame collision info LastFrameCollisionInfo& getLastFrameCollisionInfo(); @@ -137,8 +140,32 @@ class OverlappingPair { /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); - /// Clear the contact points of the contact manifold - void clearContactPoints(); + /// Clear all the potential contact manifolds + void clearPotentialContactManifolds(); + + /// Add potential contact-points from narrow-phase into potential contact manifolds + void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo); + + /// Add a contact to the contact manifold + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); + + /// Return a reference to the temporary memory allocator + Allocator& getTemporaryAllocator(); + + /// Return true if one of the shapes of the pair is a concave shape + bool hasConcaveShape() const; + + /// Return a pointer to the first potential contact manifold in the linked-list + ContactManifoldInfo* getPotentialContactManifolds(); + + /// Reduce the number of contact points of all the potential contact manifolds + void reducePotentialContactManifolds(); + + /// Make the contact manifolds and contact points obselete + void makeContactsObselete(); + + /// Clear the obselete contact manifold and contact points + void clearObseleteManifoldsAndContactPoints(); /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -162,7 +189,7 @@ inline ProxyShape* OverlappingPair::getShape2() const { } // Add a contact to the contact manifold -inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { +inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { mContactManifoldSet.addContactManifold(contactManifoldInfo); } @@ -181,6 +208,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { return mContactManifoldSet; } +// Make the contact manifolds and contact points obselete +inline void OverlappingPair::makeContactsObselete() { + + mContactManifoldSet.makeContactsObselete(); +} + // Return the pair of bodies index inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); @@ -205,9 +238,25 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body return indexPair; } -// Clear the contact points of the contact manifold -inline void OverlappingPair::clearContactPoints() { - mContactManifoldSet.clear(); +// Return a reference to the temporary memory allocator +inline Allocator& OverlappingPair::getTemporaryAllocator() { + return mTempMemoryAllocator; +} + +// Return true if one of the shapes of the pair is a concave shape +inline bool OverlappingPair::hasConcaveShape() const { + return !getShape1()->getCollisionShape()->isConvex() || + !getShape2()->getCollisionShape()->isConvex(); +} + +// Return a pointer to the first potential contact manifold in the linked-list +inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { + return mPotentialContactManifolds; +} + +// Clear the obselete contact manifold and contact points +inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() { + mContactManifoldSet.clearObseleteManifoldsAndContactPoints(); } } diff --git a/src/memory/Allocator.h b/src/memory/Allocator.h index f7b9ac73..0440a5c9 100644 --- a/src/memory/Allocator.h +++ b/src/memory/Allocator.h @@ -49,6 +49,9 @@ class Allocator { /// Release previously allocated memory. virtual void release(void* pointer, size_t size)=0; + + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const=0; }; } diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 71e11714..0664e559 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -138,8 +138,16 @@ class PoolAllocator : public Allocator { /// Release previously allocated memory. virtual void release(void* pointer, size_t size) override; + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override; + }; +// Return true if memory needs to be release with this allocator +inline bool PoolAllocator::isReleaseNeeded() const { + return true; +} + } #endif diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h index cc030daa..426c2a2d 100644 --- a/src/memory/SingleFrameAllocator.h +++ b/src/memory/SingleFrameAllocator.h @@ -72,10 +72,15 @@ class SingleFrameAllocator : public Allocator { /// Reset the marker of the current allocated memory virtual void reset(); - - + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override; }; +// Return true if memory needs to be release with this allocator +inline bool SingleFrameAllocator::isReleaseNeeded() const { + return false; +} + } #endif diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index f6b8b8bf..dd2a6265 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -58,6 +58,8 @@ #include "collision/PolyhedronMesh.h" #include "collision/TriangleVertexArray.h" #include "collision/PolygonVertexArray.h" +#include "collision/CollisionCallback.h" +#include "collision/OverlapCallback.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 51a05412..14ceab1f 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -79,26 +79,34 @@ class ContactManager : public rp3d::CollisionCallback { /// This method will be called for each reported contact point virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - // For each contact point - rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo(); - while (contactPointInfo != nullptr) { + // For each contact manifold + rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements; + while (manifoldElement != nullptr) { - // Contact normal - rp3d::Vector3 normal = contactPointInfo->normal; - openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); + // Get the contact manifold + rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold(); - rp3d::Vector3 point1 = contactPointInfo->localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); + // For each contact point + rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { - rp3d::Vector3 point2 = contactPointInfo->localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); + // Contact normal + rp3d::Vector3 normal = contactPoint->getNormal(); + openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); - contactPointInfo = contactPointInfo->next; + rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1(); + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); + + rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2(); + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); + + contactPoint = contactPoint->getNext(); + } } } diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 2cb3e440..85e8a66f 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -342,14 +342,16 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn const rp3d::ContactManifold* manifold = *it; // For each contact point of the manifold - for (uint i=0; igetNbContactPoints(); i++) { + rp3d::ContactPoint* contactPoint = manifold->getContactPoints(); + while (contactPoint != nullptr) { - rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); rp3d::Vector3 normalWorld = contactPoint->getNormal(); openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); contactPoints.push_back(contact); + + contactPoint = contactPoint->getNext(); } }