Simplify broad-phase

src/body/RigidBody.cpp

@@ -466,9 +466,6 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
  */
 void RigidBody::setTransform(const Transform& transform) {
 
-    // Update the transform of the body
-    mWorld.mTransformComponents.setTransform(mEntity, transform);
-
     const Vector3 oldCenterOfMass = mCenterOfMassWorld;
 
     // Compute the new center of mass in world-space coordinates
@@ -480,14 +477,13 @@ void RigidBody::setTransform(const Transform& transform) {
     linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
     mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
+    CollisionBody::setTransform(transform);
+
+    // Update the transform of the body
+    mWorld.mTransformComponents.setTransform(mEntity, transform);
+
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
-
-    // Update the broad-phase state of the body
-    updateBroadPhaseState();
-
-    RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set transform=" + transform.to_string());
 }
 
 // Recompute the center of mass, total mass and inertia tensor of the body using all

src/collision/CollisionDetection.cpp

@@ -54,7 +54,7 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponen
                      mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
                      mOverlappingPairs(mMemoryManager.getPoolAllocator()),
                      mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
-                     mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false),
+                     mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
                      mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -86,13 +86,84 @@ void CollisionDetection::computeBroadPhase() {
 
     RP3D_PROFILE("CollisionDetection::computeBroadPhase()", mProfiler);
 
-    // If new collision shapes have been added to bodies
+    // Ask the broad-phase to compute all the shapes overlapping the shapes that
-    if (mIsCollisionShapesAdded) {
+    // have moved or have been added in the last frame. This call can only add new
+    // overlapping pairs in the collision detection.
+    List<Pair<int, int>> overlappingNodes(mMemoryManager.getPoolAllocator(), 32);
+    mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager, overlappingNodes);
 
-        // Ask the broad-phase to recompute the overlapping pairs of collision
+    // Create new overlapping pairs if necessary
-        // shapes. This call can only add new overlapping pairs in the collision
+    updateOverlappingPairs(overlappingNodes);
-        // detection.
+}
-        mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager);
+
+// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
+void CollisionDetection::updateOverlappingPairs(List<Pair<int, int>>& overlappingNodes) {
+
+    List<OverlappingPair*> newOverlappingPairs(mMemoryManager.getPoolAllocator(), overlappingNodes.size());
+
+    // For each overlapping pair of nodes
+    for (uint i=0; i < overlappingNodes.size(); i++) {
+
+        Pair<int, int> nodePair = overlappingNodes[i];
+
+        assert(nodePair.first != -1);
+        assert(nodePair.second != -1);
+
+        // Skip pairs with same overlapping nodes
+        if (nodePair.first != nodePair.second) {
+
+            // Get the two proxy-shapes
+            Entity proxyShape1Entity = mMapBroadPhaseIdToProxyShapeEntity[nodePair.first];
+            Entity proxyShape2Entity = mMapBroadPhaseIdToProxyShapeEntity[nodePair.second];
+
+            // Get the two bodies
+            Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity);
+            Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity);
+
+            // If the two proxy collision shapes are from the same body, skip it
+            if (body1Entity != body2Entity) {
+
+                // Compute the overlapping pair ID
+                Pair<uint, uint> pairID = OverlappingPair::computeID(nodePair.first, nodePair.second);
+
+                // Check if the overlapping pair already exists
+                if (!mOverlappingPairs.containsKey(pairID)) {
+
+                    unsigned short shape1CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity);
+                    unsigned short shape2CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity);
+
+                    unsigned short shape1CollisionCategoryBits = mProxyShapesComponents.getCollisionCategoryBits(proxyShape1Entity);
+                    unsigned short shape2CollisionCategoryBits = mProxyShapesComponents.getCollisionCategoryBits(proxyShape2Entity);
+
+                    // Check if the collision filtering allows collision between the two shapes
+                    if ((shape1CollideWithMaskBits & shape2CollisionCategoryBits) != 0 &&
+                        (shape1CollisionCategoryBits & shape2CollideWithMaskBits) != 0) {
+
+                        ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(proxyShape1Entity);
+                        ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(proxyShape2Entity);
+
+                        // Create the overlapping pair and add it into the set of overlapping pairs
+                        OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
+                                                  OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(),
+                                                                  mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig);
+
+                        assert(newPair != nullptr);
+
+                        // Add the new overlapping pair
+                        mOverlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, newPair));
+                        newOverlappingPairs.add(newPair);
+                    }
+                }
+            }
+        }
+    }
+
+    // For each new overlapping pair
+    for (uint i=0; i < newOverlappingPairs.size(); i++) {
+
+        // Wake up the two bodies of the new overlapping pair
+        mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
+        mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
     }
 }
 
@@ -337,43 +408,11 @@ void CollisionDetection::computeNarrowPhase() {
     mNarrowPhaseInput.clear();
 }
 
-// Allow the broadphase to notify the collision detection about an overlapping pair.
-/// This method is called by the broad-phase collision detection algorithm
-void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
-
-    assert(shape1->getBroadPhaseId() != -1);
-    assert(shape2->getBroadPhaseId() != -1);
-    assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId());
-
-    // Compute the overlapping pair ID
-    Pair<uint, uint> pairID = OverlappingPair::computeID(shape1, shape2);
-
-    // Check if the overlapping pair already exists
-    if (mOverlappingPairs.containsKey(pairID)) return;
-
-    // Check if the collision filtering allows collision between the two shapes
-    if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
-        (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
-
-    // Create the overlapping pair and add it into the set of overlapping pairs
-    OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
-                              OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(),
-                                              mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig);
-
-    assert(newPair != nullptr);
-
-    // Add the new overlapping pair
-    mOverlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, newPair));
-
-    // Wake up the two bodies
-    shape1->getBody()->setIsSleeping(false);
-    shape2->getBody()->setIsSleeping(false);
-}
-
 // Remove a body from the collision detection
 void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
 
     assert(proxyShape->getBroadPhaseId() != -1);
+    assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShape->getBroadPhaseId()));
 
     // Remove all the overlapping pairs involving this proxy shape
     for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -395,6 +434,8 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
         }
     }
 
+    mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShape->getBroadPhaseId());
+
     // Remove the body from the broad-phase
     mBroadPhaseSystem.removeProxyCollisionShape(proxyShape);
 }
@@ -739,7 +780,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
             if (aabb1.testCollision(aabb2)) {
 
                 OverlappingPair* pair;
-                const Pair<uint, uint> pairID = OverlappingPair::computeID(body1ProxyShape, body2ProxyShape);
+                const Pair<uint, uint> pairID = OverlappingPair::computeID(body1ProxyShape->getBroadPhaseId(), body2ProxyShape->getBroadPhaseId());
 
                 // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
                 auto itPair = overlappingPairs.find(pairID);
@@ -832,7 +873,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
                     if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
 
                         OverlappingPair* pair;
-                        const Pair<uint, uint> pairID = OverlappingPair::computeID(bodyProxyShape, proxyShape);
+                        const Pair<uint, uint> pairID = OverlappingPair::computeID(bodyProxyShape->getBroadPhaseId(), proxyShape->getBroadPhaseId());
 
                         // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
                         auto itPair = overlappingPairs.find(pairID);
@@ -905,7 +946,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
         OverlappingPair* originalPair = it->second;
 
         OverlappingPair* pair;
-        const Pair<uint, uint> pairID = OverlappingPair::computeID(originalPair->getShape1(), originalPair->getShape2());
+        const Pair<uint, uint> pairID = OverlappingPair::computeID(originalPair->getShape1()->getBroadPhaseId(), originalPair->getShape2()->getBroadPhaseId());
 
         // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
         auto itPair = overlappingPairs.find(pairID);

src/collision/CollisionDetection.h

@@ -90,8 +90,8 @@ class CollisionDetection {
         /// Set of pair of bodies that cannot collide between each other
         Set<bodyindexpair> mNoCollisionPairs;
 
-        /// True if some collision shapes have been added previously
+        /// Map a broad-phase id with the corresponding entity of the proxy-shape
-        bool mIsCollisionShapesAdded;
+        Map<int, Entity> mMapBroadPhaseIdToProxyShapeEntity;
 
         /// Narrow-phase collision detection input
         NarrowPhaseInput mNarrowPhaseInput;
@@ -114,6 +114,9 @@ class CollisionDetection {
         /// Compute the narrow-phase collision detection
         void computeNarrowPhase();
 
+        /// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
+        void updateOverlappingPairs(List<Pair<int, int> >& overlappingNodes);
+
         /// Execute the narrow-phase collision detection algorithm on batches
         bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound,
                                       bool reportContacts, MemoryAllocator& allocator);
@@ -215,9 +218,6 @@ class CollisionDetection {
         /// Test and report collisions between all shapes of the world
         void testCollision(CollisionCallback* callback);
 
-        /// Allow the broadphase to notify the collision detection about an overlapping pair.
-        void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
-
         /// Return a reference to the memory manager
         MemoryManager& getMemoryManager() const;
 
@@ -249,13 +249,17 @@ inline CollisionDispatch& CollisionDetection::getCollisionDispatch() {
 }
 
 // Add a body to the collision detection
-inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
+inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
-                                                       const AABB& aabb) {
 
     // Add the body to the broad-phase
     mBroadPhaseSystem.addProxyCollisionShape(proxyShape, aabb);
 
-    mIsCollisionShapesAdded = true;
+    int broadPhaseId = mProxyShapesComponents.getBroadPhaseId(proxyShape->getEntity());
+
+    assert(!mMapBroadPhaseIdToProxyShapeEntity.containsKey(broadPhaseId));
+
+    // Add the mapping between the proxy-shape broad-phase id and its entity
+    mMapBroadPhaseIdToProxyShapeEntity.add(Pair<int, Entity>(broadPhaseId, proxyShape->getEntity()));
 }
 
 // Add a pair of bodies that cannot collide with each other

src/collision/broadphase/DynamicAABBTree.cpp

@@ -592,15 +592,66 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
     return nodeID;
 }
 
+/// Take a list of shapes to be tested for broad-phase overlap and return a list of pair of overlapping shapes
+void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List<int>& nodesToTest, size_t startIndex,
+                                                           size_t endIndex, List<Pair<int, int>>& outOverlappingNodes) const {
+
+    // Create a stack with the nodes to visit
+    Stack<int> stack(mAllocator, 64);
+
+    // For each shape to be tested for overlap
+    for (uint i=startIndex; i < endIndex; i++) {
+
+        assert(nodesToTest[i] != -1);
+
+        stack.push(mRootNodeID);
+
+        const AABB& shapeAABB = getFatAABB(nodesToTest[i]);
+
+        // While there are still nodes to visit
+        while(stack.size() > 0) {
+
+            // Get the next node ID to visit
+            const int nodeIDToVisit = stack.pop();
+
+            // Skip it if it is a null node
+            if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
+
+            // Get the corresponding node
+            const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
+
+            // If the AABB in parameter overlaps with the AABB of the node to visit
+            if (shapeAABB.testCollision(nodeToVisit->aabb)) {
+
+                // If the node is a leaf
+                if (nodeToVisit->isLeaf()) {
+
+                    // Add the node in the list of overlapping nodes
+                    outOverlappingNodes.add(Pair<int, int>(nodesToTest[i], nodeIDToVisit));
+                }
+                else {  // If the node is not a leaf
+
+                    // We need to visit its children
+                    stack.push(nodeToVisit->children[0]);
+                    stack.push(nodeToVisit->children[1]);
+                }
+            }
+        }
+
+        stack.clear();
+    }
+}
+
 /// Report all shapes overlapping with the AABB given in parameter.
+// TODO : Do not use this method anymore. Use
 void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const {
 
     // Create a stack with the nodes to visit
-    Stack<int, 64> stack(mAllocator);
+    Stack<int> stack(mAllocator, 64);
     stack.push(mRootNodeID);
 
     // While there are still nodes to visit
-    while(stack.getNbElements() > 0) {
+    while(stack.size() > 0) {
 
         // Get the next node ID to visit
         const int nodeIDToVisit = stack.pop();
@@ -637,12 +688,12 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
 
     decimal maxFraction = ray.maxFraction;
 
-    Stack<int, 128> stack(mAllocator);
+    Stack<int> stack(mAllocator, 128);
     stack.push(mRootNodeID);
 
     // Walk through the tree from the root looking for proxy shapes
     // that overlap with the ray AABB
-    while (stack.getNbElements() > 0) {
+    while (stack.size() > 0) {
 
         // Get the next node in the stack
         int nodeID = stack.pop();

src/collision/broadphase/DynamicAABBTree.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "configuration.h"
 #include "collision/shapes/AABB.h"
+#include "containers/Set.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -236,6 +237,10 @@ class DynamicAABBTree {
         /// Return the data pointer of a given leaf node of the tree
         void* getNodeDataPointer(int nodeID) const;
 
+        /// Report all shapes overlapping with all the shapes in the map in parameter
+        void reportAllShapesOverlappingWithShapes(const List<int>& nodesToTest, size_t startIndex,
+                                                  size_t endIndex, List<Pair<int, int>>& outOverlappingNodes) const;
+
         /// Report all shapes overlapping with the AABB given in parameter.
         void reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const;
 

src/components/Components.h

@@ -107,7 +107,10 @@ class Components {
         /// Remove a component
         void removeComponent(Entity entity);
 
-        /// Notify if a given entity is disabled (sleeping or inactive) or not
+        /// Return true if an entity is sleeping
+        bool getIsEntityDisabled(Entity entity) const;
+
+        /// Notify if a given entity is sleeping
         void setIsEntityDisabled(Entity entity, bool isDisabled);
 
         /// Return true if there is a component for a given entity
@@ -120,6 +123,12 @@ class Components {
         uint32 getNbEnabledComponents() const;
 };
 
+// Return true if an entity is sleeping
+inline bool Components::getIsEntityDisabled(Entity entity) const {
+    assert(hasComponent(entity));
+    return mMapEntityToComponentIndex[entity] >= mDisabledStartIndex;
+}
+
 // Return true if there is a component for a given entity
 inline bool Components::hasComponent(Entity entity) const {
     return mMapEntityToComponentIndex.containsKey(entity);

src/components/ProxyShapeComponents.h

@@ -55,10 +55,10 @@ class ProxyShapeComponents : public Components {
 
         // -------------------- Attributes -------------------- //
 
-        /// Array of entities of each component
+        /// Array of body entity of each component
         Entity* mBodiesEntities;
 
-        /// Array of entities of each component
+        /// Array of proxy-shape entity of each component
         Entity* mProxyShapesEntities;
 
         /// Array of pointer to the proxy-shapes
@@ -140,6 +140,9 @@ class ProxyShapeComponents : public Components {
         /// Add a component
         void addComponent(Entity proxyShapeEntity, bool isSleeping, const ProxyShapeComponent& component);
 
+        /// Return the body entity of a given proxy-shape
+        Entity getBody(Entity proxyShapeEntity) const;
+
         /// Return the mass of a proxy-shape
         decimal getMass(Entity proxyShapeEntity) const;
 
@@ -178,6 +181,14 @@ class ProxyShapeComponents : public Components {
         friend class BroadPhaseSystem;
 };
 
+// Return the body entity of a given proxy-shape
+inline Entity ProxyShapeComponents::getBody(Entity proxyShapeEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity));
+
+   return mBodiesEntities[mMapEntityToComponentIndex[proxyShapeEntity]];
+}
+
 // Return the mass of a proxy-shape
 inline decimal ProxyShapeComponents::getMass(Entity proxyShapeEntity) const {
 

src/engine/CollisionWorld.cpp

@@ -250,6 +250,8 @@ void CollisionWorld::resetContactManifoldListsOfBodies() {
 // Notify the world if a body is disabled (sleeping or inactive) or not
 void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
 
+    if (isDisabled == mBodyComponents.getIsEntityDisabled(bodyEntity)) return;
+
     // TODO : Make sure we notify all the components here ...
 
     // Notify all the components

src/engine/OverlappingPair.cpp

@@ -35,7 +35,7 @@ using namespace reactphysics3d;
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator,
                                  const WorldSettings& worldSettings)
-                : mPairID(computeID(shape1, shape2)), mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings),
+                : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings),
                   mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
                   mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings) {
     

src/engine/OverlappingPair.h

@@ -192,7 +192,7 @@ class OverlappingPair {
         void makeLastFrameCollisionInfosObsolete();
 
         /// Return the pair of bodies index
-        static OverlappingPairId computeID(ProxyShape* shape1, ProxyShape* shape2);
+        static OverlappingPairId computeID(int shape1BroadPhaseId, int shape2BroadPhaseId);
 
         /// Return the pair of bodies index of the pair
         static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
@@ -234,13 +234,13 @@ inline void OverlappingPair::makeContactsObsolete() {
 }
 
 // Return the pair of bodies index
-inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
+inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) {
-    assert(shape1->getBroadPhaseId() >= 0 && shape2->getBroadPhaseId() >= 0);
+    assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0);
 
     // Construct the pair of body index
-    OverlappingPairId pairID = shape1->getBroadPhaseId() < shape2->getBroadPhaseId() ?
+    OverlappingPairId pairID = shape1BroadPhaseId < shape2BroadPhaseId ?
-                             OverlappingPairId(shape1->getBroadPhaseId(), shape2->getBroadPhaseId()) :
+                             OverlappingPairId(shape1BroadPhaseId, shape2BroadPhaseId) :
-                             OverlappingPairId(shape2->getBroadPhaseId(), shape1->getBroadPhaseId());
+                             OverlappingPairId(shape2BroadPhaseId, shape1BroadPhaseId);
     assert(pairID.first != pairID.second);
     return pairID;
 }

src/systems/BroadPhaseSystem.cpp

@@ -69,7 +69,7 @@ bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1,
 void BroadPhaseSystem::raycast(const Ray& ray, RaycastTest& raycastTest,
                                          unsigned short raycastWithCategoryMaskBits) const {
 
-    RP3D_PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler);
+    RP3D_PROFILE("BroadPhaseSystem::raycast()", mProfiler);
 
     BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
 
@@ -203,60 +203,19 @@ void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, List
 }
 
 // Compute all the overlapping pairs of collision shapes
-void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) {
+void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int>>& overlappingNodes) {
 
-    List<int> overlappingNodes(memoryManager.getPoolAllocator());
+    // Get the list of the proxy-shapes that have moved or have been created in the last frame
+    List<int> shapesToTest = mMovedShapes.toList(memoryManager.getPoolAllocator());
 
-    // For all collision shapes that have moved (or have been created) during the last simulation step
+    // Ask the dynamic AABB tree to report all collision shapes that overlap with the shapes to test
-    for (auto it = mMovedShapes.begin(); it != mMovedShapes.end(); ++it) {
+    mDynamicAABBTree.reportAllShapesOverlappingWithShapes(shapesToTest, 0, shapesToTest.size(), overlappingNodes);
-        int shapeID = *it;
-
-        if (shapeID == -1) continue;
-
-        AABBOverlapCallback callback(overlappingNodes);
-
-        // Get the AABB of the shape
-        const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
-
-        // Ask the dynamic AABB tree to report all collision shapes that overlap with
-        // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
-        // by the dynamic AABB tree for each potential overlapping pair.
-        mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
-
-        // Add the potential overlapping pairs
-        addOverlappingNodes(shapeID, overlappingNodes);
-
-        // Remove all the elements of the linked list of overlapping nodes
-        overlappingNodes.clear();
-    }
 
     // Reset the array of collision shapes that have move (or have been created) during the
     // last simulation step
     mMovedShapes.clear();
 }
 
-// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
-void BroadPhaseSystem::addOverlappingNodes(int referenceNodeId, const List<int>& overlappingNodes) {
-
-    // For each overlapping node in the list
-    for (uint i=0; i < overlappingNodes.size(); i++) {
-
-        if (referenceNodeId != overlappingNodes[i]) {
-
-            // Get the two collision shapes of the pair
-            ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(referenceNodeId));
-            ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(overlappingNodes[i]));
-
-            // If the two proxy collision shapes are from the same body, skip it
-            if (shape1->getBody()->getId() != shape2->getBody()->getId()) {
-
-                // Notify the collision detection about the overlapping pair
-                mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
-            }
-        }
-    }
-}
-
 // Called when a overlapping node has been found during the call to
 // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
 void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {

src/systems/BroadPhaseSystem.h

@@ -182,14 +182,11 @@ class BroadPhaseSystem {
         /// step and that need to be tested again for broad-phase overlapping.
         void removeMovedCollisionShape(int broadPhaseID);
 
-        /// Add potential overlapping pairs in the dynamic AABB tree
-        void addOverlappingNodes(int broadPhaseId1, const List<int>& overlappingNodes);
-
         /// Report all the shapes that are overlapping with a given AABB
         void reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int>& overlappingNodes) const;
 
         /// Compute all the overlapping pairs of collision shapes
-        void computeOverlappingPairs(MemoryManager& memoryManager);
+        void computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int> >& overlappingNodes);
 
         /// Return the proxy shape corresponding to the broad-phase node id in parameter
         ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
@@ -221,6 +218,8 @@ inline const AABB& BroadPhaseSystem::getFatAABB(int broadPhaseId) const  {
 // and that need to be tested again for broad-phase overlapping.
 inline void BroadPhaseSystem::addMovedCollisionShape(int broadPhaseID) {
 
+    assert(broadPhaseID != -1);
+
     // Store the broad-phase ID into the array of shapes that have moved
     mMovedShapes.add(broadPhaseID);
 }