diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index d637d0a7..393481d0 100644 --- a/src/body/RigidBody.cpp +++ b/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 diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 5d09b87d..6738cae0 100644 --- a/src/collision/CollisionDetection.cpp +++ b/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 - if (mIsCollisionShapesAdded) { + // Ask the broad-phase to compute all the shapes overlapping the shapes that + // have moved or have been added in the last frame. This call can only add new + // overlapping pairs in the collision detection. + List> overlappingNodes(mMemoryManager.getPoolAllocator(), 32); + mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager, overlappingNodes); - // Ask the broad-phase to recompute the overlapping pairs of collision - // shapes. This call can only add new overlapping pairs in the collision - // detection. - mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager); + // Create new overlapping pairs if necessary + updateOverlappingPairs(overlappingNodes); +} + +// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary +void CollisionDetection::updateOverlappingPairs(List>& overlappingNodes) { + + List newOverlappingPairs(mMemoryManager.getPoolAllocator(), overlappingNodes.size()); + + // For each overlapping pair of nodes + for (uint i=0; i < overlappingNodes.size(); i++) { + + Pair 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 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, 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 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, 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 pairID = OverlappingPair::computeID(body1ProxyShape, body2ProxyShape); + const Pair 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 pairID = OverlappingPair::computeID(bodyProxyShape, proxyShape); + const Pair 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 pairID = OverlappingPair::computeID(originalPair->getShape1(), originalPair->getShape2()); + const Pair 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); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index b9c7e249..8c1104ef 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -90,8 +90,8 @@ class CollisionDetection { /// Set of pair of bodies that cannot collide between each other Set mNoCollisionPairs; - /// True if some collision shapes have been added previously - bool mIsCollisionShapesAdded; + /// Map a broad-phase id with the corresponding entity of the proxy-shape + Map 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 >& 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,14 +249,18 @@ inline CollisionDispatch& CollisionDetection::getCollisionDispatch() { } // Add a body to the collision detection -inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, - const AABB& aabb) { - +inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, 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(broadPhaseId, proxyShape->getEntity())); +} // Add a pair of bodies that cannot collide with each other inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 80635721..dbf09e3b 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/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& nodesToTest, size_t startIndex, + size_t endIndex, List>& outOverlappingNodes) const { + + // Create a stack with the nodes to visit + Stack 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(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 stack(mAllocator); + Stack 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 stack(mAllocator); + Stack 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(); diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 1876b396..8a09a1db 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/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& nodesToTest, size_t startIndex, + size_t endIndex, List>& outOverlappingNodes) const; + /// Report all shapes overlapping with the AABB given in parameter. void reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const; diff --git a/src/components/Components.h b/src/components/Components.h index d0bdf129..e4dc213e 100644 --- a/src/components/Components.h +++ b/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); diff --git a/src/components/ProxyShapeComponents.h b/src/components/ProxyShapeComponents.h index d56eed49..7c9c8364 100644 --- a/src/components/ProxyShapeComponents.h +++ b/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 { diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 977030b8..09809a17 100644 --- a/src/engine/CollisionWorld.cpp +++ b/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 diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 99f8152a..67d0bb18 100644 --- a/src/engine/OverlappingPair.cpp +++ b/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) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index d25d20af..1fae4fb1 100644 --- a/src/engine/OverlappingPair.h +++ b/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) { - assert(shape1->getBroadPhaseId() >= 0 && shape2->getBroadPhaseId() >= 0); +inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) { + assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0); // Construct the pair of body index - OverlappingPairId pairID = shape1->getBroadPhaseId() < shape2->getBroadPhaseId() ? - OverlappingPairId(shape1->getBroadPhaseId(), shape2->getBroadPhaseId()) : - OverlappingPairId(shape2->getBroadPhaseId(), shape1->getBroadPhaseId()); + OverlappingPairId pairID = shape1BroadPhaseId < shape2BroadPhaseId ? + OverlappingPairId(shape1BroadPhaseId, shape2BroadPhaseId) : + OverlappingPairId(shape2BroadPhaseId, shape1BroadPhaseId); assert(pairID.first != pairID.second); return pairID; } diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index d9b0480e..448df4f1 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/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>& overlappingNodes) { - List overlappingNodes(memoryManager.getPoolAllocator()); + // Get the list of the proxy-shapes that have moved or have been created in the last frame + List shapesToTest = mMovedShapes.toList(memoryManager.getPoolAllocator()); - // For all collision shapes that have moved (or have been created) during the last simulation step - for (auto it = mMovedShapes.begin(); it != mMovedShapes.end(); ++it) { - 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(); - } + // Ask the dynamic AABB tree to report all collision shapes that overlap with the shapes to test + mDynamicAABBTree.reportAllShapesOverlappingWithShapes(shapesToTest, 0, shapesToTest.size(), overlappingNodes); // 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& 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(mDynamicAABBTree.getNodeDataPointer(referenceNodeId)); - ProxyShape* shape2 = static_cast(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) { diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index 7f991bbd..140486c3 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/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& overlappingNodes); - /// Report all the shapes that are overlapping with a given AABB void reportAllShapesOverlappingWithAABB(const AABB& aabb, List& overlappingNodes) const; /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(MemoryManager& memoryManager); + void computeOverlappingPairs(MemoryManager& memoryManager, List >& 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); }