From b410b26e2387bc2fc860c95e869f2feadc400573 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 14 Jul 2020 22:35:03 +0200 Subject: [PATCH] Fix issues in PhysicsWorld::createIslands() method and add more optimization in CollisionDetectionSystem::computeNarrowPhase() method --- .../reactphysics3d/components/Components.h | 16 +++ .../components/RigidBodyComponents.h | 13 ++ .../systems/CollisionDetectionSystem.h | 12 +- src/components/RigidBodyComponents.cpp | 10 +- src/engine/PhysicsWorld.cpp | 127 ++++++++---------- src/systems/CollisionDetectionSystem.cpp | 63 ++++----- 6 files changed, 126 insertions(+), 115 deletions(-) diff --git a/include/reactphysics3d/components/Components.h b/include/reactphysics3d/components/Components.h index 71785930..ba0c15f1 100644 --- a/include/reactphysics3d/components/Components.h +++ b/include/reactphysics3d/components/Components.h @@ -113,6 +113,9 @@ class Components { /// Return true if there is a component for a given entity bool hasComponent(Entity entity) const; + /// Return true if there is a component for a given entiy and if so set the entity index + bool hasComponentGetIndex(Entity entity, uint32& entityIndex) const; + /// Return the number of components uint32 getNbComponents() const; @@ -134,6 +137,19 @@ inline bool Components::hasComponent(Entity entity) const { return mMapEntityToComponentIndex.containsKey(entity); } +// Return true if there is a component for a given entiy and if so set the entity index +inline bool Components::hasComponentGetIndex(Entity entity, uint32& entityIndex) const { + + auto it = mMapEntityToComponentIndex.find(entity); + + if (it != mMapEntityToComponentIndex.end()) { + entityIndex = it->second; + return true; + } + + return false; +} + // Return the number of components inline uint32 Components::getNbComponents() const { return mNbComponents; diff --git a/include/reactphysics3d/components/RigidBodyComponents.h b/include/reactphysics3d/components/RigidBodyComponents.h index 1be18761..8097aaf3 100644 --- a/include/reactphysics3d/components/RigidBodyComponents.h +++ b/include/reactphysics3d/components/RigidBodyComponents.h @@ -145,6 +145,9 @@ class RigidBodyComponents : public Components { /// For each body, the list of joints entities the body is part of List* mJoints; + /// For each body, the list of the indices of contact pairs in which the body is involved + List* mContactPairs; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -342,6 +345,9 @@ class RigidBodyComponents : public Components { /// Remove a joint from a body component void removeJointFromBody(Entity bodyEntity, Entity jointEntity); + /// A an associated contact pairs into the contact pairs array of the body + void addContacPair(Entity bodyEntity, uint contactPairIndex); + // -------------------- Friendship -------------------- // friend class PhysicsWorld; @@ -769,6 +775,13 @@ inline void RigidBodyComponents::removeJointFromBody(Entity bodyEntity, Entity j mJoints[mMapEntityToComponentIndex[bodyEntity]].remove(jointEntity); } +// A an associated contact pairs into the contact pairs array of the body +inline void RigidBodyComponents::addContacPair(Entity bodyEntity, uint contactPairIndex) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mContactPairs[mMapEntityToComponentIndex[bodyEntity]].add(contactPairIndex); +} + } #endif diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h index b2d24f39..22bda63e 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -83,6 +83,9 @@ class CollisionDetectionSystem { /// Reference the collider components ColliderComponents& mCollidersComponents; + /// Reference to the rigid bodies components + RigidBodyComponents& mRigidBodyComponents; + /// Collision Detection Dispatch configuration CollisionDispatch mCollisionDispatch; @@ -163,9 +166,6 @@ class CollisionDetectionSystem { /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2) List* mCurrentContactPoints; - /// Map a body entity to the list of contact pairs in which it is involved - Map> mMapBodyToContactPairs; - #ifdef IS_RP3D_PROFILING_ENABLED /// Pointer to the profiler @@ -224,14 +224,12 @@ class CollisionDetectionSystem { void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo, List& potentialContactPoints, Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, List* contactPairs, - Map>& mapBodyToContactPairs); + List& potentialContactManifolds, List* contactPairs); /// Process the potential contacts after narrow-phase collision detection void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List& potentialContactPoints, Map* mapPairIdToContactPairIndex, - List& potentialContactManifolds, List* contactPairs, - Map>& mapBodyToContactPairs); + List& potentialContactManifolds, List* contactPairs); /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts void reducePotentialContactManifolds(List* contactPairs, List& potentialContactManifolds, diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index fa3086e1..4653daab 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -43,7 +43,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(bool) + sizeof(bool) + sizeof(List)) { + sizeof(bool) + sizeof(bool) + sizeof(List) + sizeof(List)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -89,6 +89,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); List* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); + List* newContactPairs = reinterpret_cast*>(newJoints + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -121,6 +122,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); memcpy(newJoints, mJoints, mNbComponents * sizeof(List)); + memcpy(newContactPairs, mContactPairs, mNbComponents * sizeof(List)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -155,6 +157,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mIsGravityEnabled = newIsGravityEnabled; mIsAlreadyInIsland = newIsAlreadyInIsland; mJoints = newJoints; + mContactPairs = newContactPairs; } // Add a component @@ -191,6 +194,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mIsGravityEnabled[index] = true; mIsAlreadyInIsland[index] = false; new (mJoints + index) List(mMemoryAllocator); + new (mContactPairs + index) List(mMemoryAllocator); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -235,6 +239,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; new (mJoints + destIndex) List(mJoints[srcIndex]); + new (mContactPairs + destIndex) List(mContactPairs[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -278,6 +283,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { bool isGravityEnabled1 = mIsGravityEnabled[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; List joints1 = mJoints[index1]; + List contactPairs1 = mContactPairs[index1]; // Destroy component 1 destroyComponent(index1); @@ -312,6 +318,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mIsGravityEnabled[index2] = isGravityEnabled1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; new (mJoints + index2) List(joints1); + new (mContactPairs + index2) List(contactPairs1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -347,4 +354,5 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mCentersOfMassLocal[index].~Vector3(); mCentersOfMassWorld[index].~Vector3(); mJoints[index].~List(); + mContactPairs[index].~List(); } diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 02465fbd..a9c73a60 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -233,9 +233,9 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { mCollisionBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); mTransformComponents.setIsEntityDisabled(bodyEntity, isDisabled); - if (mRigidBodyComponents.hasComponent(bodyEntity)) { - mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); - } + assert(mRigidBodyComponents.hasComponent(bodyEntity)); + + mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); // For each collider of the body const List& collidersEntities = mCollisionBodyComponents.getColliders(bodyEntity); @@ -245,27 +245,23 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { } // Disable the joints of the body if necessary - if (mRigidBodyComponents.hasComponent(bodyEntity)) { + // For each joint of the body + const List& joints = mRigidBodyComponents.getJoints(bodyEntity); + for(uint32 i=0; i < joints.size(); i++) { - // For each joint of the body - const List& joints = mRigidBodyComponents.getJoints(bodyEntity); - for(uint32 i=0; i < joints.size(); i++) { + const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); + const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); - const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); - const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); + // If both bodies of the joint are disabled + if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { - // If both bodies of the joint are disabled - if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && - mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { + // We disable the joint + setJointDisabled(joints[i], true); + } + else { - // We disable the joint - setJointDisabled(joints[i], true); - } - else { - - // Enable the joint - setJointDisabled(joints[i], false); - } + // Enable the joint + setJointDisabled(joints[i], false); } } } @@ -760,10 +756,10 @@ void PhysicsWorld::createIslands() { mIslands.reserveMemory(); // Create a stack for the bodies to visit during the Depth First Search - Stack bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame()); + Stack bodyEntitiesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame()); // List of static bodies added to the current island (used to reset the isAlreadyInIsland variable of static bodies) - List staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator()); + List staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator()); uint nbTotalManifolds = 0; @@ -777,83 +773,75 @@ void PhysicsWorld::createIslands() { if (mRigidBodyComponents.mBodyTypes[b] == BodyType::STATIC) continue; // Reset the stack of bodies to visit - bodyEntityIndicesToVisit.clear(); + bodyEntitiesToVisit.clear(); // Add the body into the stack of bodies to visit mRigidBodyComponents.mIsAlreadyInIsland[b] = true; - bodyEntityIndicesToVisit.push(b); + bodyEntitiesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]); // Create the new island uint32 islandIndex = mIslands.addIsland(nbTotalManifolds); // While there are still some bodies to visit in the stack - while (bodyEntityIndicesToVisit.size() > 0) { - - // Get the next body to visit from the stack - const uint32 bodyToVisitIndex = bodyEntityIndicesToVisit.pop(); + while (bodyEntitiesToVisit.size() > 0) { // Get the body entity - const Entity bodyToVisitEntity = mRigidBodyComponents.mBodiesEntities[bodyToVisitIndex]; + const Entity bodyToVisitEntity = bodyEntitiesToVisit.pop(); // Add the body into the island mIslands.addBodyToIsland(bodyToVisitEntity); - RigidBody* rigidBodyToVisit = mRigidBodyComponents.mRigidBodies[bodyToVisitIndex]; + RigidBody* rigidBodyToVisit = mRigidBodyComponents.getRigidBody(bodyToVisitEntity); // Awake the body if it is sleeping (note that this called might change the body index in the mRigidBodyComponents array) rigidBodyToVisit->setIsSleeping(false); - // If the current body is static, we do not want to perform the DFS search across that body - if (rigidBodyToVisit->getType() == BodyType::STATIC) { + // Compute the body index in the array (Note that it could have changed because of the previous call to rigidBodyToVisit->setIsSleeping(false)) + const uint32 bodyToVisitIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity); - // Get the new body index in the mRigidBodyComponents (this index might have changed due to the call to rigidBodyToVisite->setIsSleeping(false)) - const uint32 newBodyIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity); + // If the currenbodyEntityIndicesToVisitt body is static, we do not want to perform the DFS search across that body + if (mRigidBodyComponents.mBodyTypes[bodyToVisitIndex] == BodyType::STATIC) { - staticBodiesAddedToIsland.add(newBodyIndex); + staticBodiesAddedToIsland.add(bodyToVisitEntity); // Go to the next body continue; } // If the body is involved in contacts with other bodies - auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity); - if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) { + // For each contact pair in which the current body is involded + for (uint p=0; p < mRigidBodyComponents.mContactPairs[bodyToVisitIndex].size(); p++) { - // For each contact pair in which the current body is involded - List& contactPairs = itBodyContactPairs->second; - for (uint p=0; p < contactPairs.size(); p++) { + ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[mRigidBodyComponents.mContactPairs[bodyToVisitIndex][p]]; - ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]]; + // Check if the current contact pair has already been added into an island + if (pair.isAlreadyInIsland) continue; - // Check if the current contact pair has already been added into an island - if (pair.isAlreadyInIsland) continue; + const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; - // If the colliding body is a RigidBody (and not a CollisionBody) and is not a trigger - if (mRigidBodyComponents.hasComponent(pair.body1Entity) && mRigidBodyComponents.hasComponent(pair.body2Entity) - && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) { + // If the colliding body is a RigidBody (and not a CollisionBody) and is not a trigger + uint32 otherBodyIndex; + if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex) + && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) { - assert(pair.potentialContactManifoldsIndices.size() > 0); - nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); + assert(pair.potentialContactManifoldsIndices.size() > 0); + nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); - // Add the contact manifold into the island - mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); - pair.isAlreadyInIsland = true; + // Add the contact manifold into the island + mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); + pair.isAlreadyInIsland = true; - const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; - const uint32 otherBodyIndex = mRigidBodyComponents.getEntityIndex(otherBodyEntity); + // Check if the other body has already been added to the island + if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; - // Check if the other body has already been added to the island - if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; + // Insert the other body into the stack of bodies to visit + bodyEntitiesToVisit.push(otherBodyEntity); + mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; + } + else { - // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyIndex); - mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; - } - else { - - // Add the contact pair index in the list of contact pairs that won't be part of islands - pair.isAlreadyInIsland = true; - } + // Add the contact pair index in the list of contact pairs that won't be part of islands + pair.isAlreadyInIsland = true; } } @@ -877,7 +865,7 @@ void PhysicsWorld::createIslands() { if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyIndex); + bodyEntitiesToVisit.push(otherBodyEntity); mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; } } @@ -886,14 +874,17 @@ void PhysicsWorld::createIslands() { // can also be included in the other islands for (uint j=0; j < staticBodiesAddedToIsland.size(); j++) { - assert(mRigidBodyComponents.mBodyTypes[staticBodiesAddedToIsland[j]] == BodyType::STATIC); - mRigidBodyComponents.mIsAlreadyInIsland[staticBodiesAddedToIsland[j]] = false; + assert(mRigidBodyComponents.getBodyType(staticBodiesAddedToIsland[j]) == BodyType::STATIC); + mRigidBodyComponents.setIsAlreadyInIsland(staticBodiesAddedToIsland[j], false); } staticBodiesAddedToIsland.clear(); } - mCollisionDetection.mMapBodyToContactPairs.clear(true); + // Clear the associated contacts pairs of rigid bodies + for (uint b=0; b < mRigidBodyComponents.getNbEnabledComponents(); b++) { + mRigidBodyComponents.mContactPairs[b].clear(); + } } // Put bodies to sleep if needed. diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 9c15e7c7..492b4088 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -49,9 +49,10 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents, TransformComponents& transformComponents, - CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents), +CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents, TransformComponents& transformComponents, + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, + MemoryManager& memoryManager) + : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents), mRigidBodyComponents(rigidBodyComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mCollidersComponents, @@ -67,7 +68,7 @@ CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, Collider mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1), - mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) { + mCurrentContactPoints(&mContactPoints2) { #ifdef IS_RP3D_PROFILING_ENABLED @@ -481,8 +482,7 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar List& potentialContactPoints, Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, - List* contactPairs, - Map>& mapBodyToContactPairs) { + List* contactPairs) { assert(contactPairs->size() == 0); assert(mapPairIdToContactPairIndex->size() == 0); @@ -497,17 +497,17 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar // Process the potential contacts processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex, - potentialContactManifolds, contactPairs, mapBodyToContactPairs); + potentialContactManifolds, contactPairs); } // Compute the narrow-phase collision detection @@ -528,7 +528,7 @@ void CollisionDetectionSystem::computeNarrowPhase() { // Process all the potential contacts after narrow-phase collision processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex, - mPotentialContactManifolds, mCurrentContactPairs, mMapBodyToContactPairs); + mPotentialContactManifolds, mCurrentContactPairs); // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(mCurrentContactPairs, mPotentialContactManifolds, mPotentialContactPoints); @@ -662,18 +662,15 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn List lostContactPairs(allocator); // Not used during collision snapshots List contactManifolds(allocator); List contactPoints(allocator); - Map> mapBodyToContactPairs(allocator); // Process all the potential contacts after narrow-phase collision - processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, - &contactPairs, mapBodyToContactPairs); + processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, &contactPairs); // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints); // Create the actual contact manifolds and contact points - createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds, - potentialContactPoints); + createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds, potentialContactPoints); // Report the contacts to the user reportContacts(callback, &contactPairs, &contactManifolds, &contactPoints, lostContactPairs); @@ -728,6 +725,14 @@ void CollisionDetectionSystem::createContacts() { contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.contactPointsIndex = mCurrentContactPoints->size(); + // Add the associated contact pair to both bodies of the pair (used to create islands later) + if (mRigidBodyComponents.hasComponent(contactPair.body1Entity)) { + mRigidBodyComponents.addContacPair(contactPair.body1Entity, p); + } + if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) { + mRigidBodyComponents.addContacPair(contactPair.body2Entity, p); + } + // For each potential contact manifold of the pair for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) { @@ -985,8 +990,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na List& potentialContactPoints, Map* mapPairIdToContactPairIndex, List& potentialContactManifolds, - List* contactPairs, - Map>& mapBodyToContactPairs) { + List* contactPairs) { RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); @@ -1035,25 +1039,6 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na contactPairs->add(overlappingPairContact); pairContact = &((*contactPairs)[newContactPairIndex]); mapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); - - auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity); - if (itbodyContactPairs != mapBodyToContactPairs.end()) { - itbodyContactPairs->second.add(newContactPairIndex); - } - else { - List contactPairs(mMemoryManager.getPoolAllocator(), 1); - contactPairs.add(newContactPairIndex); - mapBodyToContactPairs.add(Pair>(body1Entity, contactPairs)); - } - itbodyContactPairs = mapBodyToContactPairs.find(body2Entity); - if (itbodyContactPairs != mapBodyToContactPairs.end()) { - itbodyContactPairs->second.add(newContactPairIndex); - } - else { - List contactPairs(mMemoryManager.getPoolAllocator(), 1); - contactPairs.add(newContactPairIndex); - mapBodyToContactPairs.add(Pair>(body2Entity, contactPairs)); - } } else { // If a ContactPair already exists for this overlapping pair, we use this one