From 1c91ef7d48922f1402a669820e190bed58221036 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 27 Apr 2019 15:02:21 +0200 Subject: [PATCH] Refactor islands creation --- CMakeLists.txt | 1 + src/collision/CollisionDetection.cpp | 86 +++++++---- src/collision/CollisionDetection.h | 14 +- src/collision/ContactPair.h | 17 ++- src/components/DynamicsComponents.cpp | 9 +- src/components/DynamicsComponents.h | 26 ++++ src/engine/DynamicsWorld.cpp | 208 +++++++++++++++++++++++++- src/engine/DynamicsWorld.h | 7 + 8 files changed, 330 insertions(+), 38 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26fcf030..4dcdce53 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,7 @@ SET (REACTPHYSICS3D_HEADERS "src/engine/DynamicsWorld.h" "src/engine/EventListener.h" "src/engine/Island.h" + "src/engine/Islands.h" "src/engine/Material.h" "src/engine/OverlappingPair.h" "src/engine/Timer.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 66b93a11..7c4e8964 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -42,6 +42,7 @@ #include "utils/Profiler.h" #include "engine/EventListener.h" #include "collision/RaycastInfo.h" +#include "engine/Islands.h" #include #include @@ -53,22 +54,23 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents), + : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mOverlappingPairs(mMemoryManager.getPoolAllocator()), - mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents), + mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, dynamicsComponents), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), - mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getPoolAllocator()), + mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts - mPotentialContactManifolds(mMemoryManager.getPoolAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), + mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2), mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()), mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2), + mContactPairsIndicesOrderingForContacts(memoryManager.getSingleFrameAllocator()), mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2), mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1), - mCurrentContactPoints(&mContactPoints2) { + mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) { #ifdef IS_PROFILING_ACTIVE @@ -420,22 +422,18 @@ void CollisionDetection::computeNarrowPhase() { // Reduce the number of contact points in the manifolds reducePotentialContactManifolds(mOverlappingPairs); - // Create the actual contact manifolds and contacts (from potential contacts) - createContacts(); - - // Initialize the current contacts with the contacts from the previous frame (for warmstarting) - initContactsWithPreviousOnes(); - // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); // Report contacts to the user reportAllContacts(); - // Clear the list of narrow-phase infos - mNarrowPhaseInput.clear(); + assert(mCurrentContactManifolds->size() == 0); + assert(mCurrentContactPoints->size() == 0); - // TODO : Do not forget to clear all the contact pair, contact manifolds and contact points lists + mContactPairsIndicesOrderingForContacts.reserve(mCurrentContactPairs->size()); + + mNarrowPhaseInput.clear(); } // Swap the previous and current contacts lists @@ -467,24 +465,26 @@ void CollisionDetection::swapPreviousAndCurrentContacts() { } } -// Create the actual contact manifolds and contacts (from potential contacts) +// Create the actual contact manifolds and contacts points +/// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of +/// same islands packed together linearly and contact pairs that are not part of islands at the end. +/// This is used when we create contact manifolds and contact points so that there are also packed +/// together linearly if they are part of the same island. void CollisionDetection::createContacts() { RP3D_PROFILE("CollisionDetection::createContacts()", mProfiler); - assert(mCurrentContactManifolds->size() == 0); - assert(mCurrentContactPoints->size() == 0); + assert(mCurrentContactPairs->size() == mContactPairsIndicesOrderingForContacts.size()); + + mCurrentContactManifolds->reserve(mCurrentContactPairs->size()); + mCurrentContactPoints->reserve(mCurrentContactManifolds->size()); // For each contact pair - for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) { - - const uint contactPairIndex = it->second; - assert(contactPairIndex < mCurrentContactPairs->size()); - ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex]; + for (uint p=0; p < mContactPairsIndicesOrderingForContacts.size(); p++) { + ContactPair& contactPair = (*mCurrentContactPairs)[mContactPairsIndicesOrderingForContacts[p]]; assert(contactPair.potentialContactManifoldsIndices.size() > 0); - // Start index and numnber of contact manifolds for this contact pair contactPair.contactManifoldsIndex = mCurrentContactManifolds->size(); contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size(); contactPair.contactPointsIndex = mCurrentContactPoints->size(); @@ -496,7 +496,8 @@ void CollisionDetection::createContacts() { // Start index and number of contact points for this manifold const uint contactPointsIndex = mCurrentContactPoints->size(); - const int8 nbContactPoints = potentialManifold.potentialContactPointsIndices.size(); + const int8 nbContactPoints = static_cast(potentialManifold.potentialContactPointsIndices.size()); + contactPair.nbToTalContactPoints += nbContactPoints; // We create a new contact manifold ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig); @@ -504,9 +505,6 @@ void CollisionDetection::createContacts() { // Add the contact manifold mCurrentContactManifolds->add(contactManifold); - // Increase the number of total contact point of the contact pair - contactPair.nbToTalContactPoints += potentialManifold.potentialContactPointsIndices.size(); - assert(potentialManifold.potentialContactPointsIndices.size() > 0); // For each contact point of the manifold @@ -523,9 +521,13 @@ void CollisionDetection::createContacts() { } } + // Initialize the current contacts with the contacts from the previous frame (for warmstarting) + initContactsWithPreviousOnes(); + // Reset the potential contacts - mPotentialContactPoints.clear(); - mPotentialContactManifolds.clear(); + mPotentialContactPoints.clear(true); + mPotentialContactManifolds.clear(true); + mContactPairsIndicesOrderingForContacts.clear(true); } // Initialize the current contacts with the contacts from the previous frame (for warmstarting) @@ -800,6 +802,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh const uint contactPointIndex = static_cast(mPotentialContactPoints.size()); // TODO : We should probably use single frame allocator here for mPotentialContactPoints + // If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame mPotentialContactPoints.add(contactPoint); bool similarManifoldFound = false; @@ -846,6 +849,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh // Create a new contact manifold for the overlapping pair // TODO : We should probably use single frame allocator here + // If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator()); // Add the contact point to the manifold @@ -854,12 +858,34 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh // If the overlapping pair contact does not exists yet if (pairContact == nullptr) { + Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity(); + Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity(); + // TODO : We should probably use a single frame allocator here - ContactPair overlappingPairContact(pairId, mMemoryManager.getPoolAllocator()); const uint newContactPairIndex = mCurrentContactPairs->size(); + ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, newContactPairIndex, mMemoryManager.getPoolAllocator()); mCurrentContactPairs->add(overlappingPairContact); pairContact = &((*mCurrentContactPairs)[newContactPairIndex]); mCurrentMapPairIdToContactPairIndex->add(Pair(pairId, newContactPairIndex)); + + auto itbodyContactPairs = mMapBodyToContactPairs.find(body1Entity); + if (itbodyContactPairs != mMapBodyToContactPairs.end()) { + itbodyContactPairs->second.add(newContactPairIndex); + } + else { + List contactPairs(mMemoryManager.getSingleFrameAllocator(), 1); + contactPairs.add(newContactPairIndex); + mMapBodyToContactPairs.add(Pair>(body1Entity, contactPairs)); + } + itbodyContactPairs = mMapBodyToContactPairs.find(body2Entity); + if (itbodyContactPairs != mMapBodyToContactPairs.end()) { + itbodyContactPairs->second.add(newContactPairIndex); + } + else { + List contactPairs(mMemoryManager.getSingleFrameAllocator(), 1); + contactPairs.add(newContactPairIndex); + mMapBodyToContactPairs.add(Pair>(body2Entity, contactPairs)); + } } assert(pairContact != nullptr); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index ed2d4ff6..8f950e67 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -82,9 +82,6 @@ class CollisionDetection { /// Reference the proxy-shape components ProxyShapeComponents& mProxyShapesComponents; - /// Reference the transform components - TransformComponents& mTransformComponents; - /// Collision Detection Dispatch configuration CollisionDispatch mCollisionDispatch; @@ -138,6 +135,12 @@ class CollisionDetection { /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2) Map* mCurrentMapPairIdToContactPairIndex; + /// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of + /// same islands packed together linearly and contact pairs that are not part of islands at the end. + /// This is used when we create contact manifolds and contact points so that there are also packed + /// together linearly if they are part of the same island. + List mContactPairsIndicesOrderingForContacts; + /// First list with the contact manifolds List mContactManifolds1; @@ -162,6 +165,9 @@ class CollisionDetection { /// 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_PROFILING_ACTIVE /// Pointer to the profiler @@ -214,7 +220,7 @@ class CollisionDetection { /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts void reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs); - /// Create the actual contact manifolds and contacts (from potential contacts) + /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair void createContacts(); /// Initialize the current contacts with the contacts from the previous frame (for warmstarting) diff --git a/src/collision/ContactPair.h b/src/collision/ContactPair.h index abcc14e1..816302d7 100644 --- a/src/collision/ContactPair.h +++ b/src/collision/ContactPair.h @@ -50,6 +50,18 @@ struct ContactPair { /// Indices of the potential contact manifolds List potentialContactManifoldsIndices; + /// Entity of the first body of the manifold + Entity body1Entity; + + /// Entity of the second body of the manifold + Entity body2Entity; + + /// True if the manifold is already in an island + bool isAlreadyInIsland; + + /// Index of the contact pair in the array of pairs + uint contactPairIndex; + /// Index of the first contact manifold in the array uint contactManifoldsIndex; @@ -65,8 +77,9 @@ struct ContactPair { // -------------------- Methods -------------------- // /// Constructor - ContactPair(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator) - : pairId(pairId), potentialContactManifoldsIndices(allocator), contactManifoldsIndex(0), nbContactManifolds(0), + ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, uint contactPairIndex, MemoryAllocator& allocator) + : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity), + isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0), contactPointsIndex(0), nbToTalContactPoints(0) { } diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index e23b3cfb..838584a4 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // Constructor DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) { + :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -57,6 +57,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { Entity* newBodies = static_cast(newBuffer); Vector3* newLinearVelocities = reinterpret_cast(newBodies + nbComponentsToAllocate); Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -65,6 +66,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity)); memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -74,6 +76,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mBodies = newBodies; mLinearVelocities = newLinearVelocities; mAngularVelocities = newAngularVelocities; + mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -87,6 +90,7 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mBodies + index) Entity(bodyEntity); new (mLinearVelocities + index) Vector3(component.linearVelocity); new (mAngularVelocities + index) Vector3(component.angularVelocity); + mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -107,6 +111,7 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mBodies + destIndex) Entity(mBodies[srcIndex]); new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]); new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]); + mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -129,6 +134,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { Entity entity1(mBodies[index1]); Vector3 linearVelocity1(mLinearVelocities[index1]); Vector3 angularVelocity1(mAngularVelocities[index1]); + bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; // Destroy component 1 destroyComponent(index1); @@ -139,6 +145,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { new (mBodies + index2) Entity(entity1); new (mLinearVelocities + index2) Vector3(linearVelocity1); new (mAngularVelocities + index2) Vector3(angularVelocity1); + mIsAlreadyInIsland[index2] = isAlreadyInIsland1; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index 7c81e61b..696208bf 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -59,6 +59,9 @@ class DynamicsComponents : public Components { /// Array with the angular velocity of each component Vector3* mAngularVelocities; + /// Array with the boolean value to know if the body has already been added into an island + bool* mIsAlreadyInIsland; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -105,15 +108,22 @@ class DynamicsComponents : public Components { /// Return the angular velocity of an entity Vector3& getAngularVelocity(Entity bodyEntity) const; + /// Return true if the entity is already in an island + bool getIsAlreadyInIsland(Entity bodyEntity) const; + /// Set the linear velocity of an entity void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity); /// Set the angular velocity of an entity void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity); + /// Set the value to know if the entity is already in an island + bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; + friend class DynamicsWorld; }; // Return the linear velocity of an entity @@ -148,6 +158,22 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity; } +// Return true if the entity is already in an island +inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]]; +} + +/// Set the value to know if the entity is already in an island +inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; +} + } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 15524353..70488265 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -32,7 +32,9 @@ #include "utils/Profiler.h" #include "engine/EventListener.h" #include "engine/Island.h" +#include "engine/Islands.h" #include "collision/ContactManifold.h" +#include "containers/Stack.h" // Namespaces using namespace reactphysics3d; @@ -60,7 +62,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), - mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { + mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0), + mIslands2(mMemoryManager.getSingleFrameAllocator()) { #ifdef IS_PROFILING_ACTIVE @@ -128,6 +131,32 @@ void DynamicsWorld::update(decimal timeStep) { // Compute the islands (separate groups of bodies with constraints between each others) computeIslands(); + // Create the islands + createIslands(); + + // TODO : DELETE THIS + /* + std::cout << "--------------------- FRAME ------------------------ " << std::endl; + std::cout << " ---- OLD ISLANDS -----" << std::endl; + for (uint i=0; i < mNbIslands; i++) { + std::cout << "Island " << i << std::endl; + for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + std::cout << "Body Id : " << mIslands[i]->getBodies()[b]->getId() << std::endl; + } + } + + std::cout << " ---- NEW ISLANDS -----" << std::endl; + for (uint i=0; i < mIslands2.getNbIslands(); i++) { + std::cout << "Island " << i << std::endl; + for (uint b=0; b < mIslands2.bodyEntities[i].size(); b++) { + std::cout << "Body Id : " << mBodyComponents.getBody(mIslands2.bodyEntities[i][b])->getId() << std::endl; + } + } + */ + + // Create the actual narrow-phase contacts + mCollisionDetection.createContacts(); + // Integrate the velocities integrateRigidBodiesVelocities(); @@ -151,6 +180,9 @@ void DynamicsWorld::update(decimal timeStep) { // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); + // Reset the islands + mIslands2.clear(); + // Reset the single frame memory allocator mMemoryManager.resetFrameAllocator(); } @@ -672,6 +704,7 @@ uint DynamicsWorld::computeNextAvailableJointId() { return jointId; } +// TODO : DELETE THIS // Compute the islands of awake bodies. /// An island is an isolated group of rigid bodies that have constraints (joints or contacts) /// between each other. This method computes the islands at each time step as follows: For each @@ -828,6 +861,179 @@ void DynamicsWorld::computeIslands() { } } +// Compute the islands using potential contacts and joints +/// We compute the islands before creating the actual contacts here because we want all +/// the contact manifolds and contact points of the same island +/// to be packed together into linear arrays of manifolds and contacts for better caching. +/// An island is an isolated group of rigid bodies that have constraints (joints or contacts) +/// between each other. This method computes the islands at each time step as follows: For each +/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body +/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to +/// find all the bodies that are connected with it (the bodies that share joints or contacts with +/// it). Then, we create an island with this group of connected bodies. +void DynamicsWorld::createIslands() { + + // TODO : Check if we handle kinematic bodies correctly in islands creation + + // list of contact pairs involving a non-rigid body + List nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator()); + + RP3D_PROFILE("DynamicsWorld::createIslandsAndContacts()", mProfiler); + + // Reset all the isAlreadyInIsland variables of bodies and joints + for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) { + mDynamicsComponents.mIsAlreadyInIsland[b] = false; + } + for (List::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { + (*it)->mIsAlreadyInIsland = false; + } + + // Create a stack for the bodies to visit during the Depth First Search + Stack bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator()); + + uint nbTotalManifolds = 0; + + // For each dynamic component + // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should + // not use a dynamic component for a static body. + for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) { + + // If the body has already been added to an island, we go to the next body + if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue; + + // If the body is static, we go to the next body + // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) + CollisionBody* body = static_cast(mBodyComponents.getBody(mDynamicsComponents.mBodies[b])); + if (body->getType() == BodyType::STATIC) continue; + + // Reset the stack of bodies to visit + bodyEntityIndicesToVisit.clear(); + + // Add the body into the stack of bodies to visit + mDynamicsComponents.mIsAlreadyInIsland[b] = true; + bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]); + + // Create the new island + uint32 islandIndex = mIslands2.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 Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop(); + + // Awake the body if it is sleeping + notifyBodyDisabled(bodyToVisitEntity, false); + + // Add the body into the island + mIslands2.bodyEntities[islandIndex].add(bodyToVisitEntity); + + // If the current body is static, we do not want to perform the DFS + // search across that body + // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) + RigidBody* rigidBodyToVisit = static_cast(mBodyComponents.getBody(bodyToVisitEntity)); + if (rigidBodyToVisit->getType() == BodyType::STATIC) 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 + List& contactPairs = itBodyContactPairs->second; + for (uint p=0; p < contactPairs.size(); p++) { + + ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]]; + assert(pair.potentialContactManifoldsIndices.size() > 0); + + // Check if the current contact pair has already been added into an island + if (pair.isAlreadyInIsland) continue; + + // Get the other body of the contact manifold + // TODO : Maybe avoid those casts here + RigidBody* body1 = dynamic_cast(mBodyComponents.getBody(pair.body1Entity)); + RigidBody* body2 = dynamic_cast(mBodyComponents.getBody(pair.body2Entity)); + + // If the colliding body is a RigidBody (and not a CollisionBody instead) + if (body1 != nullptr && body2 != nullptr) { + + nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); + + // Add the pair into the list of pair to process to create contacts + mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex); + + // Add the contact manifold into the island + mIslands2.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); + pair.isAlreadyInIsland = true; + + const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; + + // Check if the other body has already been added to the island + if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + + // Insert the other body into the stack of bodies to visit + bodyEntityIndicesToVisit.push(otherBodyEntity); + mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true); + } + else { + + // Add the contact pair index in the list of contact pairs that won't be part of islands + nonRigidBodiesContactPairs.add(pair.contactPairIndex); + } + } + } + + // For each joint in which the current body is involved + JointListElement* jointElement; + for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr; + jointElement = jointElement->next) { + + Joint* joint = jointElement->joint; + + // Check if the current joint has already been added into an island + if (joint->isAlreadyInIsland()) continue; + + // Add the joint into the island + mIslands2.joints.add(joint); + joint->mIsAlreadyInIsland = true; + + const Entity body1Entity = joint->getBody1()->getEntity(); + const Entity body2Entity = joint->getBody2()->getEntity(); + const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; + + // TODO : Maybe avoid those casts here + // Get the other body of the contact manifold + RigidBody* body1 = static_cast(joint->getBody1()); + RigidBody* body2 = static_cast(joint->getBody2()); + RigidBody* otherBody = (body1->getId() == rigidBodyToVisit->getId()) ? body2 : body1; + + // Check if the other body has already been added to the island + if (otherBody->mIsAlreadyInIsland) continue; + + // Insert the other body into the stack of bodies to visit + bodyEntityIndicesToVisit.push(otherBodyEntity); + otherBody->mIsAlreadyInIsland = true; + } + } + + // Reset the isAlreadyIsland variable of the static bodies so that they + // can also be included in the other islands + for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) { + + // If the body is static, we go to the next body + // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) + CollisionBody* body = static_cast(mBodyComponents.getBody(mDynamicsComponents.mBodies[b])); + if (body->getType() == BodyType::STATIC) { + mDynamicsComponents.mIsAlreadyInIsland[b] = false; + } + } + } + + // Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations + mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs); + + mCollisionDetection.mMapBodyToContactPairs.clear(true); +} + // Put bodies to sleep if needed. /// For each island, if all the bodies have been almost still for a long enough period of /// time, we put all the bodies of the island to sleep. diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index f17aaefd..5256063a 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -33,6 +33,7 @@ #include "utils/Logger.h" #include "engine/ContactSolver.h" #include "components/DynamicsComponents.h" +#include "engine/Islands.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -126,6 +127,9 @@ class DynamicsWorld : public CollisionWorld { /// Current joint id uint mCurrentJointId; + /// All the islands of bodies of the current frame + Islands mIslands2; + // -------------------- Methods -------------------- // /// Integrate the positions and orientations of rigid bodies. @@ -149,6 +153,9 @@ class DynamicsWorld : public CollisionWorld { /// Compute the islands of awake bodies. void computeIslands(); + /// Compute the islands using potential contacts and joints and create the actual contacts. + void createIslands(); + /// Update the postion/orientation of the bodies void updateBodiesState();