Simplify broad-phase

src/body/Body.cpp

@@ -52,6 +52,8 @@ Body::Body(Entity entity, bodyindex id)
 void Body::setIsActive(bool isActive) {
     mIsActive = isActive;
 
+    setIsSleeping(isActive);
+
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
              "Body " + std::to_string(mID) + ": Set isActive=" +
              (mIsActive ? "true" : "false"));
@@ -60,6 +62,12 @@ void Body::setIsActive(bool isActive) {
 // Set the variable to know whether or not the body is sleeping
 void Body::setIsSleeping(bool isSleeping) {
 
+    // If the body is not active, do nothing (it is sleeping)
+    if (!mIsActive) {
+        assert(mIsSleeping);
+        return;
+    }
+
     if (isSleeping) {
         mSleepTime = decimal(0.0);
     }

src/body/CollisionBody.cpp

@@ -250,9 +250,6 @@ void CollisionBody::setIsActive(bool isActive) {
 
     Body::setIsActive(isActive);
 
-    // Enable the components
-    mWorld.notifyBodyDisabled(mEntity, !isActive);
-
     // If we have to activate the body
     if (isActive) {
 
@@ -426,6 +423,9 @@ void CollisionBody::setTransform(const Transform& transform) {
 
     // TODO : Make sure this method is never called from the internal physics engine
 
+    // Awake the body if it is sleeping
+    setIsSleeping(false);
+
     // Update the transform of the body
     mWorld.mTransformComponents.setTransform(mEntity, transform);
 
@@ -436,7 +436,6 @@ void CollisionBody::setTransform(const Transform& transform) {
              "Body " + std::to_string(mID) + ": Set transform=" + transform.to_string());
 }
 
-
 // Set the variable to know whether or not the body is sleeping
 void CollisionBody::setIsSleeping(bool isSleeping) {
 

src/body/CollisionBody.h

@@ -101,6 +101,9 @@ class CollisionBody : public Body {
         /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
         int resetIsAlreadyInIslandAndCountManifolds();
 
+        /// Set the variable to know whether or not the body is sleeping
+        virtual void setIsSleeping(bool isSleeping) override;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -132,9 +135,6 @@ class CollisionBody : public Body {
         /// Set the current position and orientation
         virtual void setTransform(const Transform& transform);
 
-        /// Set the variable to know whether or not the body is sleeping
-        virtual void setIsSleeping(bool isSleeping) override;
-
         /// Add a collision shape to the body.
         virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
                                               const Transform& transform);
@@ -163,9 +163,6 @@ class CollisionBody : public Body {
         /// Return a pointer to a given proxy-shape of the body
         ProxyShape* getProxyShape(uint proxyShapeIndex);
 
-        /// Return the linked list of proxy shapes of that body
-        const ProxyShape* getProxyShapesList() const;
-
         /// Return the number of proxy-shapes associated with this body
         uint getNbProxyShapes() const;
 

src/body/RigidBody.cpp

@@ -606,14 +606,14 @@ Vector3 RigidBody::getAngularVelocity() const {
 // Set the variable to know whether or not the body is sleeping
 void RigidBody::setIsSleeping(bool isSleeping) {
 
+    CollisionBody::setIsSleeping(isSleeping);
+
     if (isSleeping) {
         mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
         mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
         mExternalForce.setToZero();
         mExternalTorque.setToZero();
     }
-
-    CollisionBody::setIsSleeping(isSleeping);
 }
 
 #ifdef IS_PROFILING_ACTIVE

src/collision/CollisionDetection.cpp

@@ -345,16 +345,16 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
     assert(shape2->getBroadPhaseId() != -1);
     assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId());
 
-    // Check if the collision filtering allows collision between the two shapes
-    if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
-        (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
-
     // 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(),

src/collision/broadphase/DynamicAABBTree.cpp

@@ -603,7 +603,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, Dynam
     while(stack.getNbElements() > 0) {
 
         // Get the next node ID to visit
-        int nodeIDToVisit = stack.pop();
+        const int nodeIDToVisit = stack.pop();
 
         // Skip it if it is a null node
         if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;

src/engine/CollisionWorld.cpp

@@ -256,6 +256,10 @@ void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
     mBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled);
     mTransformComponents.setIsEntityDisabled(bodyEntity, isDisabled);
 
+    if (mDynamicsComponents.hasComponent(bodyEntity)) {
+        mDynamicsComponents.setIsEntityDisabled(bodyEntity, isDisabled);
+    }
+
     // For each proxy-shape of the body
     const List<Entity>& proxyShapesEntities = mBodyComponents.getProxyShapes(bodyEntity);
     for (uint i=0; i < proxyShapesEntities.size(); i++) {

src/systems/BroadPhaseSystem.cpp

@@ -41,7 +41,6 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, Proxy
                      mProxyShapesComponents(proxyShapesComponents), mTransformsComponents(transformComponents),
                      mDynamicsComponents(dynamicsComponents),
                      mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()),
-                     mPotentialPairs(collisionDetection.getMemoryManager().getPoolAllocator()),
                      mCollisionDetection(collisionDetection) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -206,9 +205,6 @@ void BroadPhaseSystem::reportAllShapesOverlappingWithAABB(const AABB& aabb, List
 // Compute all the overlapping pairs of collision shapes
 void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) {
 
-    // Reset the potential overlapping pairs
-    mPotentialPairs.clear();
-
     List<int> overlappingNodes(memoryManager.getPoolAllocator());
 
     // For all collision shapes that have moved (or have been created) during the last simulation step
@@ -237,20 +233,19 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) {
     // Reset the array of collision shapes that have move (or have been created) during the
     // last simulation step
     mMovedShapes.clear();
+}
 
-    // Check all the potential overlapping pairs avoiding duplicates to report unique overlapping pairs
+// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
-    auto it = mPotentialPairs.begin();
+void BroadPhaseSystem::addOverlappingNodes(int referenceNodeId, const List<int>& overlappingNodes) {
-    while (it != mPotentialPairs.end()) {
 
-        // Get a potential overlapping pair
+    // For each overlapping node in the list
-        BroadPhasePair& pair = *it;
+    for (uint i=0; i < overlappingNodes.size(); i++) {
-        ++it;
 
-        assert(pair.shape1BroadPhaseId != pair.shape2BroadPhaseId);
+        if (referenceNodeId != overlappingNodes[i]) {
 
             // Get the two collision shapes of the pair
-        ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair.shape1BroadPhaseId));
+            ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(referenceNodeId));
-        ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair.shape2BroadPhaseId));
+            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()) {
@@ -258,34 +253,6 @@ void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager) {
                 // Notify the collision detection about the overlapping pair
                 mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
             }
-
-        // Skip the duplicate overlapping pairs
-        while (it != mPotentialPairs.end()) {
-
-            // Get the next pair
-            BroadPhasePair& nextPair = *it;
-
-            // If the next pair is different from the previous one, we stop skipping pairs
-            if (nextPair.shape1BroadPhaseId != pair.shape1BroadPhaseId ||
-                nextPair.shape2BroadPhaseId != pair.shape2BroadPhaseId) {
-                break;
-            }
-            ++it;
-        }
-    }
-}
-
-// 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 linked list
-    for (uint i=0; i < overlappingNodes.size(); i++) {
-
-        // If both the nodes are the same, we do not create the overlapping pair
-        if (referenceNodeId != overlappingNodes[i]) {
-
-            // Add the new potential pair into the array of potential overlapping pairs
-            mPotentialPairs.add(BroadPhasePair(referenceNodeId, overlappingNodes[i]));
         }
     }
 }

src/systems/BroadPhaseSystem.h

@@ -33,7 +33,6 @@
 #include "components/ProxyShapeComponents.h"
 #include "components/TransformComponents.h"
 #include "components/DynamicsComponents.h"
-#include "collision/broadphase/BroadPhasePair.h"
 #include <cstring>
 
 /// Namespace ReactPhysics3D
@@ -129,9 +128,6 @@ class BroadPhaseSystem {
         /// for overlapping in the next simulation step.
         Set<int> mMovedShapes;
 
-        /// Temporary array of potential overlapping pairs (with potential duplicates)
-        Set<BroadPhasePair> mPotentialPairs;
-
         /// Reference to the collision detection object
         CollisionDetection& mCollisionDetection;
 

test/tests/collision/TestDynamicAABBTree.h

@@ -39,6 +39,7 @@ class TestOverlapCallback : public DynamicAABBTreeOverlapCallback {
 
     public :
 
+        // TODO : Replace this by rp3d::List
         std::vector<int> mOverlapNodes;
 
         // Called when a overlapping node has been found during the call to

testbed/common/PhysicsObject.cpp

@@ -75,8 +75,6 @@ void PhysicsObject::setTransform(const rp3d::Transform& transform) {
     // Reset the transform
     mBody->setTransform(transform);
 
-    mBody->setIsSleeping(false);
-
     // Reset the velocity of the rigid body
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     if (rigidBody != nullptr) {