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

This commit is contained in:
Daniel Chappuis 2017-11-01 23:07:56 +01:00
parent 5da57a96c8
commit 6a69ef76c5
7 changed files with 178 additions and 124 deletions

View File

@ -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

View File

@ -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,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<int> 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<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
// For each overlaping proxy shape
LinkedList<int>::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<int>::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<int> 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<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
// For each overlaping proxy shape
LinkedList<int>::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<int>::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);
}

View File

@ -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)

View File

@ -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);

View File

@ -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);

View File

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

View File

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