Handle sleeping bodies in TransformComponents

src/body/CollisionBody.cpp

@@ -431,6 +431,15 @@ void CollisionBody::setTransform(const Transform& transform) {
 }
 
 
+// Set the variable to know whether or not the body is sleeping
+void CollisionBody::setIsSleeping(bool isSleeping) {
+
+    Body::setIsSleeping(isSleeping);
+
+    // Notify all the components
+    mWorld.notifyBodySleeping(mEntity, isSleeping);
+}
+
 // Return the world-space coordinates of a point given the local-space coordinates of the body
 /**
 * @param localPoint A point in the local-space coordinates of the body

src/body/CollisionBody.h

@@ -141,6 +141,9 @@ class CollisionBody : public Body {
         /// Set the current position and orientation
         virtual void setTransform(const Transform& transform);
 
+        /// Set the variable to know whether or not the body is sleeping
+        virtual void setIsSleeping(bool isSleeping) override;
+
         /// Add a collision shape to the body.
         virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
                                               const Transform& transform);

src/body/RigidBody.h

@@ -358,7 +358,7 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
         mExternalTorque.setToZero();
     }
 
-    Body::setIsSleeping(isSleeping);
+    CollisionBody::setIsSleeping(isSleeping);
 }
 
 // Apply an external force to the body at its center of mass.

src/components/TransformComponents.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
 // Constructor
 TransformComponents::TransformComponents(MemoryAllocator& allocator)
                     :mMemoryAllocator(allocator), mNbComponents(0), mNbAllocatedComponents(0),
-                     mBuffer(nullptr), mMapEntityToComponentIndex(allocator) {
+                     mSleepingStartIndex(0), mBuffer(nullptr), mMapEntityToComponentIndex(allocator) {
 
     // Allocate memory for the components data
     allocate(INIT_ALLOCATED_COMPONENTS);
@@ -94,21 +94,53 @@ void TransformComponents::allocate(uint32 nbComponentsToAllocate) {
 }
 
 // Add a component
-void TransformComponents::addComponent(Entity entity, const TransformComponent& component) {
+void TransformComponents::addComponent(Entity entity, bool isSleeping, const TransformComponent& component) {
 
     // If we need to allocate more components
     if (mNbComponents == mNbAllocatedComponents) {
         allocate(mNbAllocatedComponents * 2);
     }
 
-    // Map the entity with the new component lookup index
-    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, mNbComponents));
+    // If the component to add is part of a sleeping entity or there are no sleeping entity
+    if (isSleeping || mSleepingStartIndex == mNbComponents) {
 
-    // Insert the new component data
-    new (mEntities + mNbComponents) Entity(entity);
-    new (mTransforms + mNbComponents) Transform(component.transform);
+        // Add the component at the end of the array
+        uint32 index = mNbComponents;
+
+        // Map the entity with the new component lookup index
+        mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, index));
+
+        if (isSleeping) {
+            mSleepingStartIndex = index;
+        }
+
+        // Insert the new component data
+        new (mEntities + index) Entity(entity);
+        new (mTransforms + index) Transform(component.transform);
+    }
+    // If the component to add is not part of a sleeping entity and there are others sleeping components
+    else {
+
+        // Copy the first sleeping component to the end of the array
+        new (mEntities + mNbComponents) Entity(mEntities[mSleepingStartIndex]);
+        new (mTransforms + mNbComponents) Transform(mTransforms[mSleepingStartIndex]);
+
+        mMapEntityToComponentIndex[mEntities[mSleepingStartIndex]] = mNbComponents;
+
+        // Copy the new component to the previous location of the fist sleeping component
+        mEntities[mSleepingStartIndex] = entity;
+        mTransforms[mSleepingStartIndex] = component.transform;
+
+        // Map the entity with the new component lookup index
+        mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, mSleepingStartIndex));
+
+        mSleepingStartIndex++;
+    }
 
     mNbComponents++;
+
+    assert(mSleepingStartIndex <= mNbComponents);
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
 }
 
 // Perform garbage collection to remove unused components
@@ -146,6 +178,8 @@ void TransformComponents::garbageCollection(const EntityManager& entityManager)
         // Destroy the component
         removeComponent(i);
     }
+
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
 }
 
 // Remove a component at a given index
@@ -154,37 +188,132 @@ void TransformComponents::removeComponent(uint32 index) {
     assert(index < mNbComponents);
 
     // We want to keep the arrays tightly packed. Therefore, when a component is removed,
-    // we replace it with the last element of the array
+    // we replace it with the last element of the array. But we need to make sure that sleeping
+    // and non-sleeping components stay grouped together.
 
-    const uint32 lastIndex = mNbComponents - 1;
+    // Destroy the component
+    destroyComponent(index);
 
-    Entity entity = mEntities[index];
-    Entity lastEntity = mEntities[lastIndex];
+    // If the component to remove is sleeping
+    if (index >= mSleepingStartIndex) {
 
-    if (mNbComponents > 1 && index != lastIndex) {
+        // If the component is not the last one
+        if (index != mNbComponents - 1) {
 
-        // Replace the data of the component to destroy by the data of the last component
-        mEntities[index] = mEntities[lastIndex];
-        mTransforms[index] = mTransforms[lastIndex];
-
-        // Update the entity to component index mapping
-        mMapEntityToComponentIndex[lastEntity] = index;
+            // We replace it by the last sleeping component
+            moveComponentToIndex(mNbComponents - 1, index);
+        }
     }
-    else {
+    else {   // If the component to remove is not sleeping
 
-        // Call the destructors on the component values
-        destroyComponent(index);
+        // If it not the last awake component
+        if (index != mSleepingStartIndex - 1) {
+
+            // We replace it by the last awake component
+            moveComponentToIndex(mSleepingStartIndex - 1, index);
+        }
+
+        // If there are sleeping components at the end
+        if (mSleepingStartIndex != mNbComponents) {
+
+            // We replace the last awake component by the last sleeping component
+            moveComponentToIndex(mNbComponents - 1, index);
+
+            mSleepingStartIndex--;
+        }
     }
 
-    // Update the entity to component index mapping
-    mMapEntityToComponentIndex.remove(entity);
-
     mNbComponents--;
+
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
+}
+
+// Notify if a given entity is sleeping or not
+void TransformComponents::setIsEntitySleeping(Entity entity, bool isSleeping) {
+
+    const uint32 index = mMapEntityToComponentIndex[entity];
+
+    // If the component was sleeping and is not sleeping anymore
+    if (!isSleeping && index >= mSleepingStartIndex) {
+
+        assert(mSleepingStartIndex < mNbComponents);
+
+        // If the sleeping component is not the first sleeping component
+        if (mSleepingStartIndex != index) {
+
+            // Swap the first sleeping component with the one we need to wake up
+            swapComponents(index, mSleepingStartIndex);
+        }
+
+        mSleepingStartIndex++;
+    }
+    // If the component was awake and must now go to sleep
+    else if (isSleeping && index < mSleepingStartIndex) {
+
+        assert(mSleepingStartIndex > 0);
+
+        // If the awake component is not the only awake component
+        if (index != mSleepingStartIndex - 1) {
+
+            // Swap the last awake component with the one we need to put to sleep
+            swapComponents(index, mSleepingStartIndex - 1);
+        }
+
+        mSleepingStartIndex--;
+    }
+
+    assert(mSleepingStartIndex <= mNbComponents);
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
+}
+
+// Move a component from a source to a destination index in the components array
+// The destination location must contain a constructed object
+void TransformComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
+
+    // Copy the data of the source component to the destination location
+    new (mEntities + destIndex) Entity(mEntities[srcIndex]);
+    new (mTransforms + destIndex) Transform(mTransforms[srcIndex]);
+
+    const Entity entity = mEntities[srcIndex];
+
+    // Destroy the source component
+    destroyComponent(srcIndex);
+
+    // Update the entity to component index mapping
+    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, destIndex));
+
+    assert(mMapEntityToComponentIndex[mEntities[destIndex]] == destIndex);
+}
+
+// Swap two components in the array
+void TransformComponents::swapComponents(uint32 index1, uint32 index2) {
+
+    // Copy component 1 data
+    Entity entity1(mEntities[index1]);
+    Transform transform1(mTransforms[index1]);
+
+    // Destroy component 1
+    destroyComponent(index1);
+
+    moveComponentToIndex(index2, index1);
+
+    // Reconstruct component 1 at component 2 location
+    new (mEntities + index2) Entity(entity1);
+    new (mTransforms + index2) Transform(transform1);
+
+    // Update the entity to component index mapping
+    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
+
+    assert(mMapEntityToComponentIndex[mEntities[index1]] == index1);
+    assert(mMapEntityToComponentIndex[mEntities[index2]] == index2);
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
 }
 
 // Destroy a component at a given index
 void TransformComponents::destroyComponent(uint32 index) {
 
+    mMapEntityToComponentIndex.remove(mEntities[index]);
+
     mEntities[index].~Entity();
     mTransforms[index].~Transform();
 }

src/components/TransformComponents.h

@@ -42,6 +42,7 @@ class EntityManager;
 /**
  * This class represent the component of the ECS that contains the transforms of the
  * different entities. The position and orientation of the bodies are stored there.
+ * The components of the sleeping entities (bodies) are always stored at the end of the array.
  */
 class TransformComponents {
 
@@ -68,6 +69,9 @@ class TransformComponents {
         /// Number of allocated components
         uint32 mNbAllocatedComponents;
 
+        /// Index of the first component of a sleeping entity (sleeping components are stored at the end)
+        uint32 mSleepingStartIndex;
+
         /// Allocated memory for all the data of the components
         void* mBuffer;
 
@@ -88,6 +92,12 @@ class TransformComponents {
         /// Destroy a component at a given index
         void destroyComponent(uint32 index);
 
+        // Move a component from a source to a destination index in the components array
+        void moveComponentToIndex(uint32 srcIndex, uint32 destIndex);
+
+        /// Swap two components in the array
+        void swapComponents(uint32 index1, uint32 index2);
+
     public:
 
         /// Structure for the data of a transform component
@@ -113,7 +123,7 @@ class TransformComponents {
         void allocate(uint32 nbComponentsToAllocate);
 
         /// Add a component
-        void addComponent(Entity entity, const TransformComponent& component);
+        void addComponent(Entity entity, bool isSleeping, const TransformComponent& component);
 
         /// Perform garbage collection to remove unused components
         void garbageCollection(const EntityManager& entityManager);
@@ -123,6 +133,9 @@ class TransformComponents {
 
         /// Set the transform of an entity
         void setTransform(Entity entity, const Transform& transform);
+
+        /// Notify if a given entity is sleeping or not
+        void setIsEntitySleeping(Entity entity, bool isSleeping);
 };
 
 // Return the transform of an entity

src/engine/CollisionWorld.cpp

@@ -150,7 +150,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
     assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
 
     // Add a transform component
-    mTransformComponents.addComponent(entity, TransformComponents::TransformComponent(transform));
+    mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
 
     // Create the collision body
     CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
@@ -238,6 +238,15 @@ void CollisionWorld::resetContactManifoldListsOfBodies() {
     }
 }
 
+// Notify the world if a body is sleeping or not
+void CollisionWorld::notifyBodySleeping(Entity entity, bool isSleeping) {
+
+    // TODO : Make sure we notify all the components here ...
+
+    // Notify all the components
+    mTransformComponents.setIsEntitySleeping(entity, isSleeping);
+}
+
 // Test if the AABBs of two bodies overlap
 /**
  * @param body1 Pointer to the first body to test

src/engine/CollisionWorld.h

@@ -121,6 +121,9 @@ class CollisionWorld {
         /// Reset all the contact manifolds linked list of each body
         void resetContactManifoldListsOfBodies();
 
+        /// Notify the world if a body is sleeping or not
+        void notifyBodySleeping(Entity entity, bool isSleeping);
+
     public :
 
         // -------------------- Methods -------------------- //

src/engine/DynamicsWorld.cpp

@@ -429,7 +429,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
     assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
 
     // Add a transform component
-    mTransformComponents.addComponent(entity, TransformComponents::TransformComponent(transform));
+    mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
 
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,