Simplify broad-phase collision detection

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"

src/collision/CollisionDetection.cpp

@@ -573,15 +573,14 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
     Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
 
     // Ask the broad-phase to get all the overlapping shapes
-    LinkedList<int> overlappingNodes(mMemoryManager.getPoolAllocator());
+    List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
     mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
 
     // For each overlaping proxy shape
-    LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+    for (uint i=0; i < overlappingNodes.size(); i++) {
-    while (element != nullptr) {
 
         // 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<int> overlappingNodes(mMemoryManager.getPoolAllocator());
+            List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
             mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
 
             const bodyindex bodyId = body->getId();
 
             // For each overlaping proxy shape
-            LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+            for (uint i=0; i < overlappingNodes.size(); i++) {
-            while (element != nullptr) {
 
                 // 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<int> overlappingNodes(mMemoryManager.getPoolAllocator());
+            List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
             mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
 
             const bodyindex bodyId = body->getId();
 
             // For each overlaping proxy shape
-            LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+            for (uint i=0; i < overlappingNodes.size(); i++) {
-            while (element != nullptr) {
 
                 // 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;
             }
         }
     }

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;
+

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 <functional>
+#include <cassert>
+#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<reactphysics3d::BroadPhasePair> {
+
+    size_t operator()(const reactphysics3d::BroadPhasePair& pair) const {
+
+        assert(pair.shape1BroadPhaseId <= pair.shape2BroadPhaseId);
+
+        std::size_t seed = 0;
+        reactphysics3d::hash_combine<int>(seed, pair.shape1BroadPhaseId);
+        reactphysics3d::hash_combine<int>(seed, pair.shape2BroadPhaseId);
+
+        return seed;
+    }
+  };
+}
+
+#endif

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,
+void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const {
-                                                         DynamicAABBTreeOverlapCallback& callback) const {
 
     // Create a stack with the nodes to visit
     Stack<int, 64> stack(mAllocator);

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,
+        void reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const;
-                                                DynamicAABBTreeOverlapCallback& callback) const;
 
         /// Ray casting method
         void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;

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;
 }

src/systems/BroadPhaseSystem.cpp

@@ -195,8 +195,7 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end
     }
 }
 
-void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb,
+void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int>& overlappingNodes) const {
-                                                             LinkedList<int>& 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<int> overlappingNodes(memoryManager.getPoolAllocator());
+    List<int> 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
+    // Check all the potential overlapping pairs avoiding duplicates to report unique overlapping pairs
-    std::sort(mPotentialPairs.begin(), mPotentialPairs.end(), BroadPhasePair::smallerThan);
-
-    // 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<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair.collisionShape1ID));
+        ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair.shape1BroadPhaseId));
-        ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair.collisionShape2ID));
+        ProxyShape* shape2 = static_cast<ProxyShape*>(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 ||
+            if (nextPair.shape1BroadPhaseId != pair.shape1BroadPhaseId ||
-                nextPair.collisionShape2ID != pair.collisionShape2ID) {
+                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<int>& overlappingNodes) {
+void BroadPhaseSystem::addOverlappingNodes(int referenceNodeId, const List<int>& overlappingNodes) {
 
     // For each overlapping node in the linked list
-    LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
+    for (uint i=0; i < overlappingNodes.size(); i++) {
-    while (elem != nullptr) {
 
         // 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),
+            mPotentialPairs.add(BroadPhasePair(referenceNodeId, overlappingNodes[i]));
-                                               std::max(referenceNodeId, elem->data)));
         }
-
-        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

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 <cstring>
 
 /// 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<int>& mOverlappingNodes;
+        List<int>& mOverlappingNodes;
 
         // Constructor
-        AABBOverlapCallback(LinkedList<int>& overlappingNodes)
+        AABBOverlapCallback(List<int>& overlappingNodes) : mOverlappingNodes(overlappingNodes) {
-             : mOverlappingNodes(overlappingNodes) {
 
         }
 
@@ -158,7 +130,7 @@ class BroadPhaseSystem {
         Set<int> mMovedShapes;
 
         /// Temporary array of potential overlapping pairs (with potential duplicates)
-        List<BroadPhasePair> mPotentialPairs;
+        Set<BroadPhasePair> 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<int>& overlappingNodes);
+        void addOverlappingNodes(int broadPhaseId1, const List<int>& overlappingNodes);
 
         /// Report all the shapes that are overlapping with a given AABB
-        void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
+        void reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int>& 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);