From 98ac47cbad3e799c0b7c855e44bd5d6751779a7b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 7 Jul 2020 18:21:41 +0200 Subject: [PATCH] Optimization of the islands computation --- include/reactphysics3d/engine/Islands.h | 72 +++++++++++++++++++++---- src/engine/PhysicsWorld.cpp | 71 +++++++++++++----------- src/systems/ContactSolverSystem.cpp | 2 +- 3 files changed, 104 insertions(+), 41 deletions(-) diff --git a/include/reactphysics3d/engine/Islands.h b/include/reactphysics3d/engine/Islands.h index 2ffc26a5..59d09f7e 100644 --- a/include/reactphysics3d/engine/Islands.h +++ b/include/reactphysics3d/engine/Islands.h @@ -46,10 +46,14 @@ struct Islands { private: - // -------------------- Attributes -------------------- // + /// Number of islands in the previous frame + uint32 mNbIslandsPreviousFrame; - /// Reference to the memory allocator - MemoryAllocator& memoryAllocator; + /// Maximum number of bodies in a single island in the previous frame + uint32 mNbMaxBodiesInIslandPreviousFrame; + + /// Maximum number of bodies in a single island in the current frame + uint32 mNbMaxBodiesInIslandCurrentFrame; public: @@ -62,15 +66,22 @@ struct Islands { /// For each island, number of contact manifolds in the island List nbContactManifolds; - /// For each island, list of all the entities of the bodies in the island - List> bodyEntities; + /// List of all the entities of the bodies in the islands (stored sequentially) + List bodyEntities; + + /// For each island we store the starting index of the bodies of that island in the "bodyEntities" array + List startBodyEntitiesIndex; + + /// For each island, total number of bodies in the island + List nbBodiesInIsland; // -------------------- Methods -------------------- // /// Constructor Islands(MemoryAllocator& allocator) - :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), - bodyEntities(allocator) { + :mNbIslandsPreviousFrame(10), mNbMaxBodiesInIslandPreviousFrame(0), mNbMaxBodiesInIslandCurrentFrame(0), + contactManifoldsIndices(allocator), nbContactManifolds(allocator), + bodyEntities(allocator), startBodyEntitiesIndex(allocator), nbBodiesInIsland(allocator) { } @@ -78,7 +89,7 @@ struct Islands { ~Islands() = default; /// Assignment operator - Islands& operator=(const Islands& island) = default; + Islands& operator=(const Islands& island) = delete; /// Copy-constructor Islands(const Islands& island) = default; @@ -91,21 +102,62 @@ struct Islands { /// Add an island and return its index uint32 addIsland(uint32 contactManifoldStartIndex) { - uint32 islandIndex = contactManifoldsIndices.size(); + const uint32 islandIndex = contactManifoldsIndices.size(); contactManifoldsIndices.add(contactManifoldStartIndex); nbContactManifolds.add(0); - bodyEntities.add(List(memoryAllocator)); + startBodyEntitiesIndex.add(bodyEntities.size()); + nbBodiesInIsland.add(0); + + if (islandIndex > 0 && nbBodiesInIsland[islandIndex-1] > mNbMaxBodiesInIslandCurrentFrame) { + mNbMaxBodiesInIslandCurrentFrame = nbBodiesInIsland[islandIndex-1]; + } return islandIndex; } + void addBodyToIsland(Entity bodyEntity) { + + const uint32 islandIndex = contactManifoldsIndices.size(); + assert(islandIndex > 0); + + bodyEntities.add(bodyEntity); + nbBodiesInIsland[islandIndex - 1]++; + } + + /// Reserve memory for the current frame + void reserveMemory() { + + contactManifoldsIndices.reserve(mNbIslandsPreviousFrame); + nbContactManifolds.reserve(mNbIslandsPreviousFrame); + startBodyEntitiesIndex.reserve(mNbIslandsPreviousFrame); + nbBodiesInIsland.reserve(mNbIslandsPreviousFrame); + + bodyEntities.reserve(mNbMaxBodiesInIslandPreviousFrame); + } + /// Clear all the islands void clear() { + const uint32 nbIslands = nbContactManifolds.size(); + + if (nbIslands > 0 && nbBodiesInIsland[nbIslands-1] > mNbMaxBodiesInIslandCurrentFrame) { + mNbMaxBodiesInIslandCurrentFrame = nbBodiesInIsland[nbIslands-1]; + } + + mNbIslandsPreviousFrame = nbContactManifolds.size(); + mNbIslandsPreviousFrame = mNbMaxBodiesInIslandCurrentFrame; + mNbMaxBodiesInIslandCurrentFrame = 0; + contactManifoldsIndices.clear(true); nbContactManifolds.clear(true); bodyEntities.clear(true); + startBodyEntitiesIndex.clear(true); + nbBodiesInIsland.clear(true); + } + + uint32 getNbMaxBodiesInIslandPreviousFrame() const { + return mNbMaxBodiesInIslandPreviousFrame; } }; diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index b79a6969..02465fbd 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -756,11 +756,14 @@ void PhysicsWorld::createIslands() { mJointsComponents.mIsAlreadyInIsland[i] = false; } + // Reserve memory for the islands + mIslands.reserveMemory(); + // Create a stack for the bodies to visit during the Depth First Search - Stack bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator()); + Stack bodyEntityIndicesToVisit(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; @@ -778,7 +781,7 @@ void PhysicsWorld::createIslands() { // Add the body into the stack of bodies to visit mRigidBodyComponents.mIsAlreadyInIsland[b] = true; - bodyEntityIndicesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]); + bodyEntityIndicesToVisit.push(b); // Create the new island uint32 islandIndex = mIslands.addIsland(nbTotalManifolds); @@ -787,20 +790,26 @@ void PhysicsWorld::createIslands() { while (bodyEntityIndicesToVisit.size() > 0) { // Get the next body to visit from the stack - const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop(); + const uint32 bodyToVisitIndex = bodyEntityIndicesToVisit.pop(); + + // Get the body entity + const Entity bodyToVisitEntity = mRigidBodyComponents.mBodiesEntities[bodyToVisitIndex]; // Add the body into the island - mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity); + mIslands.addBodyToIsland(bodyToVisitEntity); - RigidBody* rigidBodyToVisit = static_cast(mCollisionBodyComponents.getBody(bodyToVisitEntity)); + RigidBody* rigidBodyToVisit = mRigidBodyComponents.mRigidBodies[bodyToVisitIndex]; - // Awake the body if it is sleeping + // 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) { - staticBodiesAddedToIsland.add(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); + + staticBodiesAddedToIsland.add(newBodyIndex); // Go to the next body continue; @@ -831,13 +840,14 @@ void PhysicsWorld::createIslands() { 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.getIsAlreadyInIsland(otherBodyEntity)) continue; + if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyEntity); - mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); + bodyEntityIndicesToVisit.push(otherBodyIndex); + mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; } else { @@ -861,12 +871,14 @@ void PhysicsWorld::createIslands() { const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; + const uint32 otherBodyIndex = mRigidBodyComponents.getEntityIndex(otherBodyEntity); + // Check if the other body has already been added to the island - if (mRigidBodyComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; + if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue; // Insert the other body into the stack of bodies to visit - bodyEntityIndicesToVisit.push(otherBodyEntity); - mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true); + bodyEntityIndicesToVisit.push(otherBodyIndex); + mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true; } } @@ -874,8 +886,8 @@ void PhysicsWorld::createIslands() { // can also be included in the other islands for (uint j=0; j < staticBodiesAddedToIsland.size(); j++) { - assert(mRigidBodyComponents.getBodyType(staticBodiesAddedToIsland[j]) == BodyType::STATIC); - mRigidBodyComponents.setIsAlreadyInIsland(staticBodiesAddedToIsland[j], false); + assert(mRigidBodyComponents.mBodyTypes[staticBodiesAddedToIsland[j]] == BodyType::STATIC); + mRigidBodyComponents.mIsAlreadyInIsland[staticBodiesAddedToIsland[j]] = false; } staticBodiesAddedToIsland.clear(); @@ -900,30 +912,29 @@ void PhysicsWorld::updateSleepingBodies(decimal timeStep) { decimal minSleepTime = DECIMAL_LARGEST; // For each body of the island - for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { + for (uint b=0; b < mIslands.nbBodiesInIsland[i]; b++) { - const Entity bodyEntity = mIslands.bodyEntities[i][b]; + const Entity bodyEntity = mIslands.bodyEntities[mIslands.startBodyEntitiesIndex[i] + b]; + const uint32 bodyIndex = mRigidBodyComponents.getEntityIndex(bodyEntity); // Skip static bodies - if (mRigidBodyComponents.getBodyType(bodyEntity) == BodyType::STATIC) continue; + if (mRigidBodyComponents.mBodyTypes[bodyIndex] == BodyType::STATIC) continue; // If the body is velocity is large enough to stay awake - if (mRigidBodyComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare || - mRigidBodyComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare || - !mRigidBodyComponents.getIsAllowedToSleep(bodyEntity)) { + if (mRigidBodyComponents.mLinearVelocities[bodyIndex].lengthSquare() > sleepLinearVelocitySquare || + mRigidBodyComponents.mAngularVelocities[bodyIndex].lengthSquare() > sleepAngularVelocitySquare || + !mRigidBodyComponents.mIsAllowedToSleep[bodyIndex]) { // Reset the sleep time of the body - mRigidBodyComponents.setSleepTime(bodyEntity, decimal(0.0)); + mRigidBodyComponents.mSleepTimes[bodyIndex] = decimal(0.0); minSleepTime = decimal(0.0); } else { // If the body velocity is below the sleeping velocity threshold // Increase the sleep time - decimal sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); - mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + timeStep); - sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); - if (sleepTime < minSleepTime) { - minSleepTime = sleepTime; + mRigidBodyComponents.mSleepTimes[bodyIndex] += timeStep; + if (mRigidBodyComponents.mSleepTimes[bodyIndex] < minSleepTime) { + minSleepTime = mRigidBodyComponents.mSleepTimes[bodyIndex]; } } } @@ -934,9 +945,9 @@ void PhysicsWorld::updateSleepingBodies(decimal timeStep) { if (minSleepTime >= mTimeBeforeSleep) { // Put all the bodies of the island to sleep - for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { + for (uint b=0; b < mIslands.nbBodiesInIsland[i]; b++) { - const Entity bodyEntity = mIslands.bodyEntities[i][b]; + const Entity bodyEntity = mIslands.bodyEntities[mIslands.startBodyEntitiesIndex[i] + b]; RigidBody* body = mRigidBodyComponents.getRigidBody(bodyEntity); body->setIsSleeping(true); } diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index f384a78a..1f6b6f74 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -113,7 +113,7 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler); - assert(mIslands.bodyEntities[islandIndex].size() > 0); + assert(mIslands.nbBodiesInIsland[islandIndex] > 0); assert(mIslands.nbContactManifolds[islandIndex] > 0); // For each contact manifold of the island