From 6a69ef76c5cd6467f90c4b3b20ba30704d960696 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 1 Nov 2017 23:07:56 +0100 Subject: [PATCH] Fix issue with ProxyShape::mBroadPhaseId not set when body was sleeping or inactive --- src/body/CollisionBody.cpp | 26 +- src/collision/CollisionDetection.cpp | 245 ++++++++++-------- src/collision/CollisionDetection.h | 8 +- .../broadphase/BroadPhaseAlgorithm.cpp | 6 + .../broadphase/BroadPhaseAlgorithm.h | 3 + src/engine/CollisionWorld.cpp | 11 + src/engine/CollisionWorld.h | 3 + 7 files changed, 178 insertions(+), 124 deletions(-) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 39d47de4..64ce6467 100644 --- a/src/body/CollisionBody.cpp +++ b/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,12 +202,15 @@ 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 { - // Recompute the world-space AABB of the collision shape - AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + if (proxyShape->mBroadPhaseID != -1) { - // Update the broad-phase state for the proxy collision shape - mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); + // Recompute the world-space AABB of the collision shape + AABB aabb; + proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + + // 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,8 +243,11 @@ void CollisionBody::setIsActive(bool isActive) { // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { - // Remove the proxy shape from the collision detection - mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + if (shape->mBroadPhaseID != -1) { + + // Remove the proxy shape from the collision detection + mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + } } // Reset the contact manifold list of the body diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f650351a..baaf0db5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/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::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { @@ -683,87 +689,90 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mPoolAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // If the proxy shape is from a body that we have not already reported collision and the - // two proxy collision shapes are not from the same body - if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() && - proxyShape->getBody()->getID() != bodyId) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // If the proxy shape is from a body that we have not already reported collision and the + // two proxy collision shapes are not from the same body + if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() && + proxyShape->getBody()->getID() != bodyId) { - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); - bool isColliding = false; + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + bool isColliding = false; - // If we have not found a collision yet - if (!isColliding) { + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // If we have not found a collision yet + if (!isColliding) { - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + } } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Return if we have found a narrow-phase collision + if (isColliding) { - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); + CollisionBody* overlapBody = proxyShape->getBody(); - // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } + // Add the body into the set of reported bodies + reportedBodies.insert(overlapBody->getID()); - // Return if we have found a narrow-phase collision - if (isColliding) { - - CollisionBody* overlapBody = proxyShape->getBody(); - - // Add the body into the set of reported bodies - reportedBodies.insert(overlapBody->getID()); - - // Notify the overlap to the user - overlapCallback->notifyOverlap(overlapBody); + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } } } - } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next overlapping proxy shape + element = element->next; + } } // Go to the next proxy shape @@ -858,85 +867,88 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mPoolAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // If the two proxy collision shapes are not from the same body - if (proxyShape->getBody()->getID() != bodyId) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // If the two proxy collision shapes are not from the same body + if (proxyShape->getBody()->getID() != bodyId) { - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { - // Add the contact points as a potential contact manifold into the pair - narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + } } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Process the potential contacts + processPotentialContacts(&pair); - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); + if (pair.hasContacts()) { - // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + callback->notifyContact(collisionInfo); + } } - - // Process the potential contacts - processPotentialContacts(&pair); - - if (pair.hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - callback->notifyContact(collisionInfo); - } } + + // Go to the next overlapping proxy shape + element = element->next; } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); } - - // Go to the next proxy shape - bodyProxyShape = bodyProxyShape->getNext(); } } @@ -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); +} diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 848e7d09..6f28f788 100644 --- a/src/collision/CollisionDetection.h +++ b/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,7 +262,10 @@ 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) { - mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + + if (shape->mBroadPhaseID != -1) { + mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + } } // Update a proxy collision shape (that has moved for instance) diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index bc03f44d..c86cd0bc 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/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); diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 8d2cda35..94499d44 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/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); diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index d160021c..e8f4155a 100644 --- a/src/engine/CollisionWorld.cpp +++ b/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); +} + diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 82d2601f..d27b8018 100644 --- a/src/engine/CollisionWorld.h +++ b/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;