Optimization of the islands computation

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
+        /// Maximum number of bodies in a single island in the previous frame
-        MemoryAllocator& memoryAllocator;
+        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<uint> nbContactManifolds;
 
-        /// For each island, list of all the entities of the bodies in the island
+        /// List of all the entities of the bodies in the islands (stored sequentially)
-        List<List<Entity>> bodyEntities;
+        List<Entity> bodyEntities;
+
+        /// For each island we store the starting index of the bodies of that island in the "bodyEntities" array
+        List<uint32> startBodyEntitiesIndex;
+
+        /// For each island, total number of bodies in the island
+        List<uint32> nbBodiesInIsland;
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
         Islands(MemoryAllocator& allocator)
-            :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator),
+            :mNbIslandsPreviousFrame(10), mNbMaxBodiesInIslandPreviousFrame(0), mNbMaxBodiesInIslandCurrentFrame(0),
-             bodyEntities(allocator) {
+             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<Entity>(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;
         }
 };
 

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<Entity> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator());
+    Stack<uint32> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame());
 
     // List of static bodies added to the current island (used to reset the isAlreadyInIsland variable of static bodies)
-    List<Entity> staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator());
+    List<uint32> 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<RigidBody*>(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);
+                        bodyEntityIndicesToVisit.push(otherBodyIndex);
-                        mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true);
+                        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);
+                bodyEntityIndicesToVisit.push(otherBodyIndex);
-                mRigidBodyComponents.setIsAlreadyInIsland(otherBodyEntity, true);
+                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);
+            assert(mRigidBodyComponents.mBodyTypes[staticBodiesAddedToIsland[j]] == BodyType::STATIC);
-            mRigidBodyComponents.setIsAlreadyInIsland(staticBodiesAddedToIsland[j], false);
+            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 ||
+            if (mRigidBodyComponents.mLinearVelocities[bodyIndex].lengthSquare() > sleepLinearVelocitySquare ||
-                mRigidBodyComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare ||
+                mRigidBodyComponents.mAngularVelocities[bodyIndex].lengthSquare() > sleepAngularVelocitySquare ||
-                !mRigidBodyComponents.getIsAllowedToSleep(bodyEntity)) {
+                !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.mSleepTimes[bodyIndex] += timeStep;
-                mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + timeStep);
+                if (mRigidBodyComponents.mSleepTimes[bodyIndex] < minSleepTime) {
-                sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity);
+                    minSleepTime = mRigidBodyComponents.mSleepTimes[bodyIndex];
-                if (sleepTime < minSleepTime) {
-                    minSleepTime = sleepTime;
                 }
             }
         }
@@ -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);
             }

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