Fix issue with ProxyShape::mBroadPhaseId not set when body was sleeping or inactive

src/body/CollisionBody.cpp

@@ -111,7 +111,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
     if (current == proxyShape) {
         mProxyCollisionShapes = current->mNext;
 
-        if (mIsActive) {
+        if (mIsActive && proxyShape->mBroadPhaseID != -1) {
             mWorld.mCollisionDetection.removeProxyCollisionShape(current);
         }
 
@@ -131,7 +131,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
             ProxyShape* elementToRemove = current->mNext;
             current->mNext = elementToRemove->mNext;
 
-            if (mIsActive) {
+            if (mIsActive && proxyShape->mBroadPhaseID != -1) {
                 mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
             }
 
@@ -157,7 +157,7 @@ void CollisionBody::removeAllCollisionShapes() {
         // Remove the proxy collision shape
         ProxyShape* nextElement = current->mNext;
 
-        if (mIsActive) {
+        if (mIsActive && current->mBroadPhaseID != -1) {
             mWorld.mCollisionDetection.removeProxyCollisionShape(current);
         }
 
@@ -202,6 +202,8 @@ void CollisionBody::updateBroadPhaseState() const {
 // Update the broad-phase state of a proxy collision shape of the body
 void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
 
+    if (proxyShape->mBroadPhaseID != -1) {
+
         // Recompute the world-space AABB of the collision shape
         AABB aabb;
         proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
@@ -209,6 +211,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
         // Update the broad-phase state for the proxy collision shape
         mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert)	;
     }
+}
 
 // Set whether or not the body is active
 /**
@@ -240,9 +243,12 @@ void CollisionBody::setIsActive(bool isActive) {
         // For each proxy shape of the body
         for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
 
+            if (shape->mBroadPhaseID != -1) {
+
                 // Remove the proxy shape from the collision detection
                 mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
             }
+        }
 
         // Reset the contact manifold list of the body
         resetContactManifoldsList();

src/collision/CollisionDetection.cpp

@@ -109,6 +109,8 @@ void CollisionDetection::computeMiddlePhase() {
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
 
+        assert(shape1->mBroadPhaseID != -1);
+        assert(shape2->mBroadPhaseID != -1);
         assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
 
         // Check if the two shapes are still overlapping. Otherwise, we destroy the
@@ -282,6 +284,8 @@ void CollisionDetection::computeNarrowPhase() {
 /// This method is called by the broad-phase collision detection algorithm
 void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
 
+    assert(shape1->mBroadPhaseID != -1);
+    assert(shape2->mBroadPhaseID != -1);
     assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
 
     // Check if the collision filtering allows collision between the two shapes
@@ -313,6 +317,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
 // Remove a body from the collision detection
 void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
 
+    assert(proxyShape->mBroadPhaseID != -1);
+
     // Remove all the overlapping pairs involving this proxy shape
     std::map<overlappingpairid, OverlappingPair*>::iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -683,6 +689,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
     ProxyShape* bodyProxyShape = body->getProxyShapesList();
     while (bodyProxyShape != nullptr) {
 
+        if (bodyProxyShape->mBroadPhaseID != -1) {
+
             // Get the AABB of the shape
             const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
 
@@ -765,6 +773,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
                 // Go to the next overlapping proxy shape
                 element = element->next;
             }
+        }
 
         // Go to the next proxy shape
         bodyProxyShape = bodyProxyShape->getNext();
@@ -858,6 +867,8 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
     ProxyShape* bodyProxyShape = body->getProxyShapesList();
     while (bodyProxyShape != nullptr) {
 
+        if (bodyProxyShape->mBroadPhaseID != -1) {
+
             // Get the AABB of the shape
             const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
 
@@ -939,6 +950,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
             bodyProxyShape = bodyProxyShape->getNext();
         }
     }
+}
 
 // Test and report collisions between all shapes of the world
 void CollisionDetection::testCollision(CollisionCallback* callback) {
@@ -1031,7 +1043,14 @@ EventListener* CollisionDetection::getWorldEventListener() {
    return mWorld->mEventListener;
 }
 
-/// Return a reference to the world memory allocator
+// Return a reference to the world memory allocator
 PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
   return mWorld->mPoolAllocator;
 }
+
+
+// Return the world-space AABB of a given proxy shape
+const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const {
+    assert(proxyShape->mBroadPhaseID > -1);
+    return mBroadPhaseAlgorithm.getFatAABB(proxyShape->mBroadPhaseID);
+}

src/collision/CollisionDetection.h

@@ -219,6 +219,9 @@ class CollisionDetection {
         /// Return a reference to the world memory allocator
         PoolAllocator& getWorldMemoryAllocator();
 
+        /// Return the world-space AABB of a given proxy shape
+        const AABB getWorldAABB(const ProxyShape* proxyShape) const;
+
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -259,8 +262,11 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
 /// We simply put the shape in the list of collision shape that have moved in the
 /// previous frame so that it is tested for collision again in the broad-phase.
 inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
+
+    if (shape->mBroadPhaseID != -1) {
         mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
     }
+}
 
 // Update a proxy collision shape (that has moved for instance)
 inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -117,6 +117,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
 // Add a proxy collision shape into the broad-phase collision detection
 void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
 
+    assert(proxyShape->mBroadPhaseID == -1);
+
     // Add the collision shape into the dynamic AABB tree and get its broad-phase ID
     int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
 
@@ -131,8 +133,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A
 // Remove a proxy collision shape from the broad-phase collision detection
 void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
 
+    assert(proxyShape->mBroadPhaseID != -1);
+
     int broadPhaseID = proxyShape->mBroadPhaseID;
 
+    proxyShape->mBroadPhaseID = -1;
+
     // Remove the collision shape from the dynamic AABB tree
     mDynamicAABBTree.removeObject(broadPhaseID);
 

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -232,6 +232,9 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad
 // Return true if the two broad-phase collision shapes are overlapping
 inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
                                                        const ProxyShape* shape2) const {
+
+    if (shape1->mBroadPhaseID == -1 || shape2->mBroadPhaseID == -1) return false;
+
     // Get the two AABBs of the collision shapes
     const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
     const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);

src/engine/CollisionWorld.cpp

@@ -164,3 +164,14 @@ bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) {
     return mCollisionDetection.testOverlap(body1, body2);
 }
 
+
+// Return the current world-space AABB of given proxy shape
+AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const {
+
+    if (proxyShape->mBroadPhaseID == -1) {
+        return AABB();
+    }
+
+   return mCollisionDetection.getWorldAABB(proxyShape);
+}
+

src/engine/CollisionWorld.h

@@ -147,6 +147,9 @@ class CollisionWorld {
         /// Test and report collisions between all shapes of the world
         void testCollision(CollisionCallback* callback);
 
+        /// Return the current world-space AABB of given proxy shape
+        AABB getWorldAABB(const ProxyShape* proxyShape) const;
+
         // -------------------- Friendship -------------------- //
 
         friend class CollisionDetection;