From 236a855c8dabe09a1a098feb89f90a65efd0c316 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 4 Jul 2019 20:24:09 +0200
Subject: [PATCH] Remove Body::mId attribute

---
 src/body/Body.cpp                         |  10 +-
 src/body/Body.h                           |  23 +---
 src/body/CollisionBody.cpp                |  14 +--
 src/body/CollisionBody.h                  |   2 +-
 src/body/RigidBody.cpp                    |  26 ++--
 src/body/RigidBody.h                      |   2 +-
 src/collision/CollisionDetection.cpp      |   2 +-
 src/collision/CollisionDetection.h        |   2 +-
 src/configuration.h                       |   4 +-
 src/engine/CollisionWorld.cpp             |  37 +-----
 src/engine/CollisionWorld.h               |   9 --
 src/engine/DynamicsWorld.cpp              |  19 +--
 src/engine/OverlappingPair.h              |  10 +-
 test/tests/collision/TestCollisionWorld.h | 144 +++++++++++-----------
 test/tests/collision/TestRaycast.h        |   2 +-
 15 files changed, 125 insertions(+), 181 deletions(-)

diff --git a/src/body/Body.cpp b/src/body/Body.cpp
index 1117572f..f1f80b0a 100644
--- a/src/body/Body.cpp
+++ b/src/body/Body.cpp
@@ -35,8 +35,8 @@ using namespace reactphysics3d;
 /**
  * @param id ID of the new body
  */
-Body::Body(Entity entity, bodyindex id)
-     : mID(id), mEntity(entity), mIsAllowedToSleep(true), mIsActive(true),
+Body::Body(Entity entity)
+     : mEntity(entity), mIsAllowedToSleep(true), mIsActive(true),
        mIsSleeping(false), mSleepTime(0), mUserData(nullptr) {
 
 #ifdef IS_LOGGING_ACTIVE
@@ -55,7 +55,7 @@ void Body::setIsActive(bool isActive) {
     setIsSleeping(isActive);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set isActive=" +
+             "Body " + std::to_string(mEntity.id) + ": Set isActive=" +
              (mIsActive ? "true" : "false"));
 }
 
@@ -82,7 +82,7 @@ void Body::setIsSleeping(bool isSleeping) {
         mIsSleeping = isSleeping;
 
         RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set isSleeping=" +
+             "Body " + std::to_string(mEntity.id) + ": Set isSleeping=" +
              (mIsSleeping ? "true" : "false"));
     }
 }
@@ -97,7 +97,7 @@ void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
     if (!mIsAllowedToSleep) setIsSleeping(false);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set isAllowedToSleep=" +
+             "Body " + std::to_string(mEntity.id) + ": Set isAllowedToSleep=" +
              (mIsAllowedToSleep ? "true" : "false"));
 }
 
diff --git a/src/body/Body.h b/src/body/Body.h
index 4c7513a5..39093dd6 100644
--- a/src/body/Body.h
+++ b/src/body/Body.h
@@ -50,10 +50,6 @@ class Body {
 
         // -------------------- Attributes -------------------- //
 
-        /// ID of the body
-        // TODO : Remove this
-        bodyindex mID;
-
         /// Identifier of the entity in the ECS
         Entity mEntity;
 
@@ -93,7 +89,7 @@ class Body {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Body(Entity entity, bodyindex id);
+        Body(Entity entity);
 
         /// Deleted copy-constructor
         Body(const Body& body) = delete;
@@ -104,9 +100,6 @@ class Body {
         /// Destructor
         virtual ~Body() = default;
 
-        /// Return the ID of the body
-        bodyindex getId() const;
-
         /// Return the corresponding entity of the body
         Entity getEntity() const;
 
@@ -142,6 +135,7 @@ class Body {
 
         // TODO : Check if those operators are still used
 
+        /*
         /// Smaller than operator
         bool operator<(const Body& body2) const;
 
@@ -153,20 +147,13 @@ class Body {
 
         /// Not equal operator
         bool operator!=(const Body& body2) const;
+        */
 
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
 };
 
-// Return the id of the body
-/**
- * @return The id of the body
- */
-inline bodyindex Body::getId() const {
-    return mID;
-}
-
 // Return the corresponding entity of the body
 /**
  * @return The entity of the body
@@ -224,6 +211,7 @@ inline void Body::setLogger(Logger* logger) {
 
 #endif
 
+/*
 // Smaller than operator
 inline bool Body::operator<(const Body& body2) const {
     return (mID < body2.mID);
@@ -243,7 +231,8 @@ inline bool Body::operator==(const Body& body2) const {
 inline bool Body::operator!=(const Body& body2) const {
     return (mID != body2.mID);
 }               
+*/
 
 }
 
- #endif
+#endif
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index e7023f3a..64d4323e 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -39,8 +39,8 @@ using namespace reactphysics3d;
  * @param world The physics world where the body is created
  * @param id ID of the body
  */
-CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id)
-              : Body(entity, id), mType(BodyType::DYNAMIC), mWorld(world) {
+CollisionBody::CollisionBody(CollisionWorld& world, Entity entity)
+              : Body(entity), mType(BodyType::DYNAMIC), mWorld(world) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -112,7 +112,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
     mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
+             "Body " + std::to_string(mEntity.id) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
              "ProxyShape " + std::to_string(proxyShape->getBroadPhaseId()) + ":  collisionShape=" +
@@ -166,7 +166,7 @@ ProxyShape* CollisionBody::getProxyShape(uint proxyShapeIndex) {
 void CollisionBody::removeCollisionShape(ProxyShape* proxyShape) {
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " removed from body");
+             "Body " + std::to_string(mEntity.id) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " removed from body");
 
     // Remove the proxy-shape from the broad-phase
     if (proxyShape->getBroadPhaseId() != -1) {
@@ -268,7 +268,7 @@ void CollisionBody::setIsActive(bool isActive) {
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set isActive=" +
+             "Body " + std::to_string(mEntity.id) + ": Set isActive=" +
              (mIsActive ? "true" : "false"));
 }
 
@@ -392,7 +392,7 @@ void CollisionBody::setTransform(const Transform& transform) {
     updateBroadPhaseState();
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set transform=" + transform.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set transform=" + transform.to_string());
 }
 
 // Set the variable to know whether or not the body is sleeping
@@ -465,7 +465,7 @@ void CollisionBody::setType(BodyType type) {
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set type=" +
+             "Body " + std::to_string(mEntity.id) + ": Set type=" +
              (mType == BodyType::STATIC ? "Static" : (mType == BodyType::DYNAMIC ? "Dynamic" : "Kinematic")));
 }
 
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 152b3ec9..846e593c 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -101,7 +101,7 @@ class CollisionBody : public Body {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        CollisionBody(CollisionWorld& world, Entity entity, bodyindex id);
+        CollisionBody(CollisionWorld& world, Entity entity);
 
         /// Destructor
         virtual ~CollisionBody() override;
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index aed6977f..39876931 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -39,8 +39,8 @@ using namespace reactphysics3d;
 * @param world The world where the body has been added
 * @param id The ID of the body
 */
-RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
-          : CollisionBody(world, entity, id),  mMaterial(world.mConfig), mJointsList(nullptr),
+RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity)
+          : CollisionBody(world, entity),  mMaterial(world.mConfig), mJointsList(nullptr),
             mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
 
     // Compute the inverse mass
@@ -197,7 +197,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
     updateInertiaTensorInverseWorld();
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
 }
 
 // Apply an external force to the body at its center of mass.
@@ -260,7 +260,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
     updateInertiaTensorInverseWorld();
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string());
 }
 
 // Set the local center of mass of the body (in local-space coordinates)
@@ -293,7 +293,7 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
     mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string());
 }
 
 // Set the mass of the rigid body
@@ -315,7 +315,7 @@ void RigidBody::setMass(decimal mass) {
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set mass=" + std::to_string(mass));
+             "Body " + std::to_string(mEntity.id) + ": Set mass=" + std::to_string(mass));
 }
 
 // Remove a joint from the joints list
@@ -429,7 +429,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
     recomputeMassInformation();
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
+             "Body " + std::to_string(mEntity.id) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body");
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape,
              "ProxyShape " + std::to_string(proxyShape->getBroadPhaseId()) + ":  collisionShape=" +
@@ -463,7 +463,7 @@ void RigidBody::enableGravity(bool isEnabled) {
     mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
+             "Body " + std::to_string(mEntity.id) + ": Set isGravityEnabled=" +
              (isEnabled ? "true" : "false"));
 }
 
@@ -477,7 +477,7 @@ void RigidBody::setLinearDamping(decimal linearDamping) {
     mWorld.mDynamicsComponents.setLinearDamping(mEntity, linearDamping);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(linearDamping));
+             "Body " + std::to_string(mEntity.id) + ": Set linearDamping=" + std::to_string(linearDamping));
 }
 
 // Set the angular damping factor. This is the ratio of the angular velocity
@@ -490,7 +490,7 @@ void RigidBody::setAngularDamping(decimal angularDamping) {
     mWorld.mDynamicsComponents.setAngularDamping(mEntity, angularDamping);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(angularDamping));
+             "Body " + std::to_string(mEntity.id) + ": Set angularDamping=" + std::to_string(angularDamping));
 }
 
 /// Update the transform of the body after a change of the center of mass
@@ -513,7 +513,7 @@ void RigidBody::setMaterial(const Material& material) {
     mMaterial = material;
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set Material" + mMaterial.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set Material" + mMaterial.to_string());
 }
 
 // Set the linear velocity of the rigid body.
@@ -534,7 +534,7 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set linearVelocity=" + linearVelocity.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set linearVelocity=" + linearVelocity.to_string());
 }
 
 // Set the angular velocity.
@@ -557,7 +557,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set angularVelocity=" + angularVelocity.to_string());
+             "Body " + std::to_string(mEntity.id) + ": Set angularVelocity=" + angularVelocity.to_string());
 }
 
 // Set the current position and orientation
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 1f8e16bb..2c9222bf 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -86,7 +86,7 @@ class RigidBody : public CollisionBody {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id);
+        RigidBody(const Transform& transform, CollisionWorld& world, Entity entity);
 
         /// Destructor
         virtual ~RigidBody() override;
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index a1647ca9..617fe2ac 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -244,7 +244,7 @@ void CollisionDetection::computeMiddlePhase(OverlappingPairMap& overlappingPairs
             if (!isBody1Active && !isBody2Active) continue;
 
             // Check if the bodies are in the set of bodies that cannot collide between each other
-            bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+            bodypair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
             if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue;
 
 			bool isShape1Convex = shape1->getCollisionShape()->isConvex();
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index e4b16417..85930a78 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -95,7 +95,7 @@ class CollisionDetection {
         BroadPhaseSystem mBroadPhaseSystem;
 
         /// Set of pair of bodies that cannot collide between each other
-        Set<bodyindexpair> mNoCollisionPairs;
+        Set<bodypair> mNoCollisionPairs;
 
         /// Map a broad-phase id with the corresponding entity of the proxy-shape
         Map<int, Entity> mMapBroadPhaseIdToProxyShapeEntity;
diff --git a/src/configuration.h b/src/configuration.h
index 3dae1e18..fa0fe421 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -62,8 +62,8 @@ using int32 = std::int32_t;
 using uint32 = std::uint32_t;
 
 // TODO : Delete this
-using bodyindex = luint;
-using bodyindexpair = Pair<bodyindex, bodyindex>;
+struct Entity;
+using bodypair = Pair<Entity, Entity>;
 
 // ------------------- Enumerations ------------------- //
 
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index c63d017a..20c08fb8 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -41,9 +41,8 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge
                  mBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()),
                  mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mDynamicsComponents(mMemoryManager.getBaseAllocator()),
                  mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mDynamicsComponents, mMemoryManager),
-                 mBodies(mMemoryManager.getPoolAllocator()), mCurrentBodyId(0), mFreeBodiesIds(mMemoryManager.getPoolAllocator()),
-                 mEventListener(nullptr), mName(worldSettings.worldName),
-                 mIsProfilerCreatedByUser(profiler != nullptr),
+                 mBodies(mMemoryManager.getPoolAllocator()),  mEventListener(nullptr),
+                 mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr),
                  mIsLoggerCreatedByUser(logger != nullptr) {
 
     // Automatically generate a name for the world
@@ -148,18 +147,12 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
     // Create a new entity for the body
     Entity entity = mEntityManager.createEntity();
 
-    // Get the next available body ID
-    bodyindex bodyID = computeNextAvailableBodyId();
-
-    // Largest index cannot be used (it is used for invalid index)
-    assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
-
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
 
     // Create the collision body
     CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
                                         sizeof(CollisionBody)))
-                                        CollisionBody(*this, entity, bodyID);
+                                        CollisionBody(*this, entity);
 
     assert(collisionBody != nullptr);
 
@@ -181,7 +174,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
 #endif
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(bodyID) + ": New collision body created");
+             "Body " + std::to_string(entity.id) + ": New collision body created");
 
     // Return the pointer to the rigid body
     return collisionBody;
@@ -194,14 +187,11 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
 void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(collisionBody->getId()) + ": collision body destroyed");
+             "Body " + std::to_string(collisionBody->getEntity().id) + ": collision body destroyed");
 
     // Remove all the collision shapes of the body
     collisionBody->removeAllCollisionShapes();
 
-    // Add the body ID to the list of free IDs
-    mFreeBodiesIds.add(collisionBody->getId());
-
     mBodyComponents.removeComponent(collisionBody->getEntity());
     mTransformComponents.removeComponent(collisionBody->getEntity());
     mEntityManager.destroyEntity(collisionBody->getEntity());
@@ -216,23 +206,6 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody));
 }
 
-// Return the next available body ID
-bodyindex CollisionWorld::computeNextAvailableBodyId() {
-
-    // Compute the body ID
-    bodyindex bodyID;
-    if (mFreeBodiesIds.size() != 0) {
-        bodyID = mFreeBodiesIds[mFreeBodiesIds.size() - 1];
-        mFreeBodiesIds.removeAt(mFreeBodiesIds.size() - 1);
-    }
-    else {
-        bodyID = mCurrentBodyId;
-        mCurrentBodyId++;
-    }
-
-    return bodyID;
-}
-
 // Notify the world if a body is disabled (sleeping or inactive) or not
 void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
 
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index f9d0b5d8..aef8201d 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -94,12 +94,6 @@ class CollisionWorld {
         /// All the bodies (rigid and soft) of the world
         List<CollisionBody*> mBodies;
 
-        /// Current body id
-        bodyindex mCurrentBodyId;
-
-        /// List of free ids for rigid bodies
-        List<luint> mFreeBodiesIds;
-
         /// Pointer to an event listener object
         EventListener* mEventListener;
 
@@ -129,9 +123,6 @@ class CollisionWorld {
 
         // -------------------- Methods -------------------- //
 
-        /// Return the next available body id
-        bodyindex computeNextAvailableBodyId();
-
         /// Notify the world if a body is disabled (slepping or inactive) or not
         void notifyBodyDisabled(Entity entity, bool isDisabled);
 
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index d3d18d06..afa5fe3b 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -375,18 +375,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
     // Create a new entity for the body
     Entity entity = mEntityManager.createEntity();
 
-    // Compute the body ID
-    bodyindex bodyID = computeNextAvailableBodyId();
-
-    // Largest index cannot be used (it is used for invalid index)
-    assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
-
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
     mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition()));
 
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
-                                     sizeof(RigidBody))) RigidBody(transform, *this, entity, bodyID);
+                                     sizeof(RigidBody))) RigidBody(transform, *this, entity);
     assert(rigidBody != nullptr);
 
     BodyComponents::BodyComponent bodyComponent(rigidBody);
@@ -405,7 +399,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
 #endif
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(bodyID) + ": New collision body created");
+             "Body " + std::to_string(entity.id) + ": New collision body created");
 
     // Return the pointer to the rigid body
     return rigidBody;
@@ -418,14 +412,11 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
 void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(rigidBody->getId()) + ": rigid body destroyed");
+             "Body " + std::to_string(rigidBody->getEntity().id) + ": rigid body destroyed");
 
     // Remove all the collision shapes of the body
     rigidBody->removeAllCollisionShapes();
 
-    // Add the body ID to the list of free IDs
-    mFreeBodiesIds.add(rigidBody->getId());
-
     // Destroy all the joints in which the rigid body to be destroyed is involved
     JointListElement* element;
     for (element = rigidBody->mJointsList; element != nullptr; element = element->next) {
@@ -590,7 +581,7 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
     joint->mBody1->mJointsList = jointListElement1;
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(joint->mBody1->getId()) + ": Joint " + std::to_string(joint->getId()) +
+             "Body " + std::to_string(joint->mBody1->getEntity().id) + ": Joint " + std::to_string(joint->getId()) +
              " added to body");
 
     // Add the joint at the beginning of the linked list of joints of the second body
@@ -601,7 +592,7 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
     joint->mBody2->mJointsList = jointListElement2;
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(joint->mBody2->getId()) + ": Joint " + std::to_string(joint->getId()) +
+             "Body " + std::to_string(joint->mBody2->getEntity().id) + ": Joint " + std::to_string(joint->getId()) +
              " added to body");
 }
 
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 2062a499..943bc1ed 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -180,7 +180,7 @@ class OverlappingPair {
         static OverlappingPairId computeID(int shape1BroadPhaseId, int shape2BroadPhaseId);
 
         /// Return the pair of bodies index of the pair
-        static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
+        static bodypair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
 
         // -------------------- Friendship -------------------- //
 
@@ -225,13 +225,13 @@ inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1B
 }
 
 // Return the pair of bodies index
-inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1,
+inline bodypair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1,
                                                              CollisionBody* body2) {
 
     // Construct the pair of body index
-    bodyindexpair indexPair = body1->getId() < body2->getId() ?
-                                 bodyindexpair(body1->getId(), body2->getId()) :
-                                 bodyindexpair(body2->getId(), body1->getId());
+    bodypair indexPair = body1->getEntity().id < body2->getEntity().id ?
+                                 bodypair(body1->getEntity(), body2->getEntity()) :
+                                 bodypair(body2->getEntity(), body1->getEntity());
     assert(indexPair.first != indexPair.second);
     return indexPair;
 }
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
index c7e78a07..3a218d1c 100644
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -631,7 +631,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			Vector3 localBody1Point(3, 0, 0);
@@ -655,7 +655,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -676,7 +676,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -697,7 +697,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -747,7 +747,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			Vector3 localBody1Point(3, 0, 0);
@@ -771,7 +771,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -792,7 +792,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -813,7 +813,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -853,7 +853,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			localBody1Point = std::sqrt(4.5f) * Vector3(1, -1, 0);
@@ -877,7 +877,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -898,7 +898,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -919,7 +919,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -959,7 +959,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			localBody1Point = std::sqrt(9.0f / 3.0f) * Vector3(1, -1, -1);
@@ -983,7 +983,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1004,7 +1004,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1025,7 +1025,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1075,7 +1075,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			Vector3 localBody1Point(0, -3, 0);
@@ -1099,7 +1099,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1120,7 +1120,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1141,7 +1141,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1181,7 +1181,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			localBody1Point = Vector3(3, 0, 0);
@@ -1205,7 +1205,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1226,7 +1226,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1247,7 +1247,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1297,7 +1297,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			Vector3 localBody1Point(3, 0, 0);
@@ -1321,7 +1321,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1342,7 +1342,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1363,7 +1363,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1403,7 +1403,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			localBody1Point = std::sqrt(4.5f) * Vector3(1, -1, 0);
@@ -1427,7 +1427,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1448,7 +1448,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1469,7 +1469,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1509,7 +1509,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
 			localBody1Point = std::sqrt(9.0f / 3.0f) * Vector3(1, -1, -1);
@@ -1533,7 +1533,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1554,7 +1554,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1575,7 +1575,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
 			// True if the bodies are swapped in the collision callback response
-			swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
 			// Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1625,7 +1625,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point(0, -3, 0);
@@ -1649,7 +1649,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1670,7 +1670,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1691,7 +1691,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mSphereBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mSphereBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -1741,7 +1741,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point1(-3, -2, -2);
@@ -1783,7 +1783,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -1812,7 +1812,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -1842,7 +1842,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -1901,7 +1901,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point1(-3, -2, -2);
@@ -1943,7 +1943,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -1972,7 +1972,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2002,7 +2002,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2061,7 +2061,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point1(-3, -2, -2);
@@ -2103,7 +2103,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2132,7 +2132,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2162,7 +2162,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2221,7 +2221,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point1(3, 1, 0);
@@ -2244,7 +2244,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2265,7 +2265,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2286,7 +2286,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mBoxBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mBoxBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2336,7 +2336,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point1(3, 1, 0);
@@ -2359,7 +2359,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2380,7 +2380,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2401,7 +2401,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mConvexMeshBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mConvexMeshBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point1 : localBody1Point1,
@@ -2647,7 +2647,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point(2, 3, 0);
@@ -2671,7 +2671,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2692,7 +2692,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2713,7 +2713,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2753,7 +2753,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             localBody1Point = Vector3(0, 5, 0);
@@ -2777,7 +2777,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2798,7 +2798,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2819,7 +2819,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2869,7 +2869,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            bool swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            bool swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             Vector3 localBody1Point(0, -5, 0);
@@ -2893,7 +2893,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2914,7 +2914,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
@@ -2935,7 +2935,7 @@ class TestCollisionWorld : public Test {
             rp3d_test(collisionData->getTotalNbContactPoints() == 1);
 
             // True if the bodies are swapped in the collision callback response
-            swappedBodiesCollisionData = collisionData->getBody1()->getId() != mCapsuleBody1->getId();
+            swappedBodiesCollisionData = collisionData->getBody1()->getEntity() != mCapsuleBody1->getEntity();
 
             // Test contact points
             rp3d_test(collisionData->hasContactPointSimilarTo(swappedBodiesCollisionData ? localBody2Point : localBody1Point,
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 892b797b..995108f2 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -66,7 +66,7 @@ class WorldRaycastCallback : public RaycastCallback {
 
         virtual decimal notifyRaycastHit(const RaycastInfo& info) override {
 
-            if (shapeToTest->getBody()->getId() == info.body->getId()) {
+            if (shapeToTest->getBody()->getEntity() == info.body->getEntity()) {
                 raycastInfo.body = info.body;
                 raycastInfo.hitFraction = info.hitFraction;
                 raycastInfo.proxyShape = info.proxyShape;