From 102651832a4a9f20434f6b951761d3162972b11e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 25 Mar 2019 18:47:42 +0100 Subject: [PATCH] Simplify broad-phase collision detection --- CMakeLists.txt | 1 + src/collision/CollisionDetection.cpp | 30 ++---- src/collision/broadphase/BroadPhasePair.cpp | 31 ++++++ src/collision/broadphase/BroadPhasePair.h | 104 +++++++++++++++++++ src/collision/broadphase/DynamicAABBTree.cpp | 3 +- src/collision/broadphase/DynamicAABBTree.h | 3 +- src/engine/Entity.h | 4 +- src/systems/BroadPhaseSystem.cpp | 39 +++---- src/systems/BroadPhaseSystem.h | 53 ++-------- 9 files changed, 170 insertions(+), 98 deletions(-) create mode 100644 src/collision/broadphase/BroadPhasePair.cpp create mode 100644 src/collision/broadphase/BroadPhasePair.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 63cc8a17..445b8a70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,6 +82,7 @@ SET (REACTPHYSICS3D_HEADERS "src/body/RigidBody.h" "src/collision/ContactPointInfo.h" "src/collision/broadphase/DynamicAABBTree.h" + "src/collision/broadphase/BroadPhasePair.h" "src/collision/narrowphase/CollisionDispatch.h" "src/collision/narrowphase/GJK/VoronoiSimplex.h" "src/collision/narrowphase/GJK/GJKAlgorithm.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 92c5e36c..caabf756 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -573,15 +573,14 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over Set reportedBodies(mMemoryManager.getPoolAllocator()); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + List overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + for (uint i=0; i < overlappingNodes.size(); i++) { // Get the overlapping proxy shape - int broadPhaseId = element->data; + const int broadPhaseId = overlappingNodes[i]; ProxyShape* proxyShape = mBroadPhaseSystem.getProxyShapeForBroadPhaseId(broadPhaseId); CollisionBody* overlapBody = proxyShape->getBody(); @@ -599,9 +598,6 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over overlapCallback->notifyOverlap(overlapBody); } } - - // Go to the next overlapping proxy shape - element = element->next; } } @@ -668,17 +664,16 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const AABB& shapeAABB = mBroadPhaseSystem.getFatAABB(bodyProxyShape->getBroadPhaseId()); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + List overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getId(); // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + for (uint i=0; i < overlappingNodes.size(); i++) { // Get the overlapping proxy shape - int broadPhaseId = element->data; + const int broadPhaseId = overlappingNodes[i]; ProxyShape* proxyShape = mBroadPhaseSystem.getProxyShapeForBroadPhaseId(broadPhaseId); // If the proxy shape is from a body that we have not already reported collision and the @@ -711,9 +706,6 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl narrowPhaseInput.clear(); } } - - // Go to the next overlapping proxy shape - element = element->next; } } } @@ -821,17 +813,16 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const AABB& shapeAABB = mBroadPhaseSystem.getFatAABB(bodyProxyShape->getBroadPhaseId()); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + List overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getId(); // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + for (uint i=0; i < overlappingNodes.size(); i++) { // Get the overlapping proxy shape - int broadPhaseId = element->data; + const int broadPhaseId = overlappingNodes[i]; ProxyShape* proxyShape = mBroadPhaseSystem.getProxyShapeForBroadPhaseId(broadPhaseId); // If the two proxy collision shapes are not from the same body @@ -866,9 +857,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput); } } - - // Go to the next overlapping proxy shape - element = element->next; } } } diff --git a/src/collision/broadphase/BroadPhasePair.cpp b/src/collision/broadphase/BroadPhasePair.cpp new file mode 100644 index 00000000..ba590031 --- /dev/null +++ b/src/collision/broadphase/BroadPhasePair.cpp @@ -0,0 +1,31 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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 "BroadPhasePair.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + diff --git a/src/collision/broadphase/BroadPhasePair.h b/src/collision/broadphase/BroadPhasePair.h new file mode 100644 index 00000000..7379365f --- /dev/null +++ b/src/collision/broadphase/BroadPhasePair.h @@ -0,0 +1,104 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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_BROAD_PHASE_PAIR_H +#define REACTPHYSICS3D_BROAD_PHASE_PAIR_H + +// Libraries +#include +#include +#include"containers/Pair.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Structure BroadPhasePair +/** + * This structure represent a potential overlapping pair during the + * broad-phase collision detection. + */ +struct BroadPhasePair { + + public: + + // -------------------- Attributes -------------------- // + + // Broad-phase id of the first collision shape + int shape1BroadPhaseId; + + // Broad-phase id of the second collision shape + int shape2BroadPhaseId; + + // -------------------- Methods -------------------- // + + /// Constructor + BroadPhasePair(int shapeId1, int shapeId2) + : shape1BroadPhaseId(std::min(shapeId1, shapeId2)), + shape2BroadPhaseId(std::max(shapeId1, shapeId2)) { + + assert(shape1BroadPhaseId != -1); + assert(shape2BroadPhaseId != -1); + } + + /// Equality operator + bool operator==(const BroadPhasePair& pair) const; + + /// Inequality operator + bool operator!=(const BroadPhasePair& pair) const; +}; + +// Equality operator +inline bool BroadPhasePair::operator==(const BroadPhasePair& pair) const { + + return shape1BroadPhaseId == pair.shape1BroadPhaseId && shape2BroadPhaseId == pair.shape2BroadPhaseId; +} + +// Inequality operator +inline bool BroadPhasePair::operator!=(const BroadPhasePair& pair) const { + return shape1BroadPhaseId != pair.shape1BroadPhaseId || shape2BroadPhaseId != pair.shape2BroadPhaseId; +} + +} + +// Hash function for a reactphysics3d BroadPhasePair +namespace std { + + template <> struct hash { + + size_t operator()(const reactphysics3d::BroadPhasePair& pair) const { + + assert(pair.shape1BroadPhaseId <= pair.shape2BroadPhaseId); + + std::size_t seed = 0; + reactphysics3d::hash_combine(seed, pair.shape1BroadPhaseId); + reactphysics3d::hash_combine(seed, pair.shape2BroadPhaseId); + + return seed; + } + }; +} + +#endif diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index bdc85b5e..f693ab79 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -593,8 +593,7 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { } /// Report all shapes overlapping with the AABB given in parameter. -void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const { +void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const { // Create a stack with the nodes to visit Stack stack(mAllocator); diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 0b0a5828..1876b396 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -237,8 +237,7 @@ class DynamicAABBTree { void* getNodeDataPointer(int nodeID) const; /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const; + void reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const; /// Ray casting method void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; diff --git a/src/engine/Entity.h b/src/engine/Entity.h index 85f3d0a9..222195fd 100644 --- a/src/engine/Entity.h +++ b/src/engine/Entity.h @@ -97,7 +97,7 @@ struct Entity { /// Equality operator bool operator==(const Entity& entity) const; - /// Inequality operator (it != end()) + /// Inequality operator bool operator!=(const Entity& entity) const; // -------------------- Friendship -------------------- // @@ -122,7 +122,7 @@ inline bool Entity::operator==(const Entity& entity) const { return entity.id == id; } -// Inequality operator (it != end()) +// Inequality operator inline bool Entity::operator!=(const Entity& entity) const { return entity.id != id; } diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index f9cdfa92..8d9a0174 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -195,8 +195,7 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end } } -void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, - LinkedList& overlappingNodes) const { +void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const { AABBOverlapCallback callback(overlappingNodes); @@ -207,12 +206,10 @@ void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, // Compute all the overlapping pairs of collision shapes void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) { - // TODO : Try to see if we can allocate potential pairs in single frame allocator - // Reset the potential overlapping pairs mPotentialPairs.clear(); - LinkedList overlappingNodes(memoryManager.getPoolAllocator()); + List overlappingNodes(memoryManager.getPoolAllocator()); // For all collision shapes that have moved (or have been created) during the last simulation step for (auto it = mMovedShapes.begin(); it != mMovedShapes.end(); ++it) { @@ -234,18 +231,14 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) { addOverlappingNodes(shapeID, overlappingNodes); // Remove all the elements of the linked list of overlapping nodes - overlappingNodes.reset(); + overlappingNodes.clear(); } // Reset the array of collision shapes that have move (or have been created) during the // last simulation step mMovedShapes.clear(); - // Sort the array of potential overlapping pairs in order to remove duplicate pairs - std::sort(mPotentialPairs.begin(), mPotentialPairs.end(), BroadPhasePair::smallerThan); - - // Check all the potential overlapping pairs avoiding duplicates to report unique - // overlapping pairs + // Check all the potential overlapping pairs avoiding duplicates to report unique overlapping pairs auto it = mPotentialPairs.begin(); while (it != mPotentialPairs.end()) { @@ -253,11 +246,11 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) { BroadPhasePair& pair = *it; ++it; - assert(pair.collisionShape1ID != pair.collisionShape2ID); + assert(pair.shape1BroadPhaseId != pair.shape2BroadPhaseId); // Get the two collision shapes of the pair - ProxyShape* shape1 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair.collisionShape1ID)); - ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair.collisionShape2ID)); + ProxyShape* shape1 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair.shape1BroadPhaseId)); + ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair.shape2BroadPhaseId)); // If the two proxy collision shapes are from the same body, skip it if (shape1->getBody()->getId() != shape2->getBody()->getId()) { @@ -273,8 +266,8 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) { BroadPhasePair& nextPair = *it; // If the next pair is different from the previous one, we stop skipping pairs - if (nextPair.collisionShape1ID != pair.collisionShape1ID || - nextPair.collisionShape2ID != pair.collisionShape2ID) { + if (nextPair.shape1BroadPhaseId != pair.shape1BroadPhaseId || + nextPair.shape2BroadPhaseId != pair.shape2BroadPhaseId) { break; } ++it; @@ -283,28 +276,24 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) { } // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree -void BroadPhaseSystem::addOverlappingNodes(int referenceNodeId, const LinkedList& overlappingNodes) { +void BroadPhaseSystem::addOverlappingNodes(int referenceNodeId, const List& overlappingNodes) { // For each overlapping node in the linked list - LinkedList::ListElement* elem = overlappingNodes.getListHead(); - while (elem != nullptr) { + for (uint i=0; i < overlappingNodes.size(); i++) { // If both the nodes are the same, we do not create the overlapping pair - if (referenceNodeId != elem->data) { + if (referenceNodeId != overlappingNodes[i]) { // Add the new potential pair into the array of potential overlapping pairs - mPotentialPairs.add(BroadPhasePair(std::min(referenceNodeId, elem->data), - std::max(referenceNodeId, elem->data))); + mPotentialPairs.add(BroadPhasePair(referenceNodeId, overlappingNodes[i])); } - - elem = elem->next; } } // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { - mOverlappingNodes.insert(nodeId); + mOverlappingNodes.add(nodeId); } // Called for a broad-phase shape that has to be tested for raycast diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index 94047e36..d57a0ac8 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/src/systems/BroadPhaseSystem.h @@ -33,6 +33,8 @@ #include "components/ProxyShapeComponents.h" #include "components/TransformComponents.h" #include "components/DynamicsComponents.h" +#include "collision/broadphase/BroadPhasePair.h" +#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -45,45 +47,15 @@ class ProxyShape; class MemoryManager; class Profiler; -// Structure BroadPhasePair -/** - * This structure represent a potential overlapping pair during the - * broad-phase collision detection. - */ -struct BroadPhasePair { - - // -------------------- Attributes -------------------- // - - /// Broad-phase ID of the first collision shape - int collisionShape1ID; - - /// Broad-phase ID of the second collision shape - int collisionShape2ID; - - // -------------------- Methods -------------------- // - - /// Constructor - BroadPhasePair(int shapeId1, int shapeId2) { - collisionShape1ID = shapeId1; - collisionShape2ID = shapeId2; - } - - /// Method used to compare two pairs for sorting algorithm - static bool smallerThan(const BroadPhasePair &pair1, const BroadPhasePair &pair2); -}; - // class AABBOverlapCallback class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - private: - public: - LinkedList& mOverlappingNodes; + List& mOverlappingNodes; // Constructor - AABBOverlapCallback(LinkedList& overlappingNodes) - : mOverlappingNodes(overlappingNodes) { + AABBOverlapCallback(List& overlappingNodes) : mOverlappingNodes(overlappingNodes) { } @@ -158,7 +130,7 @@ class BroadPhaseSystem { Set mMovedShapes; /// Temporary array of potential overlapping pairs (with potential duplicates) - List mPotentialPairs; + Set mPotentialPairs; /// Reference to the collision detection object CollisionDetection& mCollisionDetection; @@ -215,10 +187,10 @@ class BroadPhaseSystem { void removeMovedCollisionShape(int broadPhaseID); /// Add potential overlapping pairs in the dynamic AABB tree - void addOverlappingNodes(int broadPhaseId1, const LinkedList& overlappingNodes); + void addOverlappingNodes(int broadPhaseId1, const List& overlappingNodes); /// Report all the shapes that are overlapping with a given AABB - void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList& overlappingNodes) const; + void reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const; /// Compute all the overlapping pairs of collision shapes void computeOverlappingPairs(MemoryManager& memoryManager); @@ -244,17 +216,6 @@ class BroadPhaseSystem { }; -// Method used to compare two pairs for sorting algorithm -inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, - const BroadPhasePair& pair2) { - - if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true; - if (pair1.collisionShape1ID == pair2.collisionShape1ID) { - return pair1.collisionShape2ID < pair2.collisionShape2ID; - } - return false; -} - // Return the fat AABB of a given broad-phase shape inline const AABB& BroadPhaseSystem::getFatAABB(int broadPhaseId) const { return mDynamicAABBTree.getFatAABB(broadPhaseId);