Merge branch 'contacts' into ecs

CHANGELOG.md

@@ -1,5 +1,18 @@
 # Changelog
 
+## Develop
+
+### Changed
+
+ - The CollisionWorld::testCollision() methods do not have the 'categoryMaskBits' parameter anymore.
+ - The CollisionWorld::testOverlap() methods do not have the 'categoryMaskBits' parameter anymore.
+ - Many methods in the EventListener class have changed. Check the user manual for more information.
+ - The way to retrieve contacts from a CollisionCallbackInfo object has changed. Check the user manual for more information.
+
+### Removed
+
+ - DynamicsWorld::getContactsList(). You need to use the EventListener class to retrieve contacts now.
+
 ## Release Candidate
 
 ### Fixed
@@ -25,6 +38,7 @@
  - The methods CollisionBody::getProxyShapesList() has been remove. You can now use the
    CollisionBody::getNbProxyShapes() method to know the number of proxy-shapes of a body and the
    CollisionBody::getProxyShape(uint proxyShapeIndex) method to get a given proxy-shape of the body.
+ - The CollisionWorld::testAABBOverlap() methods have been removed.
 
 ## Version 0.7.0 (May 1, 2018)
 

CMakeLists.txt

@@ -81,6 +81,8 @@ SET (REACTPHYSICS3D_HEADERS
     "src/body/CollisionBody.h"
     "src/body/RigidBody.h"
     "src/collision/ContactPointInfo.h"
+    "src/collision/ContactManifoldInfo.h"
+    "src/collision/ContactPair.h"
     "src/collision/broadphase/DynamicAABBTree.h"
     "src/collision/narrowphase/CollisionDispatch.h"
     "src/collision/narrowphase/GJK/VoronoiSimplex.h"
@@ -119,7 +121,6 @@ SET (REACTPHYSICS3D_HEADERS
     "src/collision/HalfEdgeStructure.h"
     "src/collision/CollisionDetection.h"
     "src/collision/ContactManifold.h"
-    "src/collision/ContactManifoldSet.h"
     "src/collision/MiddlePhaseTriangleCallback.h"
     "src/constraint/BallAndSocketJoint.h"
     "src/constraint/ContactPoint.h"
@@ -135,6 +136,7 @@ SET (REACTPHYSICS3D_HEADERS
     "src/engine/DynamicsWorld.h"
     "src/engine/EventListener.h"
     "src/engine/Island.h"
+    "src/engine/Islands.h"
     "src/engine/Material.h"
     "src/engine/OverlappingPair.h"
     "src/engine/Timer.h"
@@ -213,7 +215,6 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/HalfEdgeStructure.cpp"
     "src/collision/CollisionDetection.cpp"
     "src/collision/ContactManifold.cpp"
-    "src/collision/ContactManifoldSet.cpp"
     "src/collision/MiddlePhaseTriangleCallback.cpp"
     "src/constraint/BallAndSocketJoint.cpp"
     "src/constraint/ContactPoint.cpp"
@@ -238,6 +239,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/components/ProxyShapeComponents.cpp"
     "src/components/DynamicsComponents.cpp"
     "src/collision/CollisionCallback.cpp"
+    "src/collision/OverlapCallback.cpp"
     "src/mathematics/mathematics_functions.cpp"
     "src/mathematics/Matrix2x2.cpp"
     "src/mathematics/Matrix3x3.cpp"

src/body/Body.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
  * @param id ID of the new body
  */
 Body::Body(Entity entity, bodyindex id)
-     : mID(id), mEntity(entity), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
+     : mID(id), mEntity(entity), mIsAllowedToSleep(true), mIsActive(true),
        mIsSleeping(false), mSleepTime(0), mUserData(nullptr) {
 
 #ifdef IS_LOGGING_ACTIVE

src/body/Body.h

@@ -57,9 +57,6 @@ class Body {
         /// Identifier of the entity in the ECS
         Entity mEntity;
 
-        /// True if the body has already been added in an island (for sleeping technique)
-        bool mIsAlreadyInIsland;
-
         /// True if the body is allowed to go to sleep for better efficiency
         bool mIsAllowedToSleep;
 
@@ -75,8 +72,10 @@ class Body {
         bool mIsActive;
 
         /// True if the body is sleeping (for sleeping technique)
+        // TODO : DELETE THIS
         bool mIsSleeping;
 
+        // TODO : Move this into the body components
         /// Elapsed time since the body velocity was bellow the sleep velocity
         decimal mSleepTime;
 
@@ -141,6 +140,8 @@ class Body {
         void setLogger(Logger* logger);
 #endif
 
+        // TODO : Check if those operators are still used
+
         /// Smaller than operator
         bool operator<(const Body& body2) const;
 

src/body/CollisionBody.cpp

@@ -40,8 +40,7 @@ using namespace reactphysics3d;
  * @param id ID of the body
  */
 CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id)
-              : Body(entity, id), mType(BodyType::DYNAMIC),
+              : Body(entity, id), mType(BodyType::DYNAMIC), mWorld(world) {
-                mContactManifoldsList(nullptr), mWorld(world) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -51,7 +50,7 @@ CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id)
 
 // Destructor
 CollisionBody::~CollisionBody() {
-    assert(mContactManifoldsList == nullptr);
+
 }
 
 // Add a collision shape to the body. Note that you can share a collision
@@ -198,23 +197,6 @@ void CollisionBody::removeAllCollisionShapes() {
     }
 }
 
-// Reset the contact manifold lists
-void CollisionBody::resetContactManifoldsList() {
-
-    // Delete the linked list of contact manifolds of that body
-    ContactManifoldListElement* currentElement = mContactManifoldsList;
-    while (currentElement != nullptr) {
-        ContactManifoldListElement* nextElement = currentElement->getNext();
-
-        // Delete the current element
-        currentElement->~ContactManifoldListElement();
-        mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement));
-
-        currentElement = nextElement;
-    }
-    mContactManifoldsList = nullptr;
-}
-
 // Return the current position and orientation
 /**
  * @return The current transformation of the body that transforms the local-space
@@ -283,9 +265,6 @@ void CollisionBody::setIsActive(bool isActive) {
                 mWorld.mCollisionDetection.removeProxyCollisionShape(proxyShape);
             }
         }
-
-        // Reset the contact manifold list of the body
-        resetContactManifoldsList();
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -307,26 +286,6 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
     }
 }
 
-// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
-/// This method also returns the number of contact manifolds of the body.
-int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
-
-    mIsAlreadyInIsland = false;
-
-    int nbManifolds = 0;
-
-    // Reset the mIsAlreadyInIsland variable of the contact manifolds for
-    // this body
-    ContactManifoldListElement* currentElement = mContactManifoldsList;
-    while (currentElement != nullptr) {
-        currentElement->getContactManifold()->mIsAlreadyInIsland = false;
-        currentElement = currentElement->getNext();
-        nbManifolds++;
-    }
-
-    return nbManifolds;
-}
-
 // Return true if a point is inside the collision body
 /// This method returns true if a point is inside any collision shape of the body
 /**

src/body/CollisionBody.h

@@ -67,12 +67,10 @@ class CollisionBody : public Body {
 
         // -------------------- Attributes -------------------- //
 
+        // TODO : Move this into the dynamics components
         /// Type of body (static, kinematic or dynamic)
         BodyType mType;
 
-        /// First element of the linked list of contact manifolds involving this body
-        ContactManifoldListElement* mContactManifoldsList;
-
         /// Reference to the world the body belongs to
         CollisionWorld& mWorld;
 
@@ -85,9 +83,6 @@ class CollisionBody : public Body {
 
         // -------------------- Methods -------------------- //
 
-        /// Reset the contact manifold lists
-        void resetContactManifoldsList();
-
         /// Remove all the collision shapes
         void removeAllCollisionShapes();
 
@@ -98,9 +93,6 @@ class CollisionBody : public Body {
         /// (as if the body has moved).
         void askForBroadPhaseCollisionCheck() const;
 
-        /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
-        int resetIsAlreadyInIslandAndCountManifolds();
-
         /// Set the variable to know whether or not the body is sleeping
         virtual void setIsSleeping(bool isSleeping) override;
 
@@ -142,9 +134,6 @@ class CollisionBody : public Body {
         /// Remove a collision shape from the body
         virtual void removeCollisionShape(ProxyShape *proxyShape);
 
-        /// Return the first element of the linked list of contact manifolds involving this body
-        const ContactManifoldListElement* getContactManifoldsList() const;
-
         /// Return true if a point is inside the collision body
         bool testPointInside(const Vector3& worldPoint) const;
 
@@ -203,15 +192,6 @@ inline BodyType CollisionBody::getType() const {
     return mType;
 }
 
-// Return the first element of the linked list of contact manifolds involving this body
-/**
- * @return A pointer to the first element of the linked-list with the contact
- *         manifolds of this body
- */
-inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const {
-    return mContactManifoldsList;
-}
-
 /// Test if the collision body overlaps with a given AABB
 /**
 * @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap

src/body/RigidBody.cpp

@@ -40,13 +40,11 @@ using namespace reactphysics3d;
 * @param id The ID of the body
 */
 RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
-          : CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)),
+          : CollisionBody(world, entity, id),  mMaterial(world.mConfig), mJointsList(nullptr),
-            mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
+            mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
-            mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
-            mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
 
     // Compute the inverse mass
-    mMassInverse = decimal(1.0) / mInitMass;
+    mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -91,15 +89,15 @@ void RigidBody::setType(BodyType type) {
     if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
 
         // Reset the inverse mass and inverse inertia tensor to zero
-        mMassInverse = decimal(0.0);
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0));
-        mInertiaTensorLocalInverse.setToZero();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
-        mInertiaTensorInverseWorld.setToZero();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
     }
     else {  // If it is a dynamic body
-        mMassInverse = decimal(1.0) / mInitMass;
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
 
         if (mIsInertiaTensorSetByUser) {
-            mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+            mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
         }
     }
 
@@ -109,16 +107,73 @@ void RigidBody::setType(BodyType type) {
     // Awake the body
     setIsSleeping(false);
 
-    // Remove all the contacts with this body
-    resetContactManifoldsList();
-
     // Ask the broad-phase to test again the collision shapes of the body for collision
     // detection (as if the body has moved)
     askForBroadPhaseCollisionCheck();
 
     // Reset the force and torque on the body
-    mExternalForce.setToZero();
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
-    mExternalTorque.setToZero();
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
+}
+
+// Get the inverse local inertia tensor of the body (in body coordinates)
+const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
+    return mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+}
+
+// Return the inverse of the inertia tensor in world coordinates.
+/// The inertia tensor I_w in world coordinates is computed with the
+/// local inverse inertia tensor I_b^-1 in body coordinates
+/// by I_w = R * I_b^-1 * R^T
+/// where R is the rotation matrix (and R^T its transpose) of the
+/// current orientation quaternion of the body
+/**
+ * @return The 3x3 inverse inertia tensor matrix of the body in world-space
+ *         coordinates
+ */
+Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
+
+    // Compute and return the inertia tensor in world coordinates
+    return mWorld.mDynamicsComponents.getInertiaTensorWorldInverse(mEntity);
+}
+
+// Method that return the mass of the body
+/**
+ * @return The mass (in kilograms) of the body
+ */
+decimal RigidBody::getMass() const {
+    return mWorld.mDynamicsComponents.getInitMass(mEntity);
+}
+
+// Apply an external force to the body at a given point (in world-space coordinates).
+/// If the point is not at the center of mass of the body, it will also
+/// generate some torque and therefore, change the angular velocity of the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The force to apply on the body
+ * @param point The point where the force is applied (in world-space coordinates)
+ */
+void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force
+    const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+
+    // Add the torque
+    const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+    const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force));
 }
 
 // Set the local inertia tensor of the body (in local-space coordinates)
@@ -136,7 +191,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
     if (mType != BodyType::DYNAMIC) return;
 
     // Compute the inverse local inertia tensor
-    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+    mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -145,6 +200,45 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
              "Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
 }
 
+// Apply an external force to the body at its center of mass.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The external force to apply on the center of mass of the body
+ */
+void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force
+    const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+}
+
+// Return the linear velocity damping factor
+/**
+ * @return The linear damping factor of this body
+ */
+decimal RigidBody::getLinearDamping() const {
+    return mWorld.mDynamicsComponents.getLinearDamping(mEntity);
+}
+
+// Return the angular velocity damping factor
+/**
+ * @return The angular damping factor of this body
+ */
+decimal RigidBody::getAngularDamping() const {
+    return mWorld.mDynamicsComponents.getAngularDamping(mEntity);
+}
+
 // Set the inverse local inertia tensor of the body (in local-space coordinates)
 /// If the inverse inertia tensor is set with this method, it will not be computed
 /// using the collision shapes of the body.
@@ -160,7 +254,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
     if (mType != BodyType::DYNAMIC) return;
 
     // Compute the inverse local inertia tensor
-    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+    mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -178,20 +272,24 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
  */
 void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
 
+    // TODO : Check if we need to update the postion of the body here at the end (transform of the body)
+
     if (mType != BodyType::DYNAMIC) return;
 
     mIsCenterOfMassSetByUser = true;
 
-    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
-    mCenterOfMassLocal = centerOfMassLocal;
+    mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal);
 
     // Compute the center of mass in world-space coordinates
-    mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal;
+    const Vector3& updatedCenterOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+    mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal);
 
     // Update the linear velocity of the center of mass
     Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
     const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
-    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+    linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
     mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -206,14 +304,14 @@ void RigidBody::setMass(decimal mass) {
 
     if (mType != BodyType::DYNAMIC) return;
 
-    mInitMass = mass;
+    mWorld.mDynamicsComponents.setInitMass(mEntity, mass);
 
-    if (mInitMass > decimal(0.0)) {
+    if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
-        mMassInverse = decimal(1.0) / mInitMass;
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
     }
     else {
-        mInitMass = decimal(1.0);
+        mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0));
-        mMassInverse = decimal(1.0);
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0));
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -261,7 +359,8 @@ void RigidBody::updateInertiaTensorInverseWorld() {
     // TODO : Make sure we do this in a system
 
     Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
-    mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
+    const Matrix3x3& inverseInertiaLocalTensor = mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+    mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
 }
 
 // Add a collision shape to the body.
@@ -361,11 +460,11 @@ void RigidBody::removeCollisionShape(ProxyShape* proxyShape) {
  * @param isEnabled True if you want the gravity to be applied to this body
  */
 void RigidBody::enableGravity(bool isEnabled) {
-    mIsGravityEnabled = isEnabled;
+    mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
              "Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
-             (mIsGravityEnabled ? "true" : "false"));
+             (isEnabled ? "true" : "false"));
 }
 
 // Set the linear damping factor. This is the ratio of the linear velocity
@@ -375,10 +474,10 @@ void RigidBody::enableGravity(bool isEnabled) {
  */
 void RigidBody::setLinearDamping(decimal linearDamping) {
     assert(linearDamping >= decimal(0.0));
-    mLinearDamping = 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(mLinearDamping));
+             "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(linearDamping));
 }
 
 // Set the angular damping factor. This is the ratio of the angular velocity
@@ -388,10 +487,10 @@ void RigidBody::setLinearDamping(decimal linearDamping) {
  */
 void RigidBody::setAngularDamping(decimal angularDamping) {
     assert(angularDamping >= decimal(0.0));
-    mAngularDamping = 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(mAngularDamping));
+             "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(angularDamping));
 }
 
 /// Update the transform of the body after a change of the center of mass
@@ -401,7 +500,9 @@ void RigidBody::updateTransformWithCenterOfMass() {
 
     // Translate the body according to the translation of the center of mass position
     Transform& transform = mWorld.mTransformComponents.getTransform(mEntity);
-    transform.setPosition(mCenterOfMassWorld - transform.getOrientation() * mCenterOfMassLocal);
+    const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+    const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+    transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
 }
 
 // Set a new material for this rigid body
@@ -466,15 +567,17 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
  */
 void RigidBody::setTransform(const Transform& transform) {
 
-    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
 
     // Compute the new center of mass in world-space coordinates
-    mCenterOfMassWorld = transform * mCenterOfMassLocal;
+    const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+    mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal);
 
     // Update the linear velocity of the center of mass
     Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
     const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
-    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+    linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
     mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     CollisionBody::setTransform(transform);
@@ -490,11 +593,11 @@ void RigidBody::setTransform(const Transform& transform) {
 // the collision shapes attached to the body.
 void RigidBody::recomputeMassInformation() {
 
-    mInitMass = decimal(0.0);
+    mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
-    mMassInverse = decimal(0.0);
+    mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0));
-    if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
+    if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
-    if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
+    if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
-    if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
+    if (!mIsCenterOfMassSetByUser) mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, Vector3::zero());
     Matrix3x3 inertiaTensorLocal;
     inertiaTensorLocal.setToZero();
 
@@ -502,7 +605,7 @@ void RigidBody::recomputeMassInformation() {
 
     // If it is a STATIC or a KINEMATIC body
     if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
-        mCenterOfMassWorld = transform.getPosition();
+        mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
         return;
     }
 
@@ -512,29 +615,30 @@ void RigidBody::recomputeMassInformation() {
     const List<Entity>& proxyShapesEntities = mWorld.mBodyComponents.getProxyShapes(mEntity);
     for (uint i=0; i < proxyShapesEntities.size(); i++) {
         ProxyShape* proxyShape = mWorld.mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]);
-        mInitMass += proxyShape->getMass();
+        mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass());
 
         if (!mIsCenterOfMassSetByUser) {
-            mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass();
+            mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) +
+                                                            proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass());
         }
     }
 
-    if (mInitMass > decimal(0.0)) {
+    if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
-        mMassInverse = decimal(1.0) / mInitMass;
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
     }
     else {
-        mCenterOfMassWorld = transform.getPosition();
+        mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
         return;
     }
 
     // Compute the center of mass
-    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
 
     if (!mIsCenterOfMassSetByUser) {
-        mCenterOfMassLocal *= mMassInverse;
+        mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) * mWorld.mDynamicsComponents.getMassInverse(mEntity));
     }
 
-    mCenterOfMassWorld = transform * mCenterOfMassLocal;
+    mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity));
 
     if (!mIsInertiaTensorSetByUser) {
 
@@ -555,7 +659,7 @@ void RigidBody::recomputeMassInformation() {
 
             // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
             // center into a inertia tensor w.r.t to the body origin.
-            Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
+            Vector3 offset = shapeTransform.getPosition() - mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
             decimal offsetSquare = offset.lengthSquare();
             Matrix3x3 offsetMatrix;
             offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
@@ -570,7 +674,7 @@ void RigidBody::recomputeMassInformation() {
         }
 
         // Compute the local inverse inertia tensor
-        mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
     }
 
     // Update the world inverse inertia tensor
@@ -579,7 +683,7 @@ void RigidBody::recomputeMassInformation() {
     // Update the linear velocity of the center of mass
     Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
     Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
-    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass);
     mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 }
 
@@ -599,16 +703,48 @@ Vector3 RigidBody::getAngularVelocity() const {
     return mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
 }
 
+// Return true if the gravity needs to be applied to this rigid body
+/**
+ * @return True if the gravity is applied to the body
+ */
+bool RigidBody::isGravityEnabled() const {
+    return mWorld.mDynamicsComponents.getIsGravityEnabled(mEntity);
+}
+
+// Apply an external torque to the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied torques and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param torque The external torque to apply on the body
+ */
+void RigidBody::applyTorque(const Vector3& torque) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the torque
+    const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + torque);
+}
+
 // Set the variable to know whether or not the body is sleeping
 void RigidBody::setIsSleeping(bool isSleeping) {
 
     CollisionBody::setIsSleeping(isSleeping);
 
     if (isSleeping) {
+
         mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
         mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
-        mExternalForce.setToZero();
+        mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
-        mExternalTorque.setToZero();
+        mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
     }
 }
 

src/body/RigidBody.h

@@ -50,62 +50,17 @@ class MemoryManager;
   */
 class RigidBody : public CollisionBody {
 
-    private :
-
-        /// Index of the body in arrays for contact/constraint solver
-        uint mArrayIndex;
-
     protected :
 
         // -------------------- Attributes -------------------- //
 
-        /// Intial mass of the body
-        decimal mInitMass;
-
-        /// Center of mass of the body in local-space coordinates.
-        /// The center of mass can therefore be different from the body origin
-        Vector3 mCenterOfMassLocal;
-
-        /// Center of mass of the body in world-space coordinates
-        Vector3 mCenterOfMassWorld;
-
-        /// Linear velocity of the body
-        //Vector3 mLinearVelocity;
-
-        /// Angular velocity of the body
-        //Vector3 mAngularVelocity;
-
-        /// Current external force on the body
-        Vector3 mExternalForce;
-
-        /// Current external torque on the body
-        Vector3 mExternalTorque;
-
         /// Inverse Local inertia tensor of the body (in local-space) set
         /// by the user with respect to the center of mass of the body
         Matrix3x3 mUserInertiaTensorLocalInverse;
 
-        /// Inverse of the inertia tensor of the body
-        Matrix3x3 mInertiaTensorLocalInverse;
-
-        /// Inverse of the world inertia tensor of the body
-        Matrix3x3 mInertiaTensorInverseWorld;
-
-        /// Inverse of the mass of the body
-        decimal mMassInverse;
-
-        /// True if the gravity needs to be applied to this rigid body
-        bool mIsGravityEnabled;
-
         /// Material properties of the rigid body
         Material mMaterial;
 
-        /// Linear velocity damping factor
-        decimal mLinearDamping;
-
-        /// Angular velocity damping factor
-        decimal mAngularDamping;
-
         /// First element of the linked list of joints involving this body
         JointListElement* mJointsList;
 
@@ -252,43 +207,6 @@ class RigidBody : public CollisionBody {
         friend class FixedJoint;
 };
 
-// Method that return the mass of the body
-/**
- * @return The mass (in kilograms) of the body
- */
-inline decimal RigidBody::getMass() const {
-    return mInitMass;
-}
-
-// Get the inverse local inertia tensor of the body (in body coordinates)
-inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
-    return mInertiaTensorLocalInverse;
-}
-
-// Return the inverse of the inertia tensor in world coordinates.
-/// The inertia tensor I_w in world coordinates is computed with the
-/// local inverse inertia tensor I_b^-1 in body coordinates
-/// by I_w = R * I_b^-1 * R^T
-/// where R is the rotation matrix (and R^T its transpose) of the
-/// current orientation quaternion of the body
-/**
- * @return The 3x3 inverse inertia tensor matrix of the body in world-space
- *         coordinates
- */
-inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
-
-    // Compute and return the inertia tensor in world coordinates
-    return mInertiaTensorInverseWorld;
-}
-
-// Return true if the gravity needs to be applied to this rigid body
-/**
- * @return True if the gravity is applied to the body
- */
-inline bool RigidBody::isGravityEnabled() const {
-    return mIsGravityEnabled;
-}
-
 // Return a reference to the material properties of the rigid body
 /**
  * @return A reference to the material of the body
@@ -297,22 +215,6 @@ inline Material& RigidBody::getMaterial() {
     return mMaterial;
 }
 
-// Return the linear velocity damping factor
-/**
- * @return The linear damping factor of this body
- */
-inline decimal RigidBody::getLinearDamping() const {
-    return mLinearDamping;
-}
-
-// Return the angular velocity damping factor
-/**
- * @return The angular damping factor of this body
- */
-inline decimal RigidBody::getAngularDamping() const {
-    return mAngularDamping;
-}
-
 // Return the first element of the linked list of joints involving this body
 /**
  * @return The first element of the linked-list of all the joints involving this body
@@ -329,76 +231,6 @@ inline JointListElement* RigidBody::getJointsList() {
     return mJointsList;
 }
 
-// Apply an external force to the body at its center of mass.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied forces and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The external force to apply on the center of mass of the body
- */
-inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the force
-    mExternalForce += force;
-}
-
-// Apply an external force to the body at a given point (in world-space coordinates).
-/// If the point is not at the center of mass of the body, it will also
-/// generate some torque and therefore, change the angular velocity of the body.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied forces and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The force to apply on the body
- * @param point The point where the force is applied (in world-space coordinates)
- */
-inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the force and torque
-    mExternalForce += force;
-    mExternalTorque += (point - mCenterOfMassWorld).cross(force);
-}
-
-// Apply an external torque to the body.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied torques and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param torque The external torque to apply on the body
- */
-inline void RigidBody::applyTorque(const Vector3& torque) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the torque
-    mExternalTorque += torque;
-}
-
 }
 
  #endif

src/collision/CollisionCallback.cpp

@@ -25,59 +25,71 @@
 
 // Libraries
 #include "collision/CollisionCallback.h"
-#include "engine/OverlappingPair.h"
+#include "collision/ContactPair.h"
-#include "memory/MemoryAllocator.h"
+#include "constraint/ContactPoint.h"
-#include "collision/ContactManifold.h"
+#include "engine/CollisionWorld.h"
-#include "memory/MemoryManager.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
 // Constructor
-CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) :
+CollisionCallback::ContactPoint::ContactPoint(const reactphysics3d::ContactPoint& contactPoint) : mContactPoint(contactPoint) {
-    contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
-    body2(pair->getShape2()->getBody()),
-    proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
-    mMemoryManager(memoryManager) {
 
-    assert(pair != nullptr);
-
-    const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
-
-    // For each contact manifold in the set of manifolds in the pair
-    ContactManifold* contactManifold = manifoldSet.getContactManifolds();
-	assert(contactManifold != nullptr);
-    while (contactManifold != nullptr) {
-
-        assert(contactManifold->getNbContactPoints() > 0);
-
-        // Add the contact manifold at the beginning of the linked
-        // list of contact manifolds of the first body
-        ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
-                                                                           sizeof(ContactManifoldListElement)))
-                                                      ContactManifoldListElement(contactManifold,
-                                                                         contactManifoldElements);
-        contactManifoldElements = element;
-
-        contactManifold = contactManifold->getNext();
-    }
 }
 
-// Destructor
+// Contact Pair Constructor
-CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
+CollisionCallback::ContactPair::ContactPair(const reactphysics3d::ContactPair& contactPair,
+                                            List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world)
+                               :mContactPair(contactPair), mContactPoints(contactPoints),
+                                mWorld(world) {
 
-    // Release memory allocator for the contact manifold list elements
-    ContactManifoldListElement* element = contactManifoldElements;
-    while (element != nullptr) {
-
-        ContactManifoldListElement* nextElement = element->getNext();
-
-        // Delete and release memory
-        element->~ContactManifoldListElement();
-        mMemoryManager.release(MemoryManager::AllocationType::Pool, element,
-                               sizeof(ContactManifoldListElement));
-
-        element = nextElement;
-    }
 }
 
+// Return a pointer to the first body in contact
+CollisionBody* CollisionCallback::ContactPair::getBody1() const {
+    return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mContactPair.body1Entity));
+}
+
+// Return a pointer to the second body in contact
+CollisionBody* CollisionCallback::ContactPair::getBody2() const {
+    return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mContactPair.body2Entity));
+}
+
+// Return a pointer to the first proxy-shape in contact (in body 1)
+ProxyShape* CollisionCallback::ContactPair::getProxyShape1() const {
+    return mWorld.mProxyShapesComponents.getProxyShape(mContactPair.proxyShape1Entity);
+}
+
+// Return a pointer to the second proxy-shape in contact (in body 1)
+ProxyShape* CollisionCallback::ContactPair::getProxyShape2() const {
+    return mWorld.mProxyShapesComponents.getProxyShape(mContactPair.proxyShape2Entity);
+}
+
+// CollisionCallbackInfo Constructor
+CollisionCallback::CallbackData::CallbackData(List<reactphysics3d::ContactPair>* contactPairs, List<ContactManifold>* manifolds,
+                                                                List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world)
+                      :mContactPairs(contactPairs), mContactManifolds(manifolds), mContactPoints(contactPoints), mWorld(world) {
+
+}
+
+// Return a given contact point of the contact pair
+/// Note that the returned ContactPoint object is only valid during the call of the CollisionCallback::onContact()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the ContactPoint
+/// object itself because it won't be valid after the CollisionCallback::onContact() call.
+CollisionCallback::ContactPoint CollisionCallback::ContactPair::getContactPoint(uint index) const {
+
+    assert(index < getNbContactPoints());
+
+    return CollisionCallback::ContactPoint((*mContactPoints)[mContactPair.contactPointsIndex + index]);
+}
+
+// Return a given contact pair
+/// Note that the returned ContactPair object is only valid during the call of the CollisionCallback::onContact()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the ContactPair
+/// object itself because it won't be valid after the CollisionCallback::onContact() call.
+CollisionCallback::ContactPair CollisionCallback::CallbackData::getContactPair(uint index) const {
+
+    assert(index < getNbContactPairs());
+
+    return CollisionCallback::ContactPair((*mContactPairs)[index], mContactPoints, mWorld);
+}

src/collision/CollisionCallback.h

@@ -26,6 +26,11 @@
 #ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H
 #define REACTPHYSICS3D_COLLISION_CALLBACK_H
 
+// Libraries
+#include "containers/List.h"
+#include "collision/ContactPair.h"
+#include "constraint/ContactPoint.h"
+
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
@@ -36,10 +41,11 @@ struct ContactManifoldListElement;
 class CollisionBody;
 class ProxyShape;
 class MemoryManager;
+class CollisionCallbackInfo;
 
 // Class CollisionCallback
 /**
- * This class can be used to register a callback for collision test queries.
+ * This abstract class can be used to register a callback for collision test queries.
  * You should implement your own class inherited from this one and implement
  * the notifyContact() method. This method will called each time a contact
  * point is reported.
@@ -48,47 +54,221 @@ class CollisionCallback {
 
     public:
 
-        // Structure CollisionCallbackInfo
+        // Class ContactPoint
         /**
-         * This structure represents the contact information between two collision
+         * This class represents a contact point between two bodies of the physics world.
-         * shapes of two bodies
          */
-        struct CollisionCallbackInfo {
+        class ContactPoint {
+
+            private:
+
+                // -------------------- Attributes -------------------- //
+
+                const reactphysics3d::ContactPoint& mContactPoint;
+
+                // -------------------- Methods -------------------- //
+
+                /// Constructor
+                ContactPoint(const reactphysics3d::ContactPoint& contactPoint);
 
             public:
 
-                /// Pointer to the first element of the linked-list that contains contact manifolds
+                // -------------------- Methods -------------------- //
-                ContactManifoldListElement* contactManifoldElements;
 
-                /// Pointer to the first body of the contact
+                /// Copy constructor
-                CollisionBody* body1;
+                ContactPoint(const ContactPoint& contactPoint) = default;
 
-                /// Pointer to the second body of the contact
+                /// Assignment operator
-                CollisionBody* body2;
+                ContactPoint& operator=(const ContactPoint& contactPoint) = default;
 
-                /// Pointer to the proxy shape of first body
+                /// Destructor
-                const ProxyShape* proxyShape1;
+                ~ContactPoint() = default;
 
-                /// Pointer to the proxy shape of second body
+                /// Return the penetration depth
-                const ProxyShape* proxyShape2;
+                decimal getPenetrationDepth() const;
 
-                /// Memory manager
+                /// Return the world-space contact normal
-                MemoryManager& mMemoryManager;
+                const Vector3& getWorldNormal() const;
 
-                // Constructor
+                /// Return the contact point on the first proxy shape in the local-space of the first proxy shape
-                CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager);
+                const Vector3& getLocalPointOnShape1() const;
 
-                // Destructor
+                /// Return the contact point on the second proxy shape in the local-space of the second proxy shape
-                ~CollisionCallbackInfo();
+                const Vector3& getLocalPointOnShape2() const;
+
+                // -------------------- Friendship -------------------- //
+
+                friend class CollisionCallback;
+        };
+
+        // Class ContactPair
+        /**
+         * This class represents the contact between two bodies of the physics world.
+         * A contact pair contains a list of contact points.
+         */
+        class ContactPair {
+
+            private:
+
+                // -------------------- Attributes -------------------- //
+
+                const reactphysics3d::ContactPair& mContactPair;
+
+                /// Pointer to the contact points
+                List<reactphysics3d::ContactPoint>* mContactPoints;
+
+                /// Reference to the physics world
+                CollisionWorld& mWorld;
+
+                // -------------------- Methods -------------------- //
+
+                /// Constructor
+                ContactPair(const reactphysics3d::ContactPair& contactPair,  List<reactphysics3d::ContactPoint>* contactPoints,
+                            CollisionWorld& world);
+
+            public:
+
+                // -------------------- Methods -------------------- //
+
+                /// Copy constructor
+                ContactPair(const ContactPair& contactPair) = default;
+
+                /// Assignment operator
+                ContactPair& operator=(const ContactPair& contactPair) = default;
+
+                /// Destructor
+                ~ContactPair() = default;
+
+                /// Return the number of contact points in the contact pair
+                uint getNbContactPoints() const;
+
+                /// Return a given contact point
+                ContactPoint getContactPoint(uint index) const;
+
+                /// Return a pointer to the first body in contact
+                CollisionBody* getBody1() const;
+
+                /// Return a pointer to the second body in contact
+                CollisionBody* getBody2() const;
+
+                /// Return a pointer to the first proxy-shape in contact (in body 1)
+                ProxyShape* getProxyShape1() const;
+
+                /// Return a pointer to the second proxy-shape in contact (in body 2)
+                ProxyShape* getProxyShape2() const;
+
+                // -------------------- Friendship -------------------- //
+
+                friend class CollisionCallback;
+        };
+
+        // Class CallbackData
+        /**
+         * This class contains data about contacts between bodies
+         */
+        class CallbackData {
+
+            private:
+
+                // -------------------- Attributes -------------------- //
+
+                /// Pointer to the list of contact pairs
+                List<reactphysics3d::ContactPair>* mContactPairs;
+
+                /// Pointer to the list of contact manifolds
+                List<ContactManifold>* mContactManifolds;
+
+                /// Pointer to the contact points
+                List<reactphysics3d::ContactPoint>* mContactPoints;
+
+                /// Reference to the physics world
+                CollisionWorld& mWorld;
+
+                // -------------------- Methods -------------------- //
+
+                /// Constructor
+                CallbackData(List<reactphysics3d::ContactPair>* contactPairs, List<ContactManifold>* manifolds,
+                             List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world);
+
+                /// Deleted copy constructor
+                CallbackData(const CallbackData& callbackData) = delete;
+
+                /// Deleted assignment operator
+                CallbackData& operator=(const CallbackData& callbackData) = delete;
+
+                /// Destructor
+                ~CallbackData() = default;
+
+            public:
+
+                // -------------------- Methods -------------------- //
+
+                /// Return the number of contact pairs
+                uint getNbContactPairs() const;
+
+                /// Return a given contact pair
+                ContactPair getContactPair(uint index) const;
+
+                // -------------------- Friendship -------------------- //
+
+                friend class CollisionDetection;
         };
 
         /// Destructor
         virtual ~CollisionCallback() = default;
 
-        /// This method will be called for each reported contact point
+        /// This method is called when some contacts occur
-        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+        virtual void onContact(const CallbackData& callbackData)=0;
 };
 
+// Return the number of contact pairs (there is a single contact pair between two bodies in contact)
+/**
+ * @return The number of contact pairs
+ */
+inline uint CollisionCallback::CallbackData::getNbContactPairs() const {
+    return mContactPairs->size();
+}
+
+// Return the number of contact points in the contact pair
+/**
+ * @return The number of contact points
+ */
+inline uint CollisionCallback::ContactPair::getNbContactPoints() const {
+   return mContactPair.nbToTalContactPoints;
+}
+
+// Return the penetration depth between the two bodies in contact
+/**
+ * @return The penetration depth (larger than zero)
+ */
+inline decimal CollisionCallback::ContactPoint::getPenetrationDepth() const {
+   return mContactPoint.getPenetrationDepth();
+}
+
+// Return the world-space contact normal (vector from first body toward second body)
+/**
+ * @return The contact normal direction at the contact point (in world-space)
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getWorldNormal() const {
+   return mContactPoint.getNormal();
+}
+
+// Return the contact point on the first proxy shape in the local-space of the first proxy shape
+/**
+ * @return The contact point in the local-space of the first proxy-shape (from body1) in contact
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnShape1() const {
+   return mContactPoint.getLocalPointOnShape1();
+}
+
+// Return the contact point on the second proxy shape in the local-space of the second proxy shape
+/**
+ * @return The contact point in the local-space of the second proxy-shape (from body2) in contact
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnShape2() const {
+   return mContactPoint.getLocalPointOnShape2();
+}
+
 }
 
 #endif

src/collision/CollisionDetection.h

@@ -30,6 +30,11 @@
 #include "body/CollisionBody.h"
 #include "systems/BroadPhaseSystem.h"
 #include "collision/shapes/CollisionShape.h"
+#include "collision/ContactPointInfo.h"
+#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
+#include "collision/ContactManifold.h"
+#include "collision/ContactPair.h"
 #include "engine/OverlappingPair.h"
 #include "collision/narrowphase/NarrowPhaseInput.h"
 #include "collision/narrowphase/CollisionDispatch.h"
@@ -64,6 +69,11 @@ class CollisionDetection {
 
         using OverlappingPairMap = Map<Pair<uint, uint>, OverlappingPair*>;
 
+        // -------------------- Constants -------------------- //
+
+        /// Maximum number of contact points in a reduced contact manifold
+        const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
+
         // -------------------- Attributes -------------------- //
 
         /// Memory manager
@@ -72,9 +82,6 @@ class CollisionDetection {
         /// Reference the proxy-shape components
         ProxyShapeComponents& mProxyShapesComponents;
 
-        /// Reference the transform components
-        TransformComponents& mTransformComponents;
-
         /// Collision Detection Dispatch configuration
         CollisionDispatch mCollisionDispatch;
 
@@ -96,6 +103,71 @@ class CollisionDetection {
         /// Narrow-phase collision detection input
         NarrowPhaseInput mNarrowPhaseInput;
 
+        /// List of the potential contact points
+        List<ContactPointInfo> mPotentialContactPoints;
+
+        /// List of the potential contact manifolds
+        List<ContactManifoldInfo> mPotentialContactManifolds;
+
+        /// First list of narrow-phase pair contacts
+        List<ContactPair> mContactPairs1;
+
+        /// Second list of narrow-phase pair contacts
+        List<ContactPair> mContactPairs2;
+
+        /// Pointer to the list of contact pairs of the previous frame (either mContactPairs1 or mContactPairs2)
+        List<ContactPair>* mPreviousContactPairs;
+
+        /// Pointer to the list of contact pairs of the current frame (either mContactPairs1 or mContactPairs2)
+        List<ContactPair>* mCurrentContactPairs;
+
+        /// First map of overlapping pair id to the index of the corresponding pair contact
+        Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex1;
+
+        /// Second map of overlapping pair id to the index of the corresponding pair contact
+        Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex2;
+
+        /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame
+        /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
+        Map<OverlappingPair::OverlappingPairId, uint>* mPreviousMapPairIdToContactPairIndex;
+
+        /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame
+        /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
+        Map<OverlappingPair::OverlappingPairId, uint>* mCurrentMapPairIdToContactPairIndex;
+
+        /// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
+        /// same islands packed together linearly and contact pairs that are not part of islands at the end.
+        /// This is used when we create contact manifolds and contact points so that there are also packed
+        /// together linearly if they are part of the same island.
+        List<uint> mContactPairsIndicesOrderingForContacts;
+
+        /// First list with the contact manifolds
+        List<ContactManifold> mContactManifolds1;
+
+        /// Second list with the contact manifolds
+        List<ContactManifold> mContactManifolds2;
+
+        /// Pointer to the list of contact manifolds from the previous frame (either mContactManifolds1 or mContactManifolds2)
+        List<ContactManifold>* mPreviousContactManifolds;
+
+        /// Pointer to the list of contact manifolds from the current frame (either mContactManifolds1 or mContactManifolds2)
+        List<ContactManifold>* mCurrentContactManifolds;
+
+        /// Second list of contact points (contact points from either the current frame of the previous frame)
+        List<ContactPoint> mContactPoints1;
+
+        /// Second list of contact points (contact points from either the current frame of the previous frame)
+        List<ContactPoint> mContactPoints2;
+
+        /// Pointer to the contact points of the previous frame (either mContactPoints1 or mContactPoints2)
+        List<ContactPoint>* mPreviousContactPoints;
+
+        /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2)
+        List<ContactPoint>* mCurrentContactPoints;
+
+        /// Map a body entity to the list of contact pairs in which it is involved
+        Map<Entity, List<uint>> mMapBodyToContactPairs;
+
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -109,48 +181,89 @@ class CollisionDetection {
         void computeBroadPhase();
 
         /// Compute the middle-phase collision detection
-        void computeMiddlePhase();
+        void computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput);
 
         /// Compute the narrow-phase collision detection
         void computeNarrowPhase();
 
+        /// Compute the narrow-phase collision detection for the testOverlap() methods.
+        bool computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback);
+
+        /// Compute the narrow-phase collision detection for the testCollision() methods
+        bool computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& narrowPhaseInput, CollisionCallback& callback);
+
+        /// Process the potential contacts after narrow-phase collision detection
+        void computeSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, List<Pair<Entity, Entity>>& overlapPairs) const;
+
+        /// Convert the potential contact into actual contacts
+        void computeSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List<Pair<Entity, Entity>>& overlapPairs) const;
+
         /// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
-        void updateOverlappingPairs(List<Pair<int, int> >& overlappingNodes);
+        void updateOverlappingPairs(const List<Pair<int, int>>& overlappingNodes);
+
+        /// Remove pairs that are not overlapping anymore
+        void removeNonOverlappingPairs();
 
         /// Execute the narrow-phase collision detection algorithm on batches
-        bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound,
+        bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator);
-                                      bool reportContacts, MemoryAllocator& allocator);
-
-        /// Add a contact manifold to the linked list of contact manifolds of the two bodies
-        /// involved in the corresponding contact.
-        void addContactManifoldToBody(OverlappingPair* pair);
-
-        /// Add all the contact manifold of colliding pairs to their bodies
-        void addAllContactManifoldsToBodies();
 
         /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
         void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
                                                NarrowPhaseInput& narrowPhaseInput);
 
-        /// Compute the middle-phase collision detection between two proxy shapes
+        /// Swap the previous and current contacts lists
-        void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput);
+        void swapPreviousAndCurrentContacts();
 
         /// Convert the potential contact into actual contacts
         void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
-                                      bool updateLastFrameInfo);
+                                      bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
+                                      Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                      List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs,
+                                      Map<Entity, List<uint>>& mapBodyToContactPairs);
 
         /// Process the potential contacts after narrow-phase collision detection
-        void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo);
+        void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
+                                         Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                         List<ContactManifoldInfo> &potentialContactManifolds, List<ContactPair>* contactPairs,
+                                         Map<Entity, List<uint>>& mapBodyToContactPairs);
 
-        /// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
+        /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
-        void reduceContactManifolds(const OverlappingPairMap& overlappingPairs);
+        void reducePotentialContactManifolds(List<ContactPair>* contactPairs, List<ContactManifoldInfo>& potentialContactManifolds,
+                                             const List<ContactPointInfo>& potentialContactPoints) const;
 
-        /// Report contacts for all the colliding overlapping pairs
+        /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
-        void reportAllContacts();
+        void createContacts();
+
+        /// Create the actual contact manifolds and contacts points for testCollision() methods
+        void createSnapshotContacts(List<ContactPair>& contactPairs, List<ContactManifold> &contactManifolds,
+                                    List<ContactPoint>& contactPoints,
+                                    List<ContactManifoldInfo>& potentialContactManifolds,
+                                    List<ContactPointInfo>& potentialContactPoints);
+
+        /// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+        void initContactsWithPreviousOnes();
+
+        /// Reduce the number of contact points of a potential contact manifold
+        void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
+                                 const List<ContactPointInfo>& potentialContactPoints) const;
+
+        /// Report contacts
+        void reportContacts(CollisionCallback& callback, List<ContactPair>* contactPairs,
+                            List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints);
+
+        /// Return the largest depth of all the contact points of a potential manifold
+        decimal computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
+                                                            const List<ContactPointInfo>& potentialContactPoints) const;
 
         /// Process the potential contacts where one collion is a concave shape
         void processSmoothMeshContacts(OverlappingPair* pair);
 
+        /// Filter the overlapping pairs to keep only the pairs where a given body is involved
+        void filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const;
+
+        /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
+        void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -193,6 +306,9 @@ class CollisionDetection {
         /// Ask for a collision shape to be tested again during broad-phase.
         void askForBroadPhaseCollisionCheck(ProxyShape* shape);
 
+        /// Report contacts
+        void reportContacts();
+
         /// Compute the collision detection
         void computeCollisionDetection();
 
@@ -200,23 +316,23 @@ class CollisionDetection {
         void raycast(RaycastCallback* raycastCallback, const Ray& ray,
                      unsigned short raycastWithCategoryMaskBits) const;
 
-        /// Report all the bodies that overlap with the aabb in parameter
+        /// Return true if two bodies (collide) overlap
-        void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-
-        /// Return true if two bodies overlap
         bool testOverlap(CollisionBody* body1, CollisionBody* body2);
 
-        /// Report all the bodies that overlap with the body in parameter
+        /// Report all the bodies that overlap (collide) with the body in parameter
-        void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
+        void testOverlap(CollisionBody* body, OverlapCallback& callback);
 
-        /// Test and report collisions between two bodies
+        /// Report all the bodies that overlap (collide) in the world
-        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+        void testOverlap(OverlapCallback& overlapCallback);
 
-        /// Test and report collisions between a body and all the others bodies of the world
+        /// Test collision and report contacts between two bodies.
-        void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
+        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback);
 
-        /// Test and report collisions between all shapes of the world
+        /// Test collision and report all the contacts involving the body in parameter
-        void testCollision(CollisionCallback* callback);
+        void testCollision(CollisionBody* body, CollisionCallback& callback);
+
+        /// Test collision and report contacts between each colliding bodies in the world
+        void testCollision(CollisionCallback& callback);
 
         /// Return a reference to the memory manager
         MemoryManager& getMemoryManager() const;

src/collision/ContactManifold.cpp

@@ -26,364 +26,23 @@
 // Libraries
 #include "ContactManifold.h"
 #include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
 
 using namespace reactphysics3d;
 
 // Constructor
-ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
+ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
-                                 MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
+                                 uint contactPointsIndex, int8 nbContactPoints)
-                : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
+                :mContactPointsIndex(0), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
-                  mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
+                 proxyShapeEntity1(proxyShapeEntity1), proxyShapeEntity2(proxyShapeEntity2), mFrictionImpulse1(0), mFrictionImpulse2(0),
-                  mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
+                 mFrictionTwistImpulse(0), mIsAlreadyInIsland(false) {
-                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
-                  mWorldSettings(worldSettings) {
 
+
+    mContactPointsIndex = contactPointsIndex;
+    mNbContactPoints = nbContactPoints;
 }
 
 // Destructor
 ContactManifold::~ContactManifold() {
 
-    // Delete all the contact points
-    ContactPoint* contactPoint = mContactPoints;
-    while(contactPoint != nullptr) {
-
-        ContactPoint* nextContactPoint = contactPoint->getNext();
-
-        // Delete the contact point
-        contactPoint->~ContactPoint();
-        mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
-
-        contactPoint = nextContactPoint;
-    }
-}
-
-// Remove a contact point
-void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
-
-    assert(mNbContactPoints > 0);
-    assert(mContactPoints != nullptr);
-    assert(contactPoint != nullptr);
-
-    ContactPoint* previous = contactPoint->getPrevious();
-    ContactPoint* next = contactPoint->getNext();
-
-    if (previous != nullptr) {
-        previous->setNext(next);
-    }
-    else {
-        mContactPoints = next;
-    }
-
-    if (next != nullptr) {
-        next->setPrevious(previous);
-    }
-
-    // Delete the contact point
-    contactPoint->~ContactPoint();
-    mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
-
-    mNbContactPoints--;
-    assert(mNbContactPoints >= 0);
-}
-
-// Return the largest depth of all the contact points
-decimal ContactManifold::getLargestContactDepth() const {
-    decimal largestDepth = 0.0f;
-
-    assert(mNbContactPoints > 0);
-
-    ContactPoint* contactPoint = mContactPoints;
-    while(contactPoint != nullptr){
-        decimal depth = contactPoint->getPenetrationDepth();
-        if (depth > largestDepth) {
-            largestDepth = depth;
-        }
-
-        contactPoint = contactPoint->getNext();
-    }
-
-    return largestDepth;
-}
-
-// Add a contact point
-void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
-
-       // For each contact point in the manifold
-       bool isSimilarPointFound = false;
-       ContactPoint* oldContactPoint = mContactPoints;
-       while (oldContactPoint != nullptr) {
-
-           assert(oldContactPoint != nullptr);
-
-            // If the new contact point is similar (very close) to the old contact point
-            if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
-
-                // Replace (update) the old contact point with the new one
-                oldContactPoint->update(contactPointInfo);
-                isSimilarPointFound = true;
-                break;
-            }
-
-            oldContactPoint = oldContactPoint->getNext();
-       }
-
-       // If we have not found a similar contact point
-       if (!isSimilarPointFound) {
-
-            // Create the new contact point
-            ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mWorldSettings);
-
-            // Add the new contact point into the manifold
-            contactPoint->setNext(mContactPoints);
-            contactPoint->setPrevious(nullptr);
-            if (mContactPoints != nullptr) {
-                mContactPoints->setPrevious(contactPoint);
-            }
-
-            mContactPoints = contactPoint;
-
-            mNbContactPoints++;
-       }
-
-    // The old manifold is no longer obsolete
-    mIsObsolete = false;
-}
-
-// Set to true to make the manifold obsolete
-void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) {
-    mIsObsolete = isObsolete;
-
-    if (setContactPoints) {
-        ContactPoint* contactPoint = mContactPoints;
-        while (contactPoint != nullptr) {
-            contactPoint->setIsObsolete(isObsolete);
-
-            contactPoint = contactPoint->getNext();
-        }
-    }
-}
-
-// Clear the obsolete contact points
-void ContactManifold::clearObsoleteContactPoints() {
-
-    assert(mContactPoints != nullptr);
-
-    // For each contact point of the manifold
-    ContactPoint* contactPoint = mContactPoints;
-    while (contactPoint != nullptr) {
-
-        ContactPoint* nextContactPoint =  contactPoint->getNext();
-
-        // If the contact point is obsolete
-        if (contactPoint->getIsObsolete()) {
-
-            // Remove the contact point
-            removeContactPoint(contactPoint);
-        }
-
-        contactPoint = nextContactPoint;
-    }
-
-    assert(mNbContactPoints > 0);
-    assert(mContactPoints != nullptr);
-}
-
-// Reduce the number of contact points of the currently computed manifold
-// This is based on the technique described by Dirk Gregorius in his
-// "Contacts Creation" GDC presentation. This method will reduce the number of
-// contact points to a maximum of 4 points (but it can be less).
-void ContactManifold::reduce(const Transform& shape1ToWorldTransform) {
-
-    assert(mContactPoints != nullptr);
-
-    // The following algorithm only works to reduce to a maximum of 4 contact points
-    assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
-
-    // If there are too many contact points in the manifold
-    if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
-
-        uint nbReducedPoints = 0;
-
-        ContactPoint* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
-        for (int i=0; i<MAX_CONTACT_POINTS_IN_MANIFOLD; i++) {
-            pointsToKeep[i] = nullptr;
-        }
-
-        //  Compute the initial contact point we need to keep.
-        // The first point we keep is always the point in a given
-        // constant direction (in order to always have same contact points
-        // between frames for better stability)
-
-        const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
-
-        // Compute the contact normal of the manifold (we use the first contact point)
-        // in the local-space of the first collision shape
-        const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPoints->getNormal();
-
-        // Compute a search direction
-        const Vector3 searchDirection(1, 1, 1);
-        ContactPoint* element = mContactPoints;
-        pointsToKeep[0] = element;
-        decimal maxDotProduct = searchDirection.dot(element->getLocalPointOnShape1());
-        element = element->getNext();
-        nbReducedPoints = 1;
-        while(element != nullptr) {
-
-            decimal dotProduct = searchDirection.dot(element->getLocalPointOnShape1());
-            if (dotProduct > maxDotProduct) {
-                maxDotProduct = dotProduct;
-                pointsToKeep[0] = element;
-            }
-            element = element->getNext();
-        }
-        assert(pointsToKeep[0] != nullptr);
-        assert(nbReducedPoints == 1);
-
-        // Compute the second contact point we need to keep.
-        // The second point we keep is the one farthest away from the first point.
-
-        decimal maxDistance = decimal(0.0);
-        element = mContactPoints;
-        while(element != nullptr) {
-
-            if (element == pointsToKeep[0]) {
-                element = element->getNext();
-                continue;
-            }
-
-            decimal distance = (pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1()).lengthSquare();
-            if (distance >= maxDistance) {
-                maxDistance = distance;
-                pointsToKeep[1] = element;
-                nbReducedPoints = 2;
-            }
-            element = element->getNext();
-        }
-        assert(pointsToKeep[1] != nullptr);
-        assert(nbReducedPoints == 2);
-
-        // Compute the third contact point we need to keep.
-        // The second point is the one producing the triangle with the larger area
-        // with first and second point.
-
-        // We compute the most positive or most negative triangle area (depending on winding)
-        ContactPoint* thirdPointMaxArea = nullptr;
-        ContactPoint* thirdPointMinArea = nullptr;
-        decimal minArea = decimal(0.0);
-        decimal maxArea = decimal(0.0);
-        bool isPreviousAreaPositive = true;
-        element = mContactPoints;
-        while(element != nullptr) {
-
-            if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
-                element = element->getNext();
-                continue;
-            }
-
-            const Vector3 newToFirst = pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-            const Vector3 newToSecond = pointsToKeep[1]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-
-            // Compute the triangle area
-            decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
-
-            if (area >= maxArea) {
-                maxArea = area;
-                thirdPointMaxArea = element;
-            }
-            if (area <= minArea) {
-                minArea = area;
-                thirdPointMinArea = element;
-            }
-            element = element->getNext();
-        }
-        assert(minArea <= decimal(0.0));
-        assert(maxArea >= decimal(0.0));
-        if (maxArea > (-minArea)) {
-            isPreviousAreaPositive = true;
-            pointsToKeep[2] = thirdPointMaxArea;
-            nbReducedPoints = 3;
-        }
-        else {
-            isPreviousAreaPositive = false;
-            pointsToKeep[2] = thirdPointMinArea;
-            nbReducedPoints = 3;
-        }
-
-        // Compute the 4th point by choosing the triangle that add the most
-        // triangle area to the previous triangle and has opposite sign area (opposite winding)
-
-        decimal largestArea = decimal(0.0); // Largest area (positive or negative)
-        element = mContactPoints;
-
-        if (nbReducedPoints == 3) {
-
-            // For each remaining point
-            while(element != nullptr) {
-
-                if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
-                element = element->getNext();
-                continue;
-                }
-
-                // For each edge of the triangle made by the first three points
-                for (uint i=0; i<3; i++) {
-
-                uint edgeVertex1Index = i;
-                uint edgeVertex2Index = i < 2 ? i + 1 : 0;
-
-                const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-                const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-
-                // Compute the triangle area
-                decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
-
-                // We are looking at the triangle with maximal area (positive or negative).
-                // If the previous area is positive, we are looking at negative area now.
-                // If the previous area is negative, we are looking at the positive area now.
-                if (isPreviousAreaPositive && area <= largestArea) {
-                    largestArea = area;
-                    pointsToKeep[3] = element;
-                    nbReducedPoints = 4;
-                }
-                else if (!isPreviousAreaPositive && area >= largestArea) {
-                    largestArea = area;
-                    pointsToKeep[3] = element;
-                    nbReducedPoints = 4;
-                }
-                }
-
-                element = element->getNext();
-            }
-        }
-
-        // Delete the contact points we do not want to keep from the linked list
-        element = mContactPoints;
-        ContactPoint* previousElement = nullptr;
-        while(element != nullptr) {
-
-            bool deletePoint = true;
-
-            // Skip the points we want to keep
-            for (uint i=0; i<nbReducedPoints; i++) {
-
-                if (element == pointsToKeep[i]) {
-
-                    previousElement = element;
-                    element = element->getNext();
-                    deletePoint = false;
-                }
-            }
-
-            if (deletePoint) {
-
-                ContactPoint* contactPointToDelete = element;
-                element = element->getNext();
-
-                removeContactPoint(contactPointToDelete);
-            }
-        }
-
-        assert(nbReducedPoints > 0 && nbReducedPoints <= 4);
-        mNbContactPoints = nbReducedPoints;
-    }
 }

src/collision/ContactManifold.h

@@ -40,44 +40,6 @@ class CollisionBody;
 class ContactPoint;
 class PoolAllocator;
 
-// Structure ContactManifoldListElement
-/**
- * This structure represents a single element of a linked list of contact manifolds
- */
-struct ContactManifoldListElement {
-
-    private:
-
-        // -------------------- Attributes -------------------- //
-
-        /// Pointer to a contact manifold with contact points
-        ContactManifold* mContactManifold;
-
-        /// Next element of the list
-        ContactManifoldListElement* mNext;
-
-   public:
-
-        // -------------------- Methods -------------------- //
-
-        /// Constructor
-        ContactManifoldListElement(ContactManifold* contactManifold,
-                                   ContactManifoldListElement* next)
-                                  :mContactManifold(contactManifold), mNext(next) {
-
-        }
-
-        /// Return the contact manifold
-        ContactManifold* getContactManifold() {
-            return mContactManifold;
-        }
-
-        /// Return the next element in the linked-list
-        ContactManifoldListElement* getNext() {
-            return mNext;
-        }
-};
-
 // Class ContactManifold
 /**
  * This class represents a set of contact points between two bodies that
@@ -91,6 +53,7 @@ struct ContactManifoldListElement {
  */
 class ContactManifold {
 
+    // TODO : Check if we can use public fields in this class (maybe this class is used by users directly)
     private:
 
         // -------------------- Constants -------------------- //
@@ -100,14 +63,22 @@ class ContactManifold {
 
         // -------------------- Attributes -------------------- //
 
-        /// Pointer to the first proxy shape of the contact
+        // TODO : For each of the attributes, check if we need to keep it or not
-        ProxyShape* mShape1;
 
-        /// Pointer to the second proxy shape of the contact
+        /// Index of the first contact point of the manifold in the list of contact points
-        ProxyShape* mShape2;
+        uint mContactPointsIndex;
 
-        /// Contact points in the manifold
+        /// Entity of the first body in contact
-        ContactPoint* mContactPoints;
+        Entity bodyEntity1;
+
+        /// Entity of the second body in contact
+        Entity bodyEntity2;
+
+        /// Entity of the first proxy-shape in contact
+        Entity proxyShapeEntity1;
+
+        /// Entity of the second proxy-shape in contact
+        Entity proxyShapeEntity2;
 
         /// Number of contacts in the cache
         int8 mNbContactPoints;
@@ -133,44 +104,11 @@ class ContactManifold {
         /// True if the contact manifold has already been added into an island
         bool mIsAlreadyInIsland;
 
-        /// Reference to the memory allocator
-        MemoryAllocator& mMemoryAllocator;
-
-        /// Pointer to the next contact manifold in the linked-list
-        ContactManifold* mNext;
-
-        /// Pointer to the previous contact manifold in linked-list
-        ContactManifold* mPrevious;
-
-        /// True if the contact manifold is obsolete
-        bool mIsObsolete;
-
-        /// World settings
-        const WorldSettings& mWorldSettings;
-
         // -------------------- Methods -------------------- //
 
         /// Return true if the contact manifold has already been added into an island
         bool isAlreadyInIsland() const;
 
-        /// Set the pointer to the next element in the linked-list
-        void setNext(ContactManifold* nextManifold);
-
-        /// Return true if the manifold is obsolete
-        bool getIsObsolete() const;
-
-        /// Set to true to make the manifold obsolete
-        void setIsObsolete(bool isObselete, bool setContactPoints);
-
-        /// Clear the obsolete contact points
-        void clearObsoleteContactPoints();
-
-        /// Return the contact normal direction Id of the manifold
-        short getContactNormalId() const;
-
-        /// Return the largest depth of all the contact points
-        decimal getLargestContactDepth() const;
-
         /// set the first friction vector at the center of the contact manifold
         void setFrictionVector1(const Vector3& mFrictionVector1);
 
@@ -183,24 +121,12 @@ class ContactManifold {
         /// Set the second friction accumulated impulse
         void setFrictionImpulse2(decimal frictionImpulse2);
 
-        /// Add a contact point
-        void addContactPoint(const ContactPointInfo* contactPointInfo);
-
-        /// Reduce the number of contact points of the currently computed manifold
-        void reduce(const Transform& shape1ToWorldTransform);
-
-        /// Remove a contact point
-        void removeContactPoint(ContactPoint* contactPoint);
-
         /// Set the friction twist accumulated impulse
         void setFrictionTwistImpulse(decimal frictionTwistImpulse);
 
         /// Set the accumulated rolling resistance impulse
         void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
 
-        /// Set the pointer to the previous element in the linked-list
-        void setPrevious(ContactManifold* previousManifold);
-
         /// Return the first friction vector at the center of the contact manifold
         const Vector3& getFrictionVector1() const;
 
@@ -221,42 +147,21 @@ class ContactManifold {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifold(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator,
+        ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
-                        const WorldSettings& worldSettings);
+                        uint contactPointsIndex, int8 nbContactPoints);
 
         /// Destructor
         ~ContactManifold();
 
-        /// Deleted copy-constructor
+        /// Copy-constructor
-        ContactManifold(const ContactManifold& contactManifold) = delete;
+        ContactManifold(const ContactManifold& contactManifold) = default;
 
-        /// Deleted assignment operator
+        /// Assignment operator
-        ContactManifold& operator=(const ContactManifold& contactManifold) = delete;
+        ContactManifold& operator=(const ContactManifold& contactManifold) = default;
-
-        /// Return a pointer to the first proxy shape of the contact
-        ProxyShape* getShape1() const;
-
-        /// Return a pointer to the second proxy shape of the contact
-        ProxyShape* getShape2() const;
-
-        /// Return a pointer to the first body of the contact manifold
-        CollisionBody* getBody1() const;
-
-        /// Return a pointer to the second body of the contact manifold
-        CollisionBody* getBody2() const;
 
         /// Return the number of contact points in the manifold
         int8 getNbContactPoints() const;
 
-        /// Return a pointer to the first contact point of the manifold
-        ContactPoint* getContactPoints() const;
-
-        /// Return a pointer to the previous element in the linked-list
-        ContactManifold* getPrevious() const;
-
-        /// Return a pointer to the next element in the linked-list
-        ContactManifold* getNext() const;
-
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -264,28 +169,9 @@ class ContactManifold {
         friend class CollisionBody;
         friend class ContactManifoldSet;
         friend class ContactSolver;
+        friend class CollisionDetection;
 };
 
-// Return a pointer to the first proxy shape of the contact
-inline ProxyShape* ContactManifold::getShape1() const {
-    return mShape1;
-}
-
-// Return a pointer to the second proxy shape of the contact
-inline ProxyShape* ContactManifold::getShape2() const {
-    return mShape2;
-}
-
-// Return a pointer to the first body of the contact manifold
-inline CollisionBody* ContactManifold::getBody1() const {
-    return mShape1->getBody();
-}
-
-// Return a pointer to the second body of the contact manifold
-inline CollisionBody* ContactManifold::getBody2() const {
-    return mShape2->getBody();
-}
-
 // Return the number of contact points in the manifold
 inline int8 ContactManifold::getNbContactPoints() const {
     return mNbContactPoints;
@@ -346,41 +232,11 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
     mRollingResistanceImpulse = rollingResistanceImpulse;
 }
 
-// Return a pointer to the first contact point of the manifold
-inline ContactPoint* ContactManifold::getContactPoints() const {
-    return mContactPoints;
-}
-
 // Return true if the contact manifold has already been added into an island
 inline bool ContactManifold::isAlreadyInIsland() const {
     return mIsAlreadyInIsland;
 }
 
-// Return a pointer to the previous element in the linked-list
-inline ContactManifold* ContactManifold::getPrevious() const {
-    return mPrevious;
-}
-
-// Set the pointer to the previous element in the linked-list
-inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
-    mPrevious = previousManifold;
-}
-
-// Return a pointer to the next element in the linked-list
-inline ContactManifold* ContactManifold::getNext() const {
-    return mNext;
-}
-
-// Set the pointer to the next element in the linked-list
-inline void ContactManifold::setNext(ContactManifold* nextManifold) {
-    mNext = nextManifold;
-}
-
-// Return true if the manifold is obsolete
-inline bool ContactManifold::getIsObsolete() const {
-    return mIsObsolete;
-}
-
 }
 
 #endif

src/collision/ContactManifoldInfo.h

@@ -0,0 +1,67 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactManifoldInfo
+/**
+ * This structure contains informations about a collision contact
+ * manifold computed during the narrow-phase collision detection.
+ */
+struct ContactManifoldInfo {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Indices of the contact points in the mPotentialContactPoints array
+        List<uint> potentialContactPointsIndices;
+
+        /// Overlapping pair id
+        OverlappingPair::OverlappingPairId pairId;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldInfo(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+               : potentialContactPointsIndices(allocator), pairId(pairId) {
+
+        }
+
+
+};
+
+}
+
+#endif

src/collision/ContactManifoldSet.cpp

@@ -1,295 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
-* Copyright (c) 2010-2018 Daniel Chappuis                                       *
-*********************************************************************************
-*                                                                               *
-* This software is provided 'as-is', without any express or implied warranty.   *
-* In no event will the authors be held liable for any damages arising from the  *
-* use of this software.                                                         *
-*                                                                               *
-* Permission is granted to anyone to use this software for any purpose,         *
-* including commercial applications, and to alter it and redistribute it        *
-* freely, subject to the following restrictions:                                *
-*                                                                               *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-*    that you wrote the original software. If you use this software in a        *
-*    product, an acknowledgment in the product documentation would be           *
-*    appreciated but is not required.                                           *
-*                                                                               *
-* 2. Altered source versions must be plainly marked as such, and must not be    *
-*    misrepresented as being the original software.                             *
-*                                                                               *
-* 3. This notice may not be removed or altered from any source distribution.    *
-*                                                                               *
-********************************************************************************/
-
-// Libraries
-#include "ContactManifoldSet.h"
-#include "narrowphase/NarrowPhaseInfoBatch.h"
-#include "constraint/ContactPoint.h"
-#include "ProxyShape.h"
-#include "collision/ContactManifold.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
-                                       MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
-                   : mNbManifolds(0), mShape1(shape1),
-                     mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr), mWorldSettings(worldSettings) {
-
-    // Compute the maximum number of manifolds allowed between the two shapes
-    mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
-}
-
-// Destructor
-ContactManifoldSet::~ContactManifoldSet() {
-
-    // Clear all the contact manifolds
-    clear();
-}
-
-void ContactManifoldSet::addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
-
-    assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
-
-    // For each potential contact point to add
-    for (uint i=0; i < narrowPhaseInfoBatch.contactPoints[batchIndex].size(); i++) {
-
-        ContactPointInfo* contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][i];
-
-        // Look if the contact point correspond to an existing potential manifold
-        // (if the contact point normal is similar to the normal of an existing manifold)
-        ContactManifold* manifold = mManifolds;
-        bool similarManifoldFound = false;
-        while(manifold != nullptr) {
-
-            // Get the first contact point
-            const ContactPoint* point = manifold->getContactPoints();
-            assert(point != nullptr);
-
-            // If we have found a corresponding manifold for the new contact point
-            // (a manifold with a similar contact normal direction)
-            if (point->getNormal().dot(contactPoint->normal) >= mWorldSettings.cosAngleSimilarContactManifold) {
-
-                // Add the contact point to the manifold
-                manifold->addContactPoint(contactPoint);
-
-               similarManifoldFound = true;
-
-               break;
-            }
-
-            manifold = manifold->getNext();
-        }
-
-        // If we have not found an existing manifold with a similar contact normal
-        if (!similarManifoldFound) {
-
-            // Create a new contact manifold
-            ContactManifold* manifold = createManifold();
-
-            // Add the contact point to the manifold
-            manifold->addContactPoint(contactPoint);
-        }
-    }
-}
-
-// Return the total number of contact points in the set of manifolds
-int ContactManifoldSet::getTotalNbContactPoints() const {
-    int nbPoints = 0;
-
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-        nbPoints += manifold->getNbContactPoints();
-
-        manifold = manifold->getNext();
-    }
-
-    return nbPoints;
-}
-
-// Return the maximum number of contact manifolds allowed between to collision shapes
-int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
-
-    // If both shapes are convex
-    if (shape1->isConvex() && shape2->isConvex()) {
-        return mWorldSettings.nbMaxContactManifoldsConvexShape;
-
-    }   // If there is at least one concave shape
-    else {
-        return mWorldSettings.nbMaxContactManifoldsConcaveShape;
-    }
-}
-
-// Remove a contact manifold that is the least optimal (smaller penetration depth)
-void ContactManifoldSet::removeNonOptimalManifold() {
-
-    assert(mNbManifolds > mNbMaxManifolds);
-    assert(mManifolds != nullptr);
-
-    // Look for a manifold that is not new and with the smallest contact penetration depth.
-    // At the same time, we also look for a new manifold with the smallest contact penetration depth
-    // in case no old manifold exists.
-    ContactManifold* minDepthManifold = nullptr;
-    decimal minDepth = DECIMAL_LARGEST;
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-
-        // Get the largest contact point penetration depth of the manifold
-        const decimal depth = manifold->getLargestContactDepth();
-
-        if (depth < minDepth) {
-            minDepth = depth;
-            minDepthManifold = manifold;
-        }
-
-        manifold = manifold->getNext();
-    }
-
-    // Remove the non optimal manifold
-    assert(minDepthManifold != nullptr);
-    removeManifold(minDepthManifold);
-}
-
-// Create a new contact manifold and add it to the set
-ContactManifold* ContactManifoldSet::createManifold() {
-
-    ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
-                                        ContactManifold(mShape1, mShape2, mMemoryAllocator, mWorldSettings);
-    manifold->setPrevious(nullptr);
-    manifold->setNext(mManifolds);
-    if (mManifolds != nullptr) {
-        mManifolds->setPrevious(manifold);
-    }
-    mManifolds = manifold;
-
-    mNbManifolds++;
-
-    return manifold;
-}
-
-// Return the contact manifold with a similar contact normal.
-// If no manifold has close enough contact normal, it returns nullptr
-ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& contactNormal) const {
-
-    ContactManifold* manifold = mManifolds;
-
-    // Return the Id of the manifold with the same normal direction id (if exists)
-    while (manifold != nullptr) {
-
-        // Get the first contact point of the current manifold
-        const ContactPoint* point = manifold->getContactPoints();
-        assert(point != nullptr);
-
-        // If the contact normal of the two manifolds are close enough
-        if (contactNormal.dot(point->getNormal()) >= mWorldSettings.cosAngleSimilarContactManifold) {
-            return manifold;
-        }
-
-        manifold = manifold->getNext();
-    }
-
-    return nullptr;
-}
-
-// Clear the contact manifold set
-void ContactManifoldSet::clear() {
-
-    ContactManifold* manifold = mManifolds;
-    while(manifold != nullptr) {
-
-        ContactManifold* nextManifold = manifold->getNext();
-
-        // Delete the contact manifold
-        manifold->~ContactManifold();
-        mMemoryAllocator.release(manifold, sizeof(ContactManifold));
-
-        manifold = nextManifold;
-
-        mNbManifolds--;
-    }
-
-    mManifolds = nullptr;
-
-    assert(mNbManifolds == 0);
-}
-
-// Remove a contact manifold from the set
-void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
-
-    assert(mNbManifolds > 0);
-	assert(manifold != nullptr);
-
-    ContactManifold* previous = manifold->getPrevious();
-    ContactManifold* next = manifold->getNext();
-
-    if (previous != nullptr) {
-        previous->setNext(next);
-    }
-	else {
-		mManifolds = next;
-	}
-
-    if (next != nullptr) {
-        next->setPrevious(previous);
-    }
-
-    // Delete the contact manifold
-    manifold->~ContactManifold();
-    mMemoryAllocator.release(manifold, sizeof(ContactManifold));
-    mNbManifolds--;
-}
-
-// Make all the contact manifolds and contact points obsolete
-void ContactManifoldSet::makeContactsObsolete() {
-
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-
-        manifold->setIsObsolete(true, true);
-
-        manifold = manifold->getNext();
-    }
-}
-
-// Clear the obsolete contact manifolds and contact points
-void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
-
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-
-        // Get the next manifold in the linked-list
-        ContactManifold* nextManifold = manifold->getNext();
-
-        // If the manifold is obsolete
-        if (manifold->getIsObsolete()) {
-
-            // Delete the contact manifold
-            removeManifold(manifold);
-        }
-        else {
-
-            // Clear the obsolete contact points of the manifold
-            manifold->clearObsoleteContactPoints();
-        }
-
-        manifold = nextManifold;
-    }
-}
-
-// Remove some contact manifolds and contact points if there are too many of them
-void ContactManifoldSet::reduce() {
-
-    // Remove non optimal contact manifold while there are too many manifolds in the set
-    while (mNbManifolds > mNbMaxManifolds) {
-        removeNonOptimalManifold();
-    }
-
-    // Reduce all the contact manifolds in case they have too many contact points
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-        manifold->reduce(mShape1->getLocalToWorldTransform());
-        manifold = manifold->getNext();
-    }
-}

src/collision/ContactManifoldSet.h

@@ -1,166 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
-* Copyright (c) 2010-2018 Daniel Chappuis                                       *
-*********************************************************************************
-*                                                                               *
-* This software is provided 'as-is', without any express or implied warranty.   *
-* In no event will the authors be held liable for any damages arising from the  *
-* use of this software.                                                         *
-*                                                                               *
-* Permission is granted to anyone to use this software for any purpose,         *
-* including commercial applications, and to alter it and redistribute it        *
-* freely, subject to the following restrictions:                                *
-*                                                                               *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-*    that you wrote the original software. If you use this software in a        *
-*    product, an acknowledgment in the product documentation would be           *
-*    appreciated but is not required.                                           *
-*                                                                               *
-* 2. Altered source versions must be plainly marked as such, and must not be    *
-*    misrepresented as being the original software.                             *
-*                                                                               *
-* 3. This notice may not be removed or altered from any source distribution.    *
-*                                                                               *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
-#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
-
-// Libraries
-#include "configuration.h"
-
-namespace reactphysics3d {
-
-// Declarations
-class ContactManifold;
-class ContactManifoldInfo;
-class ProxyShape;
-class MemoryAllocator;
-struct WorldSettings;
-struct NarrowPhaseInfoBatch;
-struct Vector3;
-class CollisionShape;
-class Transform;
-
-
-// Constants
-const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3;   // Maximum number of contact manifolds in the set
-const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3;    // N Number for the N x N subdivisions of the cubemap
-
-// Class ContactManifoldSet
-/**
- * This class represents a set of one or several contact manifolds. Typically a
- * convex/convex collision will have a set with a single manifold and a convex-concave
- * collision can have more than one manifolds. Note that a contact manifold can
- * contains several contact points.
- */
-class ContactManifoldSet {
-
-    private:
-
-        // -------------------- Attributes -------------------- //
-
-        /// Maximum number of contact manifolds in the set
-        int mNbMaxManifolds;
-
-        /// Current number of contact manifolds in the set
-        int mNbManifolds;
-
-        /// Pointer to the first proxy shape of the contact
-        ProxyShape* mShape1;
-
-        /// Pointer to the second proxy shape of the contact
-        ProxyShape* mShape2;
-
-        /// Reference to the memory allocator for the contact manifolds
-        MemoryAllocator& mMemoryAllocator;
-
-        /// Contact manifolds of the set
-        ContactManifold* mManifolds;
-
-        /// World settings
-        const WorldSettings& mWorldSettings;
-
-        // -------------------- Methods -------------------- //
-
-        /// Create a new contact manifold and add it to the set
-        ContactManifold* createManifold();
-
-        // Return the contact manifold with a similar contact normal.
-        ContactManifold* selectManifoldWithSimilarNormal(const Vector3& contactNormal) const;
-
-        /// Remove a contact manifold that is the least optimal (smaller penetration depth)
-        void removeNonOptimalManifold();
-
-        /// Return the maximum number of contact manifolds allowed between to collision shapes
-        int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
-
-        /// Clear the contact manifold set
-        void clear();
-
-        /// Delete a contact manifold
-        void removeManifold(ContactManifold* manifold);
-
-    public:
-
-        // -------------------- Methods -------------------- //
-
-        /// Constructor
-        ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
-                           MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings);
-
-        /// Destructor
-        ~ContactManifoldSet();
-
-        /// Add the contact points from the narrow phase
-        void addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
-
-        /// Return the first proxy shape
-        ProxyShape* getShape1() const;
-
-        /// Return the second proxy shape
-        ProxyShape* getShape2() const;
-
-        /// Return the number of manifolds in the set
-        int getNbContactManifolds() const;
-
-        /// Return a pointer to the first element of the linked-list of contact manifolds
-        ContactManifold* getContactManifolds() const;
-
-        /// Make all the contact manifolds and contact points obsolete
-        void makeContactsObsolete();
-
-        /// Return the total number of contact points in the set of manifolds
-        int getTotalNbContactPoints() const;
-
-        /// Clear the obsolete contact manifolds and contact points
-        void clearObsoleteManifoldsAndContactPoints();
-
-        // Remove some contact manifolds and contact points if there are too many of them
-        void reduce();
-};
-
-// Return the first proxy shape
-inline ProxyShape* ContactManifoldSet::getShape1() const {
-    return mShape1;
-}
-
-// Return the second proxy shape
-inline ProxyShape* ContactManifoldSet::getShape2() const {
-    return mShape2;
-}
-
-// Return the number of manifolds in the set
-inline int ContactManifoldSet::getNbContactManifolds() const {
-    return mNbManifolds;
-}
-
-// Return a pointer to the first element of the linked-list of contact manifolds
-inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
-    return mManifolds;
-}
-
-}
-
-#endif
-

src/collision/ContactPair.h

@@ -0,0 +1,98 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_OVERLAPPING_PAIR_CONTACT_H
+#define REACTPHYSICS3D_OVERLAPPING_PAIR_CONTACT_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactPair
+/**
+ * This structure represents a pair of shapes that are in contact during narrow-phase.
+ */
+struct ContactPair {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Overlapping pair Id
+        OverlappingPair::OverlappingPairId pairId;
+
+        /// Indices of the potential contact manifolds
+        List<uint> potentialContactManifoldsIndices;
+
+        /// Entity of the first body of the contact
+        Entity body1Entity;
+
+        /// Entity of the second body of the contact
+        Entity body2Entity;
+
+        /// Entity of the first proxy-shape of the contact
+        Entity proxyShape1Entity;
+
+        /// Entity of the second proxy-shape of the contact
+        Entity proxyShape2Entity;
+
+        /// True if the manifold is already in an island
+        bool isAlreadyInIsland;
+
+        /// Index of the contact pair in the array of pairs
+        uint contactPairIndex;
+
+        /// Index of the first contact manifold in the array
+        uint contactManifoldsIndex;
+
+        /// Number of contact manifolds
+        int8 nbContactManifolds;
+
+        /// Index of the first contact point in the array of contact points
+        uint contactPointsIndex;
+
+        /// Total number of contact points in all the manifolds of the contact pair
+        uint nbToTalContactPoints;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity,
+                    Entity proxyShape2Entity, uint contactPairIndex, MemoryAllocator& allocator)
+            : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
+              proxyShape1Entity(proxyShape1Entity), proxyShape2Entity(proxyShape2Entity),
+              isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
+              contactPointsIndex(0), nbToTalContactPoints(0) {
+
+        }
+};
+
+}
+
+#endif

src/collision/ContactPointInfo.h

@@ -65,9 +65,11 @@ struct ContactPointInfo {
         /// Contact point of body 2 in local space of body 2
         Vector3 localPoint2;
 
+        // TODO : Remove this field
         /// Pointer to the next contact point info
         ContactPointInfo* next;
 
+        // TODO : Remove this field
         /// True if the contact point has already been inserted into a manifold
         bool isUsed;
 

src/collision/OverlapCallback.cpp

@@ -0,0 +1,53 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "collision/OverlapCallback.h"
+#include "engine/CollisionWorld.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Contact Pair Constructor
+OverlapCallback::OverlapPair::OverlapPair(Pair<Entity, Entity>& overlapPair, CollisionWorld& world)
+                             : mOverlapPair(overlapPair), mWorld(world) {
+
+}
+
+// Return a pointer to the first body in contact
+CollisionBody* OverlapCallback::OverlapPair::getBody1() const {
+    return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mOverlapPair.first));
+}
+
+// Return a pointer to the second body in contact
+CollisionBody* OverlapCallback::OverlapPair::getBody2() const {
+    return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mOverlapPair.second));
+}
+
+// CollisionCallbackData Constructor
+OverlapCallback::CallbackData::CallbackData(List<Pair<Entity, Entity>>& overlapBodies, CollisionWorld& world)
+                :mOverlapBodies(overlapBodies), mWorld(world) {
+
+}

src/collision/OverlapCallback.h

@@ -26,32 +26,141 @@
 #ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H
 #define REACTPHYSICS3D_OVERLAP_CALLBACK_H
 
+// Libraries
+#include "containers/List.h"
+
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
 // Declarations
 class CollisionBody;
+class CollisionWorld;
+class ProxyShape;
+struct Entity;
 
 // Class OverlapCallback
 /**
- * This class can be used to register a callback for collision overlap queries.
+ * This class can be used to register a callback for collision overlap queries between bodies.
- * You should implement your own class inherited from this one and implement
+ * You should implement your own class inherited from this one and implement the onOverlap() method.
- * the notifyOverlap() method. This method will called each time a contact
- * point is reported.
  */
 class OverlapCallback {
 
     public:
 
+        // Class OverlapPair
+        /**
+         * This class represents the contact between two proxy-shapes of the physics world.
+         */
+        class OverlapPair {
+
+            private:
+
+                // -------------------- Attributes -------------------- //
+
+                /// Pair of overlapping body entities
+                Pair<Entity, Entity>& mOverlapPair;
+
+                /// Reference to the physics world
+                CollisionWorld& mWorld;
+
+                // -------------------- Methods -------------------- //
+
+                /// Constructor
+                OverlapPair(Pair<Entity, Entity>& overlapPair, CollisionWorld& world);
+
+            public:
+
+                // -------------------- Methods -------------------- //
+
+                /// Copy constructor
+                OverlapPair(const OverlapPair& contactPair) = default;
+
+                /// Assignment operator
+                OverlapPair& operator=(const OverlapPair& contactPair) = default;
+
+                /// Destructor
+                ~OverlapPair() = default;
+
+                /// Return a pointer to the first body in contact
+                CollisionBody* getBody1() const;
+
+                /// Return a pointer to the second body in contact
+                CollisionBody* getBody2() const;
+
+                // -------------------- Friendship -------------------- //
+
+                friend class OverlapCallback;
+        };
+
+        // Class CallbackData
+        /**
+         * This class contains data about overlap between bodies
+         */
+        class CallbackData {
+
+            private:
+
+                // -------------------- Attributes -------------------- //
+
+                List<Pair<Entity, Entity>>& mOverlapBodies;
+
+                /// Reference to the physics world
+                CollisionWorld& mWorld;
+
+                // -------------------- Methods -------------------- //
+
+                /// Constructor
+                CallbackData(List<Pair<Entity, Entity>>& overlapProxyShapes, CollisionWorld& world);
+
+                /// Deleted copy constructor
+                CallbackData(const CallbackData& callbackData) = delete;
+
+                /// Deleted assignment operator
+                CallbackData& operator=(const CallbackData& callbackData) = delete;
+
+                /// Destructor
+                ~CallbackData() = default;
+
+            public:
+
+                // -------------------- Methods -------------------- //
+
+                /// Return the number of overlapping pairs of bodies
+                uint getNbOverlappingPairs() const;
+
+                /// Return a given overlapping pair of bodies
+                OverlapPair getOverlappingPair(uint index) const;
+
+                // -------------------- Friendship -------------------- //
+
+                friend class CollisionDetection;
+        };
+
         /// Destructor
         virtual ~OverlapCallback() {
 
         }
 
-        /// This method will be called for each reported overlapping bodies
+        /// This method will be called to report bodies that overlap
-        virtual void notifyOverlap(CollisionBody* collisionBody)=0;
+        virtual void onOverlap(CallbackData& callbackData)=0;
 };
 
+// Return the number of overlapping pairs of bodies
+inline uint OverlapCallback::CallbackData::getNbOverlappingPairs() const {
+    return mOverlapBodies.size();
+}
+
+// Return a given overlapping pair of bodies
+/// Note that the returned OverlapPair object is only valid during the call of the CollisionCallback::onOverlap()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the OverlapPair
+/// object itself because it won't be valid after the CollisionCallback::onOverlap() call.
+inline OverlapCallback::OverlapPair OverlapCallback::CallbackData::getOverlappingPair(uint index) const {
+
+    assert(index < getNbOverlappingPairs());
+
+    return OverlapPair(mOverlapBodies[index], mWorld);
+}
+
 }
 
 #endif

src/collision/ProxyShape.cpp

@@ -78,6 +78,10 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
  */
 void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
 
+    // TODO : Here we should probably remove all overlapping pairs with this shape in the
+    //        broad-phase and add the shape in the "has moved" shape list so it is reevaluated
+    //        with the new mask bits
+
     mBody->mWorld.mProxyShapesComponents.setCollisionCategoryBits(mEntity, collisionCategoryBits);
 
     int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);
@@ -93,6 +97,10 @@ void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits)
  */
 void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
 
+    // TODO : Here we should probably remove all overlapping pairs with this shape in the
+    //        broad-phase and add the shape in the "has moved" shape list so it is reevaluated
+    //        with the new mask bits
+
     mBody->mWorld.mProxyShapesComponents.setCollideWithMaskBits(mEntity, collideWithMaskBits);
 
     int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -35,7 +35,7 @@ using namespace reactphysics3d;
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
 bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
-                                              bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+                                              bool reportContacts, MemoryAllocator& memoryAllocator) {
     
     bool isCollisionFound = false;
 
@@ -154,9 +154,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
 
                 narrowPhaseInfoBatch.isColliding[batchIndex] = true;
                 isCollisionFound = true;
-                if (stopFirstContactFound) {
-                    return isCollisionFound;
-                }
                 continue;
             }
         }
@@ -234,9 +231,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
 
             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
             isCollisionFound = true;
-            if (stopFirstContactFound) {
-                return isCollisionFound;
-            }
         }
     }
 

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -68,8 +68,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute the narrow-phase collision detection between two capsules
         bool testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
-                                   MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -41,14 +41,14 @@ using namespace reactphysics3d;
 // by Dirk Gregorius.
 bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                        uint batchStartIndex, uint batchNbItems,
-                                                       bool reportContacts, bool stopFirstContactFound,
+                                                       bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
                                                        MemoryAllocator& memoryAllocator) {
 
     bool isCollisionFound = false;
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
-    SATAlgorithm satAlgorithm(memoryAllocator);
+    SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -166,9 +166,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
             // Colision found
             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
             isCollisionFound = true;
-            if (stopFirstContactFound) {
-                return isCollisionFound;
-            }
             continue;
         }
 
@@ -183,9 +180,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
 
             if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
                 isCollisionFound = true;
-                if (stopFirstContactFound) {
-                    return isCollisionFound;
-                }
             }
         }
     }

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h

@@ -71,8 +71,8 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute the narrow-phase collision detection between a capsule and a polyhedron
         bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           uint batchNbItems, bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
-                                   MemoryAllocator& memoryAllocator);
+                           MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -37,11 +37,10 @@ using namespace reactphysics3d;
 // by Dirk Gregorius.
 bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                                 uint batchStartIndex, uint batchNbItems,
-                                                                bool reportContacts, bool stopFirstContactFound,
+                                                                bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
-                                                                MemoryAllocator& memoryAllocator) {
 
     // Run the SAT algorithm to find the separating axis and compute contact point
-    SATAlgorithm satAlgorithm(memoryAllocator);
+    SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -50,7 +49,7 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB
 #endif
 
     bool isCollisionFound = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex,
-                                                                                         batchNbItems, reportContacts, stopFirstContactFound);
+                                                                                         batchNbItems, reportContacts);
 
     for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
 
@@ -59,10 +58,6 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB
 
         lastFrameCollisionInfo->wasUsingSAT = true;
         lastFrameCollisionInfo->wasUsingGJK = false;
-
-        if (isCollisionFound && stopFirstContactFound) {
-            return isCollisionFound;
-        }
     }
 
     return isCollisionFound;

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h

@@ -65,9 +65,8 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
         ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two convex polyhedra
-        bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
+        bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
-                                   MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/NarrowPhaseInfoBatch.cpp

@@ -28,6 +28,7 @@
 #include "collision/ContactPointInfo.h"
 #include "collision/shapes/TriangleShape.h"
 #include "engine/OverlappingPair.h"
+#include <iostream>
 
 using namespace reactphysics3d;
 

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -43,7 +43,8 @@ using namespace reactphysics3d;
 const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
 
 // Constructor
-SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator)
+             : mClipWithPreviousAxisIfStillColliding(clipWithPreviousAxisIfStillColliding), mMemoryAllocator(memoryAllocator) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -54,7 +55,7 @@ SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(
 // Test collision between a sphere and a convex mesh
 bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                          uint batchStartIndex, uint batchNbItems,
-                                                         bool reportContacts, bool stopFirstContactFound) const {
+                                                         bool reportContacts) const {
 
     bool isCollisionFound = false;
 
@@ -136,9 +137,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
 
         narrowPhaseInfoBatch.isColliding[batchIndex] = true;
         isCollisionFound = true;
-        if (stopFirstContactFound) {
-            return isCollisionFound;
-        }
     }
 
     return isCollisionFound;
@@ -476,7 +474,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
 }
 
 // Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const {
 
     RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler);
 
@@ -528,7 +526,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                 }
 
                 // The two shapes were overlapping in the previous frame and still seem to overlap in this one
-                if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                     minPenetrationDepth = penetrationDepth;
                     minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -548,9 +546,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                         // Therefore, we can return without running the whole SAT algorithm
                         narrowPhaseInfoBatch.isColliding[batchIndex] = true;
                         isCollisionFound = true;
-                        if (stopFirstContactFound) {
-                            return isCollisionFound;
-                        }
                         continue;
                     }
 
@@ -571,7 +566,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                 }
 
                 // The two shapes were overlapping in the previous frame and still seem to overlap in this one
-                if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                     minPenetrationDepth = penetrationDepth;
                     minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -591,9 +586,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                         // Therefore, we can return without running the whole SAT algorithm
                         narrowPhaseInfoBatch.isColliding[batchIndex] = true;
                         isCollisionFound = true;
-                        if (stopFirstContactFound) {
-                            return isCollisionFound;
-                        }
                         continue;
                     }
 
@@ -632,7 +624,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                     }
 
                     // If the shapes were overlapping on the previous axis and still seem to overlap in this frame
-                    if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+                    if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
 
                         // Compute the closest points between the two edges (in the local-space of poylhedron 2)
                         Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
@@ -683,9 +675,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                             // we return without running the whole SAT algorithm
                             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
                             isCollisionFound = true;
-                            if (stopFirstContactFound) {
-                                return isCollisionFound;
-                            }
                             continue;
                         }
 
@@ -878,9 +867,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
 
         narrowPhaseInfoBatch.isColliding[batchIndex] = true;
         isCollisionFound = true;
-        if (stopFirstContactFound) {
-            return isCollisionFound;
-        }
     }
 
     return isCollisionFound;

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -60,6 +60,20 @@ class SATAlgorithm {
         /// make sure the contact manifold does not change too much between frames.
         static const decimal SAME_SEPARATING_AXIS_BIAS;
 
+        /// True means that if two shapes were colliding last time (previous frame) and are still colliding
+        /// we use the previous (minimum penetration depth) axis to clip the colliding features and we don't
+        /// recompute a new (minimum penetration depth) axis. This value must be true for a dynamic simulation
+        /// because it uses temporal coherence and clip the colliding features with the previous
+        /// axis (this is good for stability). However, when we use the testCollision() methods, the penetration
+        /// depths might be very large and we always want the current true axis with minimum penetration depth.
+        /// In this case, this value must be set to false. Consider the following situation. Two shapes start overlaping
+        /// with "x" being the axis of minimum penetration depth. Then, if the shapes move but are still penetrating,
+        /// it is possible that the axis of minimum penetration depth changes for axis "y" for instance. If this value
+        /// if true, we will always use the axis of the previous collision test and therefore always report that the
+        /// penetrating axis is "x" even if it has changed to axis "y" during the collision. This is not what we want
+        /// when we call the testCollision() methods.
+        bool mClipWithPreviousAxisIfStillColliding;
+
         /// Memory allocator
         MemoryAllocator& mMemoryAllocator;
 
@@ -126,7 +140,7 @@ class SATAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        SATAlgorithm(MemoryAllocator& memoryAllocator);
+        SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
 
         /// Destructor
         ~SATAlgorithm() = default;
@@ -140,7 +154,7 @@ class SATAlgorithm {
         /// Test collision between a sphere and a convex mesh
         bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                                    uint batchStartIndex, uint batchNbItems,
-                                                   bool reportContacts, bool stopFirstContactFound) const;
+                                                   bool reportContacts) const;
 
         /// Test collision between a capsule and a convex mesh
         bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool reportContacts) const;
@@ -158,7 +172,7 @@ class SATAlgorithm {
 
         /// Test collision between two convex meshes
         bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                                                             uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const;
+                                                             uint batchNbItems, bool reportContacts) const;
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
 bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
-                                             bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+                                             bool reportContacts, MemoryAllocator& memoryAllocator) {
 
     bool isCollisionFound = false;
 
@@ -137,9 +137,6 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
 
             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
             isCollisionFound = true;
-            if (stopFirstContactFound) {
-               return isCollisionFound;
-            }
             continue;
         }
     }

src/collision/narrowphase/SphereVsCapsuleAlgorithm.h

@@ -68,8 +68,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute the narrow-phase collision detection between a sphere and a capsule
         bool testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
-                                   MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
 bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
-                                                      bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+                                                      bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
@@ -73,9 +73,6 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
             // Return true
             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
             isCollisionFound = true;
-            if (stopFirstContactFound) {
-                return isCollisionFound;
-            }
             continue;
         }
 
@@ -83,7 +80,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
         if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
             // Run the SAT algorithm to find the separating axis and compute contact point
-            SATAlgorithm satAlgorithm(memoryAllocator);
+            SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -91,15 +88,11 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
 
 #endif
 
-            isCollisionFound |= satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts, stopFirstContactFound);
+            isCollisionFound |= satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts);
 
             lastFrameCollisionInfo->wasUsingGJK = false;
             lastFrameCollisionInfo->wasUsingSAT = true;
 
-            if (isCollisionFound && stopFirstContactFound) {
-                return isCollisionFound;
-            }
-
             continue;
         }
     }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -71,9 +71,8 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
-        bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
+        bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
-                                   MemoryAllocator& memoryAllocator);
 };
 
 }

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -32,7 +32,7 @@
 using namespace reactphysics3d;  
 
 bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
-                                            bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+                                            bool reportContacts, MemoryAllocator& memoryAllocator) {
 
     bool isCollisionFound = false;
 
@@ -94,9 +94,6 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
 
             narrowPhaseInfoBatch.isColliding[batchIndex] = true;
             isCollisionFound = true;
-            if (stopFirstContactFound) {
-                return isCollisionFound;
-            }
         }
     }
 

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -67,8 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Compute a contact info if the two bounding volume collide
         bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
-                                   uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
+                           uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
-                                   MemoryAllocator& memoryAllocator);
 };
 
 }

src/components/Components.h

@@ -121,6 +121,9 @@ class Components {
 
         /// Return the number of enabled components
         uint32 getNbEnabledComponents() const;
+
+        /// Return the index in the arrays for a given entity
+        uint32 getEntityIndex(Entity entity) const;
 };
 
 // Return true if an entity is sleeping
@@ -144,6 +147,11 @@ inline uint32 Components::getNbEnabledComponents() const {
     return mDisabledStartIndex;
 }
 
+// Return the index in the arrays for a given entity
+inline uint32 Components::getEntityIndex(Entity entity) const {
+    assert(hasComponent(entity));
+    return mMapEntityToComponentIndex[entity];
+}
 }
 
 #endif

src/components/DynamicsComponents.cpp

@@ -35,7 +35,11 @@ using namespace reactphysics3d;
 
 // Constructor
 DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
-                    :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) {
+                    :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
+                                           sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
+                                           sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
+                                           sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) +
+                                           sizeof(bool)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -57,6 +61,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     Entity* newBodies = static_cast<Entity*>(newBuffer);
     Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
     Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
+    Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newAngularVelocities + nbComponentsToAllocate);
+    Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
+    Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
+    Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
+    Vector3* newExternalForces = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
+    Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
+    decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
+    decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
+    decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
+    decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
+    Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
+    Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
+    Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
+    Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
+    Vector3* newCentersOfMassLocal = reinterpret_cast<Vector3*>(newConstrainedOrientations + nbComponentsToAllocate);
+    Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
+    bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
+    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -65,6 +87,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity));
         memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3));
+        memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3));
+        memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal));
+        memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
+        memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
+        memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
+        memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
+        memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
+        memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3));
+        memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion));
+        memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3));
+        memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3));
+        memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
+        memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
 
         // Deallocate previous memory
         mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@@ -74,6 +114,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     mBodies = newBodies;
     mLinearVelocities = newLinearVelocities;
     mAngularVelocities = newAngularVelocities;
+    mConstrainedLinearVelocities = newConstrainedLinearVelocities;
+    mConstrainedAngularVelocities = newConstrainedAngularVelocities;
+    mSplitLinearVelocities = newSplitLinearVelocities;
+    mSplitAngularVelocities = newSplitAngularVelocities;
+    mExternalForces = newExternalForces;
+    mExternalTorques = newExternalTorques;
+    mLinearDampings = newLinearDampings;
+    mAngularDampings = newAngularDampings;
+    mInitMasses = newInitMasses;
+    mInverseMasses = newInverseMasses;
+    mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
+    mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
+    mConstrainedPositions = newConstrainedPositions;
+    mConstrainedOrientations = newConstrainedOrientations;
+    mCentersOfMassLocal = newCentersOfMassLocal;
+    mCentersOfMassWorld = newCentersOfMassWorld;
+    mIsGravityEnabled = newIsGravityEnabled;
+    mIsAlreadyInIsland = newIsAlreadyInIsland;
     mNbAllocatedComponents = nbComponentsToAllocate;
 }
 
@@ -85,8 +143,26 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
 
     // Insert the new component data
     new (mBodies + index) Entity(bodyEntity);
-    new (mLinearVelocities + index) Vector3(component.linearVelocity);
+    new (mLinearVelocities + index) Vector3(0, 0, 0);
-    new (mAngularVelocities + index) Vector3(component.angularVelocity);
+    new (mAngularVelocities + index) Vector3(0, 0, 0);
+    new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
+    new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
+    new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
+    new (mSplitAngularVelocities + index) Vector3(0, 0, 0);
+    new (mExternalForces + index) Vector3(0, 0, 0);
+    new (mExternalTorques + index) Vector3(0, 0, 0);
+    mLinearDampings[index] = decimal(0.0);
+    mAngularDampings[index] = decimal(0.0);
+    mInitMasses[index] = decimal(1.0);
+    mInverseMasses[index] = decimal(1.0);
+    new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+    new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+    new (mConstrainedPositions + index) Vector3(0, 0, 0);
+    new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1);
+    new (mCentersOfMassLocal + index) Vector3(0, 0, 0);
+    new (mCentersOfMassWorld + index) Vector3(component.worldPosition);
+    mIsGravityEnabled[index] = true;
+    mIsAlreadyInIsland[index] = false;
 
     // Map the entity with the new component lookup index
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
@@ -107,6 +183,24 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
     new (mBodies + destIndex) Entity(mBodies[srcIndex]);
     new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
     new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
+    new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
+    new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
+    new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
+    new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]);
+    new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]);
+    new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]);
+    mLinearDampings[destIndex] = mLinearDampings[srcIndex];
+    mAngularDampings[destIndex] = mAngularDampings[srcIndex];
+    mInitMasses[destIndex] = mInitMasses[srcIndex];
+    mInverseMasses[destIndex] = mInverseMasses[srcIndex];
+    new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]);
+    new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
+    new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]);
+    new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]);
+    new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]);
+    new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]);
+    mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
+    mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
 
     // Destroy the source component
     destroyComponent(srcIndex);
@@ -129,6 +223,24 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     Entity entity1(mBodies[index1]);
     Vector3 linearVelocity1(mLinearVelocities[index1]);
     Vector3 angularVelocity1(mAngularVelocities[index1]);
+    Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
+    Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
+    Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
+    Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]);
+    Vector3 externalForce1(mExternalForces[index1]);
+    Vector3 externalTorque1(mExternalTorques[index1]);
+    decimal linearDamping1 = mLinearDampings[index1];
+    decimal angularDamping1 = mAngularDampings[index1];
+    decimal initMass1 = mInitMasses[index1];
+    decimal inverseMass1 = mInverseMasses[index1];
+    Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
+    Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
+    Vector3 constrainedPosition1 = mConstrainedPositions[index1];
+    Quaternion constrainedOrientation1 = mConstrainedOrientations[index1];
+    Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1];
+    Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1];
+    bool isGravityEnabled1 = mIsGravityEnabled[index1];
+    bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
 
     // Destroy component 1
     destroyComponent(index1);
@@ -139,6 +251,24 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     new (mBodies + index2) Entity(entity1);
     new (mLinearVelocities + index2) Vector3(linearVelocity1);
     new (mAngularVelocities + index2) Vector3(angularVelocity1);
+    new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
+    new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
+    new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
+    new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1);
+    new (mExternalForces + index2) Vector3(externalForce1);
+    new (mExternalTorques + index2) Vector3(externalTorque1);
+    mLinearDampings[index2] = linearDamping1;
+    mAngularDampings[index2] = angularDamping1;
+    mInitMasses[index2] = initMass1;
+    mInverseMasses[index2] = inverseMass1;
+    mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
+    mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
+    mConstrainedPositions[index2] = constrainedPosition1;
+    mConstrainedOrientations[index2] = constrainedOrientation1;
+    mCentersOfMassLocal[index2] = centerOfMassLocal1;
+    mCentersOfMassWorld[index2] = centerOfMassWorld1;
+    mIsGravityEnabled[index2] = isGravityEnabled1;
+    mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
 
     // Update the entity to component index mapping
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
@@ -160,4 +290,16 @@ void DynamicsComponents::destroyComponent(uint32 index) {
     mBodies[index].~Entity();
     mLinearVelocities[index].~Vector3();
     mAngularVelocities[index].~Vector3();
+    mConstrainedLinearVelocities[index].~Vector3();
+    mConstrainedAngularVelocities[index].~Vector3();
+    mSplitLinearVelocities[index].~Vector3();
+    mSplitAngularVelocities[index].~Vector3();
+    mExternalForces[index].~Vector3();
+    mExternalTorques[index].~Vector3();
+    mInverseInertiaTensorsLocal[index].~Matrix3x3();
+    mInverseInertiaTensorsWorld[index].~Matrix3x3();
+    mConstrainedPositions[index].~Vector3();
+    mConstrainedOrientations[index].~Quaternion();
+    mCentersOfMassLocal[index].~Vector3();
+    mCentersOfMassWorld[index].~Vector3();
 }

src/components/DynamicsComponents.h

@@ -30,6 +30,7 @@
 #include "mathematics/Transform.h"
 #include "engine/Entity.h"
 #include "components/Components.h"
+#include "mathematics/Matrix3x3.h"
 #include "containers/Map.h"
 
 // ReactPhysics3D namespace
@@ -59,6 +60,60 @@ class DynamicsComponents : public Components {
         /// Array with the angular velocity of each component
         Vector3* mAngularVelocities;
 
+        /// Array with the constrained linear velocity of each component
+        Vector3* mConstrainedLinearVelocities;
+
+        /// Array with the constrained angular velocity of each component
+        Vector3* mConstrainedAngularVelocities;
+
+        /// Array with the split linear velocity of each component
+        Vector3* mSplitLinearVelocities;
+
+        /// Array with the split angular velocity of each component
+        Vector3* mSplitAngularVelocities;
+
+        /// Array with the external force of each component
+        Vector3* mExternalForces;
+
+        /// Array with the external torque of each component
+        Vector3* mExternalTorques;
+
+        /// Array with the linear damping factor of each component
+        decimal* mLinearDampings;
+
+        /// Array with the angular damping factor of each component
+        decimal* mAngularDampings;
+
+        /// Array with the initial mass of each component
+        decimal* mInitMasses;
+
+        /// Array with the inverse mass of each component
+        decimal* mInverseMasses;
+
+        /// Array with the inverse of the inertia tensor of each component
+        Matrix3x3* mInverseInertiaTensorsLocal;
+
+        /// Array with the inverse of the world inertia tensor of each component
+        Matrix3x3* mInverseInertiaTensorsWorld;
+
+        /// Array with the constrained position of each component (for position error correction)
+        Vector3* mConstrainedPositions;
+
+        /// Array of constrained orientation for each component (for position error correction)
+        Quaternion* mConstrainedOrientations;
+
+        /// Array of center of mass of each component (in local-space coordinates)
+        Vector3* mCentersOfMassLocal;
+
+        /// Array of center of mass of each component (in world-space coordinates)
+        Vector3* mCentersOfMassWorld;
+
+        /// True if the gravity needs to be applied to this component
+        bool* mIsGravityEnabled;
+
+        /// Array with the boolean value to know if the body has already been added into an island
+        bool* mIsAlreadyInIsland;
+
         // -------------------- Methods -------------------- //
 
         /// Allocate memory for a given number of components
@@ -78,12 +133,11 @@ class DynamicsComponents : public Components {
         /// Structure for the data of a transform component
         struct DynamicsComponent {
 
-            const Vector3& linearVelocity;
+            const Vector3& worldPosition;
-            const Vector3& angularVelocity;
 
             /// Constructor
-            DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity)
+            DynamicsComponent(const Vector3& worldPosition)
-                : linearVelocity(linearVelocity), angularVelocity(angularVelocity) {
+                : worldPosition(worldPosition) {
 
             }
         };
@@ -100,10 +154,64 @@ class DynamicsComponents : public Components {
         void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component);
 
         /// Return the linear velocity of an entity
-        Vector3& getLinearVelocity(Entity bodyEntity) const;
+        const Vector3& getLinearVelocity(Entity bodyEntity) const;
 
         /// Return the angular velocity of an entity
-        Vector3& getAngularVelocity(Entity bodyEntity) const;
+        const Vector3& getAngularVelocity(Entity bodyEntity) const;
+
+        /// Return the constrained linear velocity of an entity
+        const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
+
+        /// Return the constrained angular velocity of an entity
+        const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const;
+
+        /// Return the split linear velocity of an entity
+        const Vector3& getSplitLinearVelocity(Entity bodyEntity) const;
+
+        /// Return the split angular velocity of an entity
+        const Vector3& getSplitAngularVelocity(Entity bodyEntity) const;
+
+        /// Return the external force of an entity
+        const Vector3& getExternalForce(Entity bodyEntity) const;
+
+        /// Return the external torque of an entity
+        const Vector3& getExternalTorque(Entity bodyEntity) const;
+
+        /// Return the linear damping factor of an entity
+        decimal getLinearDamping(Entity bodyEntity) const;
+
+        /// Return the angular damping factor of an entity
+        decimal getAngularDamping(Entity bodyEntity) const;
+
+        /// Return the initial mass of an entity
+        decimal getInitMass(Entity bodyEntity) const;
+
+        /// Return the mass inverse of an entity
+        decimal getMassInverse(Entity bodyEntity) const;
+
+        /// Return the inverse local inertia tensor of an entity
+        const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
+
+        /// Return the inverse world inertia tensor of an entity
+        const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
+
+        /// Return the constrained position of an entity
+        const Vector3& getConstrainedPosition(Entity bodyEntity);
+
+        /// Return the constrained orientation of an entity
+        const Quaternion& getConstrainedOrientation(Entity bodyEntity);
+
+        /// Return the local center of mass of an entity
+        const Vector3& getCenterOfMassLocal(Entity bodyEntity);
+
+        /// Return the world center of mass of an entity
+        const Vector3& getCenterOfMassWorld(Entity bodyEntity);
+
+        /// Return true if gravity is enabled for this entity
+        bool getIsGravityEnabled(Entity bodyEntity) const;
+
+        /// Return true if the entity is already in an island
+        bool getIsAlreadyInIsland(Entity bodyEntity) const;
 
         /// Set the linear velocity of an entity
         void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity);
@@ -111,13 +219,74 @@ class DynamicsComponents : public Components {
         /// Set the angular velocity of an entity
         void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
 
+        /// Set the constrained linear velocity of an entity
+        void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity);
+
+        /// Set the constrained angular velocity of an entity
+        void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity);
+
+        /// Set the split linear velocity of an entity
+        void setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity);
+
+        /// Set the split angular velocity of an entity
+        void setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity);
+
+        /// Set the external force of an entity
+        void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
+
+        /// Set the external force of an entity
+        void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque);
+
+        /// Set the linear damping factor of an entity
+        void setLinearDamping(Entity bodyEntity, decimal linearDamping);
+
+        /// Set the angular damping factor of an entity
+        void setAngularDamping(Entity bodyEntity, decimal angularDamping);
+
+        /// Set the initial mass of an entity
+        void setInitMass(Entity bodyEntity, decimal initMass);
+
+        /// Set the inverse mass of an entity
+        void setMassInverse(Entity bodyEntity, decimal inverseMass);
+
+        /// Set the inverse local inertia tensor of an entity
+        void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
+
+        /// Set the inverse world inertia tensor of an entity
+        void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
+
+        /// Set the constrained position of an entity
+        void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition);
+
+        /// Set the constrained orientation of an entity
+        void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation);
+
+        /// Set the local center of mass of an entity
+        void setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal);
+
+        /// Set the world center of mass of an entity
+        void setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld);
+
+        /// Set the value to know if the gravity is enabled for this entity
+        bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
+
+        /// Set the value to know if the entity is already in an island
+        bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
+
         // -------------------- Friendship -------------------- //
 
         friend class BroadPhaseSystem;
+        friend class DynamicsWorld;
+        friend class ContactSolver;
+        friend class BallAndSocketJoint;
+        friend class FixedJoint;
+        friend class HingeJoint;
+        friend class SliderJoint;
+
 };
 
 // Return the linear velocity of an entity
-inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
+inline const Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
 
    assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
 
@@ -125,7 +294,7 @@ inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
 }
 
 // Return the angular velocity of an entity
-inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
+inline const Vector3 &DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
 
    assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
 
@@ -148,6 +317,294 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect
    mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
 }
 
+// Return the constrained linear velocity of an entity
+inline const Vector3& DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained angular velocity of an entity
+inline const Vector3& DynamicsComponents::getConstrainedAngularVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the split linear velocity of an entity
+inline const Vector3& DynamicsComponents::getSplitLinearVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the split angular velocity of an entity
+inline const Vector3& DynamicsComponents::getSplitAngularVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the external force of an entity
+inline const Vector3& DynamicsComponents::getExternalForce(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mExternalForces[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the external torque of an entity
+inline const Vector3& DynamicsComponents::getExternalTorque(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the linear damping factor of an entity
+inline decimal DynamicsComponents::getLinearDamping(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mLinearDampings[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the angular damping factor of an entity
+inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the initial mass of an entity
+inline decimal DynamicsComponents::getInitMass(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInitMasses[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse mass of an entity
+inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse local inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse world inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained position of an entity
+inline const Vector3& DynamicsComponents::getConstrainedPosition(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained orientation of an entity
+inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the local center of mass of an entity
+inline const Vector3& DynamicsComponents::getCenterOfMassLocal(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the world center of mass of an entity
+inline const Vector3& DynamicsComponents::getCenterOfMassWorld(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Set the constrained linear velocity of an entity
+inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity;
+}
+
+// Set the constrained angular velocity of an entity
+inline void DynamicsComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity;
+}
+
+// Set the split linear velocity of an entity
+inline void DynamicsComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitLinearVelocity;
+}
+
+// Set the split angular velocity of an entity
+inline void DynamicsComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitAngularVelocity;
+}
+
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mExternalForces[mMapEntityToComponentIndex[bodyEntity]] = externalForce;
+}
+
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque;
+}
+
+// Set the linear damping factor of an entity
+inline void DynamicsComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mLinearDampings[mMapEntityToComponentIndex[bodyEntity]] = linearDamping;
+}
+
+// Set the angular damping factor of an entity
+inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping;
+}
+
+// Set the initial mass of an entity
+inline void DynamicsComponents::setInitMass(Entity bodyEntity, decimal initMass) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass;
+}
+
+// Set the mass inverse of an entity
+inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass;
+}
+
+// Set the inverse local inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
+}
+
+// Set the inverse world inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
+}
+
+// Set the constrained position of an entity
+inline void DynamicsComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition;
+}
+
+// Set the constrained orientation of an entity
+inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation;
+}
+
+// Set the local center of mass of an entity
+inline void DynamicsComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassLocal;
+}
+
+// Set the world center of mass of an entity
+inline void DynamicsComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassWorld;
+}
+
+// Return true if gravity is enabled for this entity
+inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return true if the entity is already in an island
+inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Set the value to know if the gravity is enabled for this entity
+inline bool DynamicsComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled;
+}
+
+/// Set the value to know if the entity is already in an island
+inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland;
+}
+
 }
 
 #endif

src/configuration.h

@@ -129,7 +129,7 @@ struct WorldSettings {
     decimal defaultBounciness = decimal(0.5);
 
     /// Velocity threshold for contact velocity restitution
-    decimal restitutionVelocityThreshold = decimal(1.0);
+    decimal restitutionVelocityThreshold = decimal(0.5);
 
     /// Default rolling resistance
     decimal defaultRollingRestistance = decimal(0.0);
@@ -154,13 +154,8 @@ struct WorldSettings {
     /// might enter sleeping mode
     decimal defaultSleepAngularVelocity = decimal(3.0) * (PI / decimal(180.0));
 
-    /// Maximum number of contact manifolds in an overlapping pair that involves two
+    /// Maximum number of contact manifolds in an overlapping pair
-    /// convex collision shapes.
+    uint nbMaxContactManifolds = 3;
-    int nbMaxContactManifoldsConvexShape = 1;
-
-    /// Maximum number of contact manifolds in an overlapping pair that involves at
-    /// least one concave collision shape.
-    int nbMaxContactManifoldsConcaveShape = 3;
 
     /// This is used to test if two contact manifold are similar (same contact normal) in order to
     /// merge them. If the cosine of the angle between the normals of the two manifold are larger
@@ -184,8 +179,7 @@ struct WorldSettings {
         ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl;
         ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl;
         ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl;
-        ss << "nbMaxContactManifoldsConvexShape=" << nbMaxContactManifoldsConvexShape << std::endl;
+        ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl;
-        ss << "nbMaxContactManifoldsConcaveShape=" << nbMaxContactManifoldsConcaveShape << std::endl;
         ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl;
 
         return ss.str();

src/constraint/BallAndSocketJoint.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "BallAndSocketJoint.h"
 #include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
 
 using namespace reactphysics3d;
 
@@ -44,13 +45,9 @@ BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jo
 // Initialize before solving the constraint
 void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
-    // Initialize the bodies index in the velocity array
-    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = mBody2->mArrayIndex;
-
     // Get the bodies center of mass and orientations
-    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
-    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -67,7 +64,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
 
     // Compute the matrix K=JM^-1J^t (3x3 matrix)
-    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+    decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+    decimal inverseMassBodies =  body1MassInverse + body2MassInverse;
     Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
@@ -98,36 +97,42 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
 void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Compute the impulse P=J^T * lambda for the body 1
     const Vector3 linearImpulseBody1 = -mImpulse;
     const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
 
     // Apply the impulse to the body 1
-    v1 += mBody1->mMassInverse * linearImpulseBody1;
+    v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
     w1 += mI1 * angularImpulseBody1;
 
     // Compute the impulse P=J^T * lambda for the body 2
     const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
 
     // Apply the impulse to the body to the body 2
-    v2 += mBody2->mMassInverse * mImpulse;
+    v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * mImpulse;
     w2 += mI2 * angularImpulseBody2;
 }
 
 // Solve the velocity constraint
 void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Compute J*v
     const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
@@ -141,14 +146,14 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
     const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
 
     // Apply the impulse to the body 1
-    v1 += mBody1->mMassInverse * linearImpulseBody1;
+    v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
     w1 += mI1 * angularImpulseBody1;
 
     // Compute the impulse P=J^T * lambda for the body 2
     const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
 
     // Apply the impulse to the body 2
-    v2 += mBody2->mMassInverse * deltaLambda;
+    v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * deltaLambda;
     w2 += mI2 * angularImpulseBody2;
 }
 
@@ -160,14 +165,14 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
     if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
 
     // Get the bodies center of mass and orientations
-    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Recompute the inverse inertia tensors
     mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -225,5 +230,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
     x2 += v2;
     q2 += Quaternion(0, w2) * q2 * decimal(0.5);
     q2.normalize();
+
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
 }
 

src/constraint/ContactPoint.cpp

@@ -45,6 +45,21 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const WorldSetti
     mIsObsolete = false;
 }
 
+// Constructor
+ContactPoint::ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings)
+             : mNormal(contactInfo.normal),
+               mPenetrationDepth(contactInfo.penetrationDepth),
+               mLocalPointOnShape1(contactInfo.localPoint1),
+               mLocalPointOnShape2(contactInfo.localPoint2),
+               mIsRestingContact(false), mPenetrationImpulse(0), mIsObsolete(false),
+               mWorldSettings(worldSettings) {
+
+    assert(mPenetrationDepth > decimal(0.0));
+    assert(mNormal.lengthSquare() > decimal(0.8));
+
+    mIsObsolete = false;
+}
+
 // Update the contact point with a new one that is similar (very close)
 /// The idea is to keep the cache impulse (for warm starting the contact solver)
 void ContactPoint::update(const ContactPointInfo* contactInfo) {

src/constraint/ContactPoint.h

@@ -93,18 +93,6 @@ class ContactPoint {
         /// Set the mIsRestingContact variable
         void setIsRestingContact(bool isRestingContact);
 
-        /// Set to true to make the contact point obsolete
-        void setIsObsolete(bool isObselete);
-
-        /// Set the next contact point in the linked list
-        void setNext(ContactPoint* next);
-
-        /// Set the previous contact point in the linked list
-        void setPrevious(ContactPoint* previous);
-
-        /// Return true if the contact point is obsolete
-        bool getIsObsolete() const;
-
     public :
 
         // -------------------- Methods -------------------- //
@@ -112,17 +100,20 @@ class ContactPoint {
         /// Constructor
         ContactPoint(const ContactPointInfo* contactInfo, const WorldSettings& worldSettings);
 
+        /// Constructor
+        ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings);
+
         /// Destructor
         ~ContactPoint() = default;
 
-        /// Deleted copy-constructor
+        /// Copy-constructor
-        ContactPoint(const ContactPoint& contact) = delete;
+        ContactPoint(const ContactPoint& contact) = default;
 
-        /// Deleted assignment operator
+        /// Assignment operator
-        ContactPoint& operator=(const ContactPoint& contact) = delete;
+        ContactPoint& operator=(const ContactPoint& contact) = default;
 
         /// Return the normal vector of the contact
-        Vector3 getNormal() const;
+        const Vector3& getNormal() const;
 
         /// Return the contact point on the first proxy shape in the local-space of the proxy shape
         const Vector3& getLocalPointOnShape1() const;
@@ -136,12 +127,6 @@ class ContactPoint {
         /// Return true if the contact is a resting contact
         bool getIsRestingContact() const;
 
-        /// Return the previous contact point in the linked list
-        inline ContactPoint* getPrevious() const;
-
-        /// Return the next contact point in the linked list
-        ContactPoint* getNext() const;
-
         /// Return the penetration depth
         decimal getPenetrationDepth() const;
 
@@ -152,13 +137,14 @@ class ContactPoint {
         friend class ContactManifold;
         friend class ContactManifoldSet;
         friend class ContactSolver;
+        friend class CollisionDetection;
 };
 
 // Return the normal vector of the contact
 /**
  * @return The contact normal
  */
-inline Vector3 ContactPoint::getNormal() const {
+inline const Vector3& ContactPoint::getNormal() const {
     return mNormal;
 }
 
@@ -216,54 +202,6 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
     mIsRestingContact = isRestingContact;
 }
 
-// Return true if the contact point is obsolete
-/**
- * @return True if the contact is obsolete
- */
-inline bool ContactPoint::getIsObsolete() const {
-    return mIsObsolete;
-}
-
-// Set to true to make the contact point obsolete
-/**
- * @param isObsolete True if the contact is obsolete
- */
-inline void ContactPoint::setIsObsolete(bool isObsolete) {
-    mIsObsolete = isObsolete;
-}
-
-// Return the next contact point in the linked list
-/**
- * @return A pointer to the next contact point in the linked-list of points
- */
-inline ContactPoint* ContactPoint::getNext() const {
-   return mNext;
-}
-
-// Set the next contact point in the linked list
-/**
- * @param next Pointer to the next contact point in the linked-list of points
- */
-inline void ContactPoint::setNext(ContactPoint* next) {
-    mNext = next;
-}
-
-// Return the previous contact point in the linked list
-/**
- * @return A pointer to the previous contact point in the linked-list of points
- */
-inline ContactPoint* ContactPoint::getPrevious() const {
-   return mPrevious;
-}
-
-// Set the previous contact point in the linked list
-/**
- * @param previous Pointer to the previous contact point in the linked-list of points
- */
-inline void ContactPoint::setPrevious(ContactPoint* previous) {
-    mPrevious = previous;
-}
-
 // Return the penetration depth of the contact
 /**
  * @return the penetration depth (in meters)

src/constraint/FixedJoint.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "FixedJoint.h"
 #include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
 
 using namespace reactphysics3d;
 
@@ -59,13 +60,9 @@ FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo)
 // Initialize before solving the constraint
 void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
-    // Initialize the bodies index in the velocity array
-    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = mBody2->mArrayIndex;
-
     // Get the bodies positions and orientations
-    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
-    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -82,7 +79,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
     Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
 
     // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
-    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+    const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+    const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
     Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
@@ -129,15 +128,18 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
 void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass of the bodies
-    const decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
     Vector3 linearImpulseBody1 = -mImpulseTranslation;
@@ -164,15 +166,18 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 // Solve the velocity constraint
 void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // --------------- Translation Constraints --------------- //
 
@@ -226,14 +231,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
 
     // Get the bodies positions and orientations
-    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Recompute the inverse inertia tensors
     mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -250,7 +255,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     // --------------- Translation Constraints --------------- //
 
     // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
-    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
     Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
@@ -348,5 +353,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     // Update the body position/orientation of body 2
     q2 += Quaternion(0, w2) * q2 * decimal(0.5);
     q2.normalize();
+
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
 }
 

src/constraint/HingeJoint.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "HingeJoint.h"
 #include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
 
 using namespace reactphysics3d;
 
@@ -66,13 +67,9 @@ HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo)
 // Initialize before solving the constraint
 void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
-    // Initialize the bodies index in the velocity array
-    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = mBody2->mArrayIndex;
-
     // Get the bodies positions and orientations
-    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
-    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -116,7 +113,9 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
     Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
 
     // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
-    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+    decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+    decimal inverseMassBodies = body1MassInverse + body2MassInverse;
     Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
@@ -198,15 +197,18 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
 void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    const decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Compute the impulse P=J^T * lambda for the 2 rotation constraints
     Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@@ -254,15 +256,18 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 // Solve the velocity constraint
 void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // --------------- Translation Constraints --------------- //
 
@@ -405,14 +410,14 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
 
     // Get the bodies positions and orientations
-    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Recompute the inverse inertia tensors
     mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -448,7 +453,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     // --------------- Translation Constraints --------------- //
 
     // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
-    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    const decimal body1InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+    const decimal body2InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+    decimal inverseMassBodies = body1InverseMass + body2InverseMass;
     Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
@@ -603,6 +610,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
             q2.normalize();
         }
     }
+
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
 }
 
 

src/constraint/Joint.cpp

@@ -30,7 +30,8 @@ using namespace reactphysics3d;
 
 // Constructor
 Joint::Joint(uint id, const JointInfo& jointInfo)
-           :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
+           :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()),
+            mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type),
             mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
             mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
 

src/constraint/Joint.h

@@ -124,20 +124,22 @@ class Joint {
         uint mId;
 
         /// Pointer to the first body of the joint
+        // TODO : Use Entities instead
         RigidBody* const mBody1;
 
         /// Pointer to the second body of the joint
+        // TODO : Use Entities instead
         RigidBody* const mBody2;
 
+        /// Entity of the first body of the joint
+        Entity mBody1Entity;
+
+        /// Entity of the second body of the joint
+        Entity mBody2Entity;
+
         /// Type of the joint
         const JointType mType;
 
-        /// Body 1 index in the velocity array to solve the constraint
-        uint mIndexBody1;
-
-        /// Body 2 index in the velocity array to solve the constraint
-        uint mIndexBody2;
-
         /// Position correction technique used for the constraint (used for joints)
         JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
 

src/constraint/SliderJoint.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "SliderJoint.h"
 #include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
 
 using namespace reactphysics3d;
 
@@ -74,13 +75,9 @@ SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo)
 // Initialize before solving the constraint
 void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
-    // Initialize the bodies index in the veloc ity array
-    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = mBody2->mArrayIndex;
-
     // Get the bodies positions and orientations
-    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
-    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -127,7 +124,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
 
     // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
     // constraints (2x2 matrix)
-    decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
+    const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+    const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+    const decimal sumInverseMass = body1MassInverse + body2MassInverse;
     Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
     Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
     Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@@ -173,7 +172,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
 
         // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
-        mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
+        mInverseMassMatrixLimit = sumInverseMass +
                                   mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
                                   mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
         mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
@@ -196,7 +195,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     if (mIsMotorEnabled) {
 
         // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
-        mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
+        mInverseMassMatrixMotor = sumInverseMass;
         mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
                     decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
     }
@@ -216,15 +215,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
 // Warm start the constraint (apply the previous impulse at the beginning of the step)
 void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    const decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
     decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
@@ -275,15 +277,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
 // Solve the velocity constraint
 void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
 
+    uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+    uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
     // Get the velocities
-    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
-    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
-    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
-    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+    Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // --------------- Translation Constraints --------------- //
 
@@ -437,14 +442,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
     if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
 
     // Get the bodies positions and orientations
-    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
 
     // Get the inverse mass and inverse inertia tensors of the bodies
-    decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Recompute the inertia tensor of bodies
     mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -483,7 +488,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
 
     // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
     // constraints (2x2 matrix)
-    decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
+    const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+    const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+    decimal sumInverseMass = body1MassInverse + body2MassInverse;
     Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
     Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
     Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@@ -603,7 +610,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
         if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
 
             // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
-            mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
+            const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+            const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+            mInverseMassMatrixLimit = body1MassInverse + body2MassInverse +
                                     mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
                                     mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
             mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
@@ -676,6 +685,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
             q2.normalize();
         }
     }
+
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+    constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+    constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
 }
 
 // Enable/Disable the limits of the joint

src/containers/Set.h

@@ -543,7 +543,7 @@ class Set {
         }
 
         /// Return a list with all the values of the set
-        List<V> toList(MemoryAllocator& listAllocator) {
+        List<V> toList(MemoryAllocator& listAllocator) const {
 
             List<V> list(listAllocator);
 

src/engine/CollisionWorld.cpp

@@ -202,9 +202,6 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     // Add the body ID to the list of free IDs
     mFreeBodiesIds.add(collisionBody->getId());
 
-    // Reset the contact manifold list of the body
-    collisionBody->resetContactManifoldsList();
-
     mBodyComponents.removeComponent(collisionBody->getEntity());
     mTransformComponents.removeComponent(collisionBody->getEntity());
     mEntityManager.destroyEntity(collisionBody->getEntity());
@@ -236,17 +233,6 @@ bodyindex CollisionWorld::computeNextAvailableBodyId() {
     return bodyID;
 }
 
-// Reset all the contact manifolds linked list of each body
-void CollisionWorld::resetContactManifoldListsOfBodies() {
-
-    // For each rigid body of the world
-    for (List<CollisionBody*>::Iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
-
-        // Reset the contact manifold list of the body
-        (*it)->resetContactManifoldsList();
-    }
-}
-
 // Notify the world if a body is disabled (sleeping or inactive) or not
 void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
 
@@ -270,37 +256,10 @@ void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
     }
 }
 
-// Test if the AABBs of two bodies overlap
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @return True if the AABBs of the two bodies overlap and false otherwise
- */
-bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
-                                     const CollisionBody* body2) const {
-
-    // If one of the body is not active, we return no overlap
-    if (!body1->isActive() || !body2->isActive()) return false;
-
-    // Compute the AABBs of both bodies
-    AABB body1AABB = body1->getAABB();
-    AABB body2AABB = body2->getAABB();
-
-    // Return true if the two AABBs overlap
-    return body1AABB.testCollision(body2AABB);
-}
-
-// Report all the bodies which have an AABB that overlaps with the AABB in parameter
-/**
- * @param aabb AABB used to test for overlap
- * @param overlapCallback Pointer to the callback class to report overlap
- * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
- */
-void CollisionWorld::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
-    mCollisionDetection.testAABBOverlap(aabb, overlapCallback, categoryMaskBits);
-}
-
 // Return true if two bodies overlap
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// if the two bodies overlap. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
 /**
  * @param body1 Pointer to the first body
  * @param body2 Pointer to a second body

src/engine/CollisionWorld.h

@@ -37,6 +37,8 @@
 #include "components/TransformComponents.h"
 #include "components/ProxyShapeComponents.h"
 #include "components/DynamicsComponents.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
 
 /// Namespace reactphysics3d
 namespace reactphysics3d {
@@ -130,9 +132,6 @@ class CollisionWorld {
         /// Return the next available body id
         bodyindex computeNextAvailableBodyId();
 
-        /// Reset all the contact manifolds linked list of each body
-        void resetContactManifoldListsOfBodies();
-
         /// Notify the world if a body is disabled (slepping or inactive) or not
         void notifyBodyDisabled(Entity entity, bool isDisabled);
 
@@ -163,30 +162,25 @@ class CollisionWorld {
         CollisionDispatch& getCollisionDispatch();
 
         /// Ray cast method
-        void raycast(const Ray& ray, RaycastCallback* raycastCallback,
+        void raycast(const Ray& ray, RaycastCallback* raycastCallback, unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
-                     unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
 
-        /// Test if the AABBs of two bodies overlap
+        /// Return true if two bodies overlap (collide)
-        bool testAABBOverlap(const CollisionBody* body1,
-                             const CollisionBody* body2) const;
-
-        /// Report all the bodies which have an AABB that overlaps with the AABB in parameter
-        void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-
-        /// Return true if two bodies overlap
         bool testOverlap(CollisionBody* body1, CollisionBody* body2);
 
-        /// Report all the bodies that overlap with the body in parameter
+        /// Report all the bodies that overlap (collide) with the body in parameter
-        void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
+        void testOverlap(CollisionBody* body, OverlapCallback& overlapCallback);
 
-        /// Test and report collisions between two bodies
+        /// Report all the bodies that overlap (collide) in the world
-        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+        void testOverlap(OverlapCallback& overlapCallback);
 
-        /// Test and report collisions between a body and all the others bodies of the world
+        /// Test collision and report contacts between two bodies.
-        void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
+        void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback);
 
-        /// Test and report collisions between all shapes of the world
+        /// Test collision and report all the contacts involving the body in parameter
-        void testCollision(CollisionCallback* callback);
+        void testCollision(CollisionBody* body, CollisionCallback& callback);
+
+        /// Test collision and report contacts between each colliding bodies in the world
+        void testCollision(CollisionCallback& callback);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -215,6 +209,8 @@ class CollisionWorld {
         friend class RigidBody;
         friend class ProxyShape;
         friend class ConvexMeshShape;
+        friend class CollisionCallback::ContactPair;
+        friend class OverlapCallback::OverlapPair;
 };
 
 // Set the collision dispatch configuration
@@ -241,42 +237,63 @@ inline void CollisionWorld::raycast(const Ray& ray,
     mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
 }
 
-// Test and report collisions between two bodies
+// Test collision and report contacts between two bodies.
+/// Use this method if you only want to get all the contacts between two bodies.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
 /**
  * @param body1 Pointer to the first body to test
  * @param body2 Pointer to the second body to test
  * @param callback Pointer to the object with the callback method
  */
-inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback) {
+inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) {
     mCollisionDetection.testCollision(body1, body2, callback);
 }
 
-// Test and report collisions between a body and all the others bodies of the world
+// Test collision and report all the contacts involving the body in parameter
+/// Use this method if you only want to get all the contacts involving a given body.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
 /**
  * @param body Pointer to the body against which we need to test collision
  * @param callback Pointer to the object with the callback method to report contacts
- * @param categoryMaskBits Bits mask corresponding to the category of bodies we need to test collision with
  */
-inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) {
+inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback& callback) {
-    mCollisionDetection.testCollision(body, callback, categoryMaskBits);
+    mCollisionDetection.testCollision(body, callback);
 }
 
-// Test and report collisions between all bodies of the world
+// Test collision and report contacts between each colliding bodies in the world
+/// Use this method if you want to get all the contacts between colliding bodies in the world.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
 /**
  * @param callback Pointer to the object with the callback method to report contacts
  */
-inline void CollisionWorld::testCollision(CollisionCallback* callback) {
+inline void CollisionWorld::testCollision(CollisionCallback& callback) {
     mCollisionDetection.testCollision(callback);
 }
 
-// Report all the bodies that overlap with the body in parameter
+// Report all the bodies that overlap (collide) with the body in parameter
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// which bodies overlap with the body in parameter. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
 /**
  * @param body Pointer to the collision body to test overlap with
  * @param overlapCallback Pointer to the callback class to report overlap
- * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
  */
-inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
+inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback& overlapCallback) {
-    mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
+    mCollisionDetection.testOverlap(body, overlapCallback);
+}
+
+// Report all the bodies that overlap (collide) in the world
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// which bodies overlap. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
+inline void CollisionWorld::testOverlap(OverlapCallback& overlapCallback) {
+    mCollisionDetection.testOverlap(overlapCallback);
 }
 
 // Return the name of the world

src/engine/ConstraintSolver.cpp

@@ -31,7 +31,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
+ConstraintSolver::ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents)
+                 : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(dynamicsComponents) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -42,13 +43,12 @@ ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
 }
 
 // Initialize the constraint solver for a given island
-void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
+void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.bodyEntities[islandIndex].size() > 0);
-    assert(island->getNbBodies() > 0);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // Set the current time step
     mTimeStep = dt;
@@ -58,49 +58,44 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
     mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Initialize the constraint before solving it
-        joints[i]->initBeforeSolve(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->initBeforeSolve(mConstraintSolverData);
 
         // Warm-start the constraint if warm-starting is enabled
         if (mIsWarmStartingActive) {
-            joints[i]->warmstart(mConstraintSolverData);
+            mIslands.joints[islandIndex][i]->warmstart(mConstraintSolverData);
         }
     }
 }
 
 // Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints(Island* island) {
+void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Solve the constraint
-        joints[i]->solveVelocityConstraint(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->solveVelocityConstraint(mConstraintSolverData);
     }
 }
 
 // Solve the position constraints
-void ConstraintSolver::solvePositionConstraints(Island* island) {
+void ConstraintSolver::solvePositionConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i < island->getNbJoints(); i++) {
 
         // Solve the constraint
-        joints[i]->solvePositionConstraint(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->solvePositionConstraint(mConstraintSolverData);
     }
 }

src/engine/ConstraintSolver.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "configuration.h"
 #include "mathematics/mathematics.h"
+#include "engine/Islands.h"
 
 namespace reactphysics3d {
 
@@ -36,6 +37,7 @@ namespace reactphysics3d {
 class Joint;
 class Island;
 class Profiler;
+class DynamicsComponents;
 
 // Structure ConstraintSolverData
 /**
@@ -49,24 +51,15 @@ struct ConstraintSolverData {
         /// Current time step of the simulation
         decimal timeStep;
 
-        /// Array with the bodies linear velocities
+        /// Reference to the dynamics components
-        Vector3* linearVelocities;
+        DynamicsComponents& dynamicsComponents;
-
-        /// Array with the bodies angular velocities
-        Vector3* angularVelocities;
-
-        /// Reference to the bodies positions
-        Vector3* positions;
-
-        /// Reference to the bodies orientations
-        Quaternion* orientations;
 
         /// True if warm starting of the solver is active
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
+        ConstraintSolverData(DynamicsComponents& dynamicsComponents)
-                                positions(nullptr), orientations(nullptr) {
+            :dynamicsComponents(dynamicsComponents) {
 
         }
 
@@ -153,6 +146,9 @@ class ConstraintSolver {
         /// True if the warm starting of the solver is active
         bool mIsWarmStartingActive;
 
+        /// Reference to the islands
+        Islands& mIslands;
+
         /// Constraint solver data used to initialize and solve the constraints
         ConstraintSolverData mConstraintSolverData;
 
@@ -167,19 +163,19 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver();
+        ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents);
 
         /// Destructor
         ~ConstraintSolver() = default;
 
         /// Initialize the constraint solver for a given island
-        void initializeForIsland(decimal dt, Island* island);
+        void initializeForIsland(decimal dt, uint islandIndex);
 
         /// Solve the constraints
-        void solveVelocityConstraints(Island* island);
+        void solveVelocityConstraints(uint islandIndex);
 
         /// Solve the position constraints
-        void solvePositionConstraints(Island* island);
+        void solvePositionConstraints(uint islandIndex);
 
         /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
         bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
@@ -187,14 +183,6 @@ class ConstraintSolver {
         /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
         void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
 
-        /// Set the constrained velocities arrays
-        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-                                            Vector3* constrainedAngularVelocities);
-
-        /// Set the constrained positions/orientations arrays
-        void setConstrainedPositionsArrays(Vector3* constrainedPositions,
-                                           Quaternion* constrainedOrientations);
-
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Set the profiler
@@ -204,28 +192,6 @@ class ConstraintSolver {
 
 };
 
-// Set the constrained velocities arrays
-inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-                                                            Vector3* constrainedAngularVelocities) {
-
-    assert(constrainedLinearVelocities != nullptr);
-    assert(constrainedAngularVelocities != nullptr);
-
-    mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
-    mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
-}
-
-// Set the constrained positions/orientations arrays
-inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
-                                                           Quaternion* constrainedOrientations) {
-
-    assert(constrainedPositions != nullptr);
-    assert(constrainedOrientations != nullptr);
-
-    mConstraintSolverData.positions = constrainedPositions;
-    mConstraintSolverData.orientations = constrainedOrientations;
-}
-
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler

src/engine/ContactSolver.cpp

@@ -30,6 +30,9 @@
 #include "constraint/ContactPoint.h"
 #include "utils/Profiler.h"
 #include "engine/Island.h"
+#include "components/BodyComponents.h"
+#include "components/DynamicsComponents.h"
+#include "components/ProxyShapeComponents.h"
 #include "collision/ContactManifold.h"
 
 using namespace reactphysics3d;
@@ -41,11 +44,12 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings)
+ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, DynamicsComponents& dynamicsComponents,
-              :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
+                             ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings)
-               mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
+              :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr),
-               mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
+               mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents),
-               mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
+               mDynamicsComponents(dynamicsComponents), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true),
+               mWorldSettings(worldSettings) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -54,23 +58,18 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings&
 }
 
 // Initialize the contact constraints
-void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
+void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
+
+    mAllContactManifolds = contactManifolds;
+    mAllContactPoints = contactPoints;
 
     RP3D_PROFILE("ContactSolver::init()", mProfiler);
 
     mTimeStep = timeStep;
 
     // TODO : Try not to count manifolds and contact points here
-    uint nbContactManifolds = 0;
+    uint nbContactManifolds = mAllContactManifolds->size();
-    uint nbContactPoints = 0;
+    uint nbContactPoints = mAllContactPoints->size();
-    for (uint i = 0; i < nbIslands; i++) {
-        uint nbManifoldsInIsland = islands[i]->getNbContactManifolds();
-        nbContactManifolds += nbManifoldsInIsland;
-
-        for (uint j=0; j < nbManifoldsInIsland; j++) {
-            nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints();
-        }
-    }
 
     mNbContactManifolds = 0;
     mNbContactPoints = 0;
@@ -90,10 +89,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
     assert(mContactConstraints != nullptr);
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
+    for (uint i = 0; i < mIslands.getNbIslands(); i++) {
 
-        if (islands[islandIndex]->getNbContactManifolds() > 0) {
+        if (mIslands.nbContactManifolds[i] > 0) {
-            initializeForIsland(islands[islandIndex]);
+            initializeForIsland(i);
         }
     }
 
@@ -102,83 +101,87 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
 }
 
 // Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(Island* island) {
+void ContactSolver::initializeForIsland(uint islandIndex) {
 
     RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.bodyEntities[islandIndex].size() > 0);
-    assert(island->getNbBodies() > 0);
+    assert(mIslands.nbContactManifolds[islandIndex] > 0);
-    assert(island->getNbContactManifolds() > 0);
-    assert(mSplitLinearVelocities != nullptr);
-    assert(mSplitAngularVelocities != nullptr);
 
     // For each contact manifold of the island
-    ContactManifold** contactManifolds = island->getContactManifolds();
+    uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
-    for (uint i=0; i<island->getNbContactManifolds(); i++) {
+    uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
+    for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
 
-        ContactManifold* externalManifold = contactManifolds[i];
+        ContactManifold& externalManifold = (*mAllContactManifolds)[m];
 
-        assert(externalManifold->getNbContactPoints() > 0);
+        assert(externalManifold.getNbContactPoints() > 0);
 
         // Get the two bodies of the contact
-        RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getBody1());
+        RigidBody* body1 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity1));
-        RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getBody2());
+        RigidBody* body2 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity2));
         assert(body1 != nullptr);
         assert(body2 != nullptr);
+        assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
+        assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
 
         // Get the two contact shapes
-        const ProxyShape* shape1 = externalManifold->getShape1();
+        // TODO : Do we really need to get the proxy-shape here
-        const ProxyShape* shape2 = externalManifold->getShape2();
+        const ProxyShape* shape1 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity1);
+        const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2);
 
         // Get the position of the two bodies
-        const Vector3& x1 = body1->mCenterOfMassWorld;
+        const Vector3& x1 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity1);
-        const Vector3& x2 = body2->mCenterOfMassWorld;
+        const Vector3& x2 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity2);
 
         // Initialize the internal contact manifold structure using the external
         // contact manifold
         new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
-        mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
+        mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity());
-        mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
+        mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity());
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
-        mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
+        mContactConstraints[mNbContactManifolds].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity());
-        mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
+        mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity());
-        mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
+        mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints();
         mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
         mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
-        mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
+        mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
         mContactConstraints[mNbContactManifolds].normal.setToZero();
         mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
         mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
 
         // Get the velocities of the bodies
-        const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
+        const Vector3& v1 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity1);
-        const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
+        const Vector3& w1 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity1);
-        const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
+        const Vector3& v2 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity2);
-        const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
+        const Vector3& w2 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity2);
 
         // For each  contact point of the contact manifold
-        ContactPoint* externalContact = externalManifold->getContactPoints();
+        assert(externalManifold.getNbContactPoints() > 0);
-        assert(externalContact != nullptr);
+        uint contactPointsStartIndex = externalManifold.mContactPointsIndex;
-        while (externalContact != nullptr) {
+        uint nbContactPoints = externalManifold.mNbContactPoints;
+        for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
+
+            ContactPoint& externalContact = (*mAllContactPoints)[c];
 
             // Get the contact point on the two bodies
-            Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1();
+            Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact.getLocalPointOnShape1();
-            Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2();
+            Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact.getLocalPointOnShape2();
 
             new (mContactPoints + mNbContactPoints) ContactPointSolver();
-            mContactPoints[mNbContactPoints].externalContact = externalContact;
+            mContactPoints[mNbContactPoints].externalContact = &externalContact;
-            mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
+            mContactPoints[mNbContactPoints].normal = externalContact.getNormal();
             mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
             mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
             mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
             mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
             mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
             mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
-            mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
+            mContactPoints[mNbContactPoints].penetrationDepth = externalContact.getPenetrationDepth();
-            mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
+            mContactPoints[mNbContactPoints].isRestingContact = externalContact.getIsRestingContact();
-            externalContact->setIsRestingContact(true);
+            externalContact.setIsRestingContact(true);
-            mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
+            mContactPoints[mNbContactPoints].penetrationImpulse = externalContact.getPenetrationImpulse();
             mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
 
             mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
@@ -240,8 +243,6 @@ void ContactSolver::initializeForIsland(Island* island) {
             mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
 
             mNbContactPoints++;
-
-            externalContact = externalContact->getNext();
         }
 
         mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
@@ -252,13 +253,13 @@ void ContactSolver::initializeForIsland(Island* island) {
         mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
         mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
         mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
-        mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
+        mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold.getFrictionVector1();
-        mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
+        mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold.getFrictionVector2();
 
         // Initialize the accumulated impulses with the previous step accumulated impulses
-        mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
+        mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold.getFrictionImpulse1();
-        mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
+        mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold.getFrictionImpulse2();
-        mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+        mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.getFrictionTwistImpulse();
 
         // Compute the inverse K matrix for the rolling resistance constraint
         bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
@@ -343,6 +344,7 @@ void ContactSolver::warmStart() {
 
         for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
 
+
             // If it is not a new contact (this contact was already existing at last time step)
             if (mContactPoints[contactPointIndex].isRestingContact) {
 
@@ -354,22 +356,22 @@ void ContactSolver::warmStart() {
                 Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
                                            mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
                                            mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
-                mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
-                mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
-                mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
 
-                mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
 
                 // Update the velocities of the body 2 by applying the impulse P
-                mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
-                mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
-                mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
+                mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
 
-                mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
             }
             else {  // If it is a new contact point
 
@@ -409,12 +411,12 @@ void ContactSolver::warmStart() {
                                         mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
-            mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Second friction constraint at the center of the contact manifold ----- //
 
@@ -430,18 +432,18 @@ void ContactSolver::warmStart() {
             angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-            mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-            mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
 
-            mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-            mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-            mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
 
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Twist friction constraint at the center of the contact manifold ------ //
 
@@ -455,10 +457,10 @@ void ContactSolver::warmStart() {
             angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Rolling resistance at the center of the contact manifold ------ //
 
@@ -466,10 +468,10 @@ void ContactSolver::warmStart() {
             angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
         }
         else {  // If it is a new contact manifold
 
@@ -497,10 +499,10 @@ void ContactSolver::solve() {
         decimal sumPenetrationImpulse = 0.0;
 
         // Get the constrained velocities
-        const Vector3& v1 = mLinearVelocities[mContactConstraints[c].indexBody1];
+        const Vector3& v1 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-        const Vector3& w1 = mAngularVelocities[mContactConstraints[c].indexBody1];
+        const Vector3& w1 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-        const Vector3& v2 = mLinearVelocities[mContactConstraints[c].indexBody2];
+        const Vector3& v2 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
-        const Vector3& w2 = mAngularVelocities[mContactConstraints[c].indexBody2];
+        const Vector3& w2 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
 
         for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
 
@@ -543,22 +545,22 @@ void ContactSolver::solve() {
                                   mContactPoints[contactPointIndex].normal.z * deltaLambda);
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-            mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-            mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
 
-            mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
-            mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
-            mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-            mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-            mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+            mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
 
-            mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
-            mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
-            mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
 
             sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
 
@@ -566,10 +568,10 @@ void ContactSolver::solve() {
             if (mIsSplitImpulseActive) {
 
                 // Split impulse (position correction)
-                const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1];
+                const Vector3& v1Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-                const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
+                const Vector3& w1Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-                const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
+                const Vector3& v2Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
-                const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
+                const Vector3& w2Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
 
                 //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
                 Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
@@ -594,22 +596,22 @@ void ContactSolver::solve() {
                                       mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-                mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-                mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
 
-                mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-                mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-                mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+                mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
 
-                mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
+                mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
             }
 
             contactPointIndex++;
@@ -650,18 +652,18 @@ void ContactSolver::solve() {
                                     mContactConstraints[c].r2CrossT1.z * deltaLambda);
 
         // Update the velocities of the body 1 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
 
-        mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
 
-        mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Second friction constraint at the center of the contact manifol ----- //
 
@@ -699,16 +701,16 @@ void ContactSolver::solve() {
         angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
-        mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+        mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
-        mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Twist friction constraint at the center of the contact manifol ------ //
 
@@ -731,10 +733,10 @@ void ContactSolver::solve() {
         angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // --------- Rolling resistance constraint at the center of the contact manifold --------- //
 
@@ -752,10 +754,10 @@ void ContactSolver::solve() {
             deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
+            mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
         }
     }
 }

src/engine/ContactSolver.h

@@ -30,6 +30,7 @@
 #include "configuration.h"
 #include "mathematics/Vector3.h"
 #include "mathematics/Matrix3x3.h"
+#include "engine/Islands.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -42,6 +43,9 @@ class MemoryManager;
 class Profiler;
 class Island;
 class RigidBody;
+class BodyComponents;
+class DynamicsComponents;
+class ProxyShapeComponents;
 
 // Class Contact Solver
 /**
@@ -170,11 +174,11 @@ class ContactSolver {
             /// Pointer to the external contact manifold
             ContactManifold* externalContactManifold;
 
-            /// Index of body 1 in the constraint solver
+            /// Index of body 1 in the dynamics components arrays
-            int32 indexBody1;
+            uint32 dynamicsComponentIndexBody1;
 
-            /// Index of body 2 in the constraint solver
+            /// Index of body 2 in the dynamics components arrays
-            int32 indexBody2;
+            uint32 dynamicsComponentIndexBody2;
 
             /// Inverse of the mass of body 1
             decimal massInverseBody1;
@@ -279,19 +283,15 @@ class ContactSolver {
         /// Memory manager
         MemoryManager& mMemoryManager;
 
-        /// Split linear velocities for the position contact solver (split impulse)
-        Vector3* mSplitLinearVelocities;
-
-        /// Split angular velocities for the position contact solver (split impulse)
-        Vector3* mSplitAngularVelocities;
-
         /// Current time step
         decimal mTimeStep;
 
         /// Contact constraints
+        // TODO : Use List<> here
         ContactManifoldSolver* mContactConstraints;
 
         /// Contact points
+        // TODO : Use List<> here
         ContactPointSolver* mContactPoints;
 
         /// Number of contact point constraints
@@ -300,11 +300,24 @@ class ContactSolver {
         /// Number of contact constraints
         uint mNbContactManifolds;
 
-        /// Array of linear velocities
+        /// Reference to the islands
-        Vector3* mLinearVelocities;
+        Islands& mIslands;
 
-        /// Array of angular velocities
+        /// Pointer to the list of contact manifolds from narrow-phase
-        Vector3* mAngularVelocities;
+        List<ContactManifold>* mAllContactManifolds;
+
+        /// Pointer to the list of contact points from narrow-phase
+        List<ContactPoint>* mAllContactPoints;
+
+        /// Reference to the body components
+        BodyComponents& mBodyComponents;
+
+        /// Reference to the dynamics components
+        DynamicsComponents& mDynamicsComponents;
+
+        /// Reference to the proxy-shapes components
+        // TODO : Do we really need to use this ?
+        ProxyShapeComponents& mProxyShapeComponents;
 
         /// True if the split impulse position correction is active
         bool mIsSplitImpulseActive;
@@ -346,24 +359,18 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings);
+        ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
+                      DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents,
+                      const WorldSettings& worldSettings);
 
         /// Destructor
         ~ContactSolver() = default;
 
         /// Initialize the contact constraints
-        void init(Island** islands, uint nbIslands, decimal timeStep);
+        void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
 
         /// Initialize the constraint solver for a given island
-        void initializeForIsland(Island* island);
+        void initializeForIsland(uint islandIndex);
-
-        /// Set the split velocities arrays
-        void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
-                                      Vector3* splitAngularVelocities);
-
-        /// Set the constrained velocities arrays
-        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-                                            Vector3* constrainedAngularVelocities);
 
         /// Store the computed impulses to use them to
         /// warm start the solver at the next iteration
@@ -386,28 +393,6 @@ class ContactSolver {
 #endif
 };
 
-// Set the split velocities arrays
-inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
-                                                    Vector3* splitAngularVelocities) {
-
-    assert(splitLinearVelocities != nullptr);
-    assert(splitAngularVelocities != nullptr);
-
-    mSplitLinearVelocities = splitLinearVelocities;
-    mSplitAngularVelocities = splitAngularVelocities;
-}
-
-// Set the constrained velocities arrays
-inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-                                                          Vector3* constrainedAngularVelocities) {
-
-    assert(constrainedLinearVelocities != nullptr);
-    assert(constrainedAngularVelocities != nullptr);
-
-    mLinearVelocities = constrainedLinearVelocities;
-    mAngularVelocities = constrainedAngularVelocities;
-}
-
 // Return true if the split impulses position correction technique is used for contacts
 inline bool ContactSolver::isSplitImpulseActive() const {
     return mIsSplitImpulseActive;

src/engine/DynamicsWorld.cpp

@@ -32,7 +32,9 @@
 #include "utils/Profiler.h"
 #include "engine/EventListener.h"
 #include "engine/Island.h"
+#include "engine/Islands.h"
 #include "collision/ContactManifold.h"
+#include "containers/Stack.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -45,21 +47,17 @@ using namespace std;
  * @param logger Pointer to the logger
  * @param profiler Pointer to the profiler
  */
-DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings,
+DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
-                             Logger* logger, Profiler* profiler)
               : CollisionWorld(worldSettings, logger, profiler),
-                mContactSolver(mMemoryManager, mConfig),
+                mIslands(mMemoryManager.getSingleFrameAllocator()),
+                mContactSolver(mMemoryManager, mIslands, mBodyComponents, mDynamicsComponents, mProxyShapesComponents, mConfig),
+                mConstraintSolver(mIslands, mDynamicsComponents),
                 mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
                 mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), 
                 mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
                 mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
-                mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
+                mIsGravityEnabled(true),  mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
-                mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
+                mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
-                mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
-                mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr),
-                mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
-                mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
-                mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
                 mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -119,14 +117,17 @@ void DynamicsWorld::update(decimal timeStep) {
     // Notify the event listener about the beginning of an internal tick
     if (mEventListener != nullptr) mEventListener->beginInternalTick();
 
-    // Reset all the contact manifolds lists of each body
-    resetContactManifoldListsOfBodies();
-
     // Compute the collision detection
     mCollisionDetection.computeCollisionDetection();
 
-    // Compute the islands (separate groups of bodies with constraints between each others)
+    // Create the islands
-    computeIslands();
+    createIslands();
+
+    // Create the actual narrow-phase contacts
+    mCollisionDetection.createContacts();
+
+    // Report the contacts to the user
+    mCollisionDetection.reportContacts();
 
     // Integrate the velocities
     integrateRigidBodiesVelocities();
@@ -151,6 +152,9 @@ void DynamicsWorld::update(decimal timeStep) {
     // Reset the external force and torque applied to the bodies
     resetBodiesForceAndTorque();
 
+    // Reset the islands
+    mIslands.clear();
+
     // Reset the single frame memory allocator
     mMemoryManager.resetFrameAllocator();
 }
@@ -162,37 +166,27 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
 
     RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
     
-    // For each island of the world
+    const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
-    for (uint i=0; i < mNbIslands; i++) {
 
-        RigidBody** bodies = mIslands[i]->getBodies();
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
 
-        // For each body of the island
+        // Get the constrained velocity
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+        Vector3 newLinVelocity = mDynamicsComponents.mConstrainedLinearVelocities[i];
+        Vector3 newAngVelocity = mDynamicsComponents.mConstrainedAngularVelocities[i];
 
-            // Get the constrained velocity
+        // Add the split impulse velocity from Contact Solver (only used
-            uint indexArray = bodies[b]->mArrayIndex;
+        // to update the position)
-            Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
+        newLinVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitLinearVelocities[i];
-            Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
+        newAngVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitAngularVelocities[i];
 
-            // Add the split impulse velocity from Contact Solver (only used
+        // Get current position and orientation of the body
-            // to update the position)
+        const Vector3& currentPosition = mDynamicsComponents.mCentersOfMassWorld[i];
-            if (mContactSolver.isSplitImpulseActive()) {
+        const Quaternion& currentOrientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation();
 
-                newLinVelocity += mSplitLinearVelocities[indexArray];
+        // Update the new constrained position and orientation of the body
-                newAngVelocity += mSplitAngularVelocities[indexArray];
+        mDynamicsComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
-            }
+        mDynamicsComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
-
+                                                          currentOrientation * decimal(0.5) * mTimeStep;
-            // Get current position and orientation of the body
-            const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
-            const Quaternion& currentOrientation = mTransformComponents.getTransform(bodies[b]->getEntity()).getOrientation();
-
-            // Update the new constrained position and orientation of the body
-            mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
-            mConstrainedOrientations[indexArray] = currentOrientation +
-                                                   Quaternion(0, newAngVelocity) *
-                                                   currentOrientation * decimal(0.5) * mTimeStep;
-        }
     }
 }
 
@@ -203,73 +197,47 @@ void DynamicsWorld::updateBodiesState() {
 
     // TODO : Make sure we compute this in a system
 
-    // For each island of the world
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-        // For each body of the island
+        // Update the linear and angular velocity of the body
-        RigidBody** bodies = mIslands[islandIndex]->getBodies();
+        mDynamicsComponents.mLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i];
+        mDynamicsComponents.mAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i];
 
-        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
+        // Update the position of the center of mass of the body
+        mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i];
 
-            uint index = bodies[b]->mArrayIndex;
+        // Update the orientation of the body
+        const Quaternion& constrainedOrientation = mDynamicsComponents.mConstrainedOrientations[i];
+        mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).setOrientation(constrainedOrientation.getUnit());
+    }
 
-            // Update the linear and angular velocity of the body
+    // Update the transform of the body (using the new center of mass and new orientation)
-            mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]);
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
-            mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]);
 
-            // Update the position of the center of mass of the body
+        Transform& transform = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]);
-            bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
+        const Vector3& centerOfMassWorld = mDynamicsComponents.mCentersOfMassWorld[i];
+        const Vector3& centerOfMassLocal = mDynamicsComponents.mCentersOfMassLocal[i];
+        transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
+    }
 
-            // Update the orientation of the body
+    // Update the world inverse inertia tensor of the body
-            mTransformComponents.getTransform(bodies[b]->getEntity()).setOrientation(mConstrainedOrientations[index].getUnit());
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
 
-            // Update the transform of the body (using the new center of mass and new orientation)
+        Matrix3x3 orientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation().getMatrix();
-            bodies[b]->updateTransformWithCenterOfMass();
+        const Matrix3x3& inverseInertiaLocalTensor = mDynamicsComponents.mInverseInertiaTensorsLocal[i];
-
+        mDynamicsComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
-            // Update the world inverse inertia tensor of the body
-            bodies[b]->updateInertiaTensorInverseWorld();
-        }
     }
 
     // Update the proxy-shapes components
     mCollisionDetection.updateProxyShapes();
 }
 
-// Initialize the bodies velocities arrays for the next simulation step.
+// Reset the split velocities of the bodies
-void DynamicsWorld::initVelocityArrays() {
+void DynamicsWorld::resetSplitVelocities() {
 
-    RP3D_PROFILE("DynamicsWorld::initVelocityArrays()", mProfiler);
+    for(uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
-
+        mDynamicsComponents.mSplitLinearVelocities[i].setToZero();
-    // Allocate memory for the bodies velocity arrays
+        mDynamicsComponents.mSplitAngularVelocities[i].setToZero();
-    uint nbBodies = mRigidBodies.size();
-
-    mSplitLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                           nbBodies * sizeof(Vector3)));
-    mSplitAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                            nbBodies * sizeof(Vector3)));
-    mConstrainedLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                                 nbBodies * sizeof(Vector3)));
-    mConstrainedAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                                  nbBodies * sizeof(Vector3)));
-    mConstrainedPositions = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                          nbBodies * sizeof(Vector3)));
-    mConstrainedOrientations = static_cast<Quaternion*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                                nbBodies * sizeof(Quaternion)));
-    assert(mSplitLinearVelocities != nullptr);
-    assert(mSplitAngularVelocities != nullptr);
-    assert(mConstrainedLinearVelocities != nullptr);
-    assert(mConstrainedAngularVelocities != nullptr);
-    assert(mConstrainedPositions != nullptr);
-    assert(mConstrainedOrientations != nullptr);
-
-    // Initialize the map of body indexes in the velocity arrays
-    uint i = 0;
-    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
-        mSplitLinearVelocities[i].setToZero();
-        mSplitAngularVelocities[i].setToZero();
-
-        (*it)->mArrayIndex = i++;
     }
 }
 
@@ -282,61 +250,55 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler);
 
-    // Initialize the bodies velocity arrays
+    // Reset the split velocities of the bodies
-    initVelocityArrays();
+    resetSplitVelocities();
 
-    // For each island of the world
+    // Integration component velocities using force/torque
-    for (uint i=0; i < mNbIslands; i++) {
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
 
-        RigidBody** bodies = mIslands[i]->getBodies();
+        assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
+        assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
 
-        // For each body of the island
+        // Integrate the external force to get the new velocity of the body
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+        mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep *
+                                                              mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i];
+        mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] +
+                                                               mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i];
+    }
 
-            // Insert the body into the map of constrained velocities
+    // Apply gravity force
-            uint indexBody = bodies[b]->mArrayIndex;
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+        // If the gravity has to be applied to this rigid body
+        if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
 
-            assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
+            // Integrate the gravity force
-            assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
+            mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep *
-
+                                                                  mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mInitMasses[i] * mGravity;
-            // Integrate the external force to get the new velocity of the body
-            mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
-                                        mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
-            mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
-                                        mTimeStep * bodies[b]->getInertiaTensorInverseWorld() *
-                                        bodies[b]->mExternalTorque;
-
-            // If the gravity has to be applied to this rigid body
-            if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
-
-                // Integrate the gravity force
-                mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse *
-                        bodies[b]->getMass() * mGravity;
-            }
-
-            // Apply the velocity damping
-            // Damping force : F_c = -c' * v (c=damping factor)
-            // Equation      : m * dv/dt = -c' * v
-            //                 => dv/dt = -c * v (with c=c'/m)
-            //                 => dv/dt + c * v = 0
-            // Solution      : v(t) = v0 * e^(-c * t)
-            //                 => v(t + dt) = v0 * e^(-c(t + dt))
-            //                              = v0 * e^(-ct) * e^(-c * dt)
-            //                              = v(t) * e^(-c * dt)
-            //                 => v2 = v1 * e^(-c * dt)
-            // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
-            //                              => e^(-x) ~ 1 - x
-            //                 => v2 = v1 * (1 - c * dt)
-            decimal linDampingFactor = bodies[b]->getLinearDamping();
-            decimal angDampingFactor = bodies[b]->getAngularDamping();
-            decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
-            decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
-            mConstrainedLinearVelocities[indexBody] *= linearDamping;
-            mConstrainedAngularVelocities[indexBody] *= angularDamping;
-
-            indexBody++;
         }
     }
+
+    // Apply the velocity damping
+    // Damping force : F_c = -c' * v (c=damping factor)
+    // Equation      : m * dv/dt = -c' * v
+    //                 => dv/dt = -c * v (with c=c'/m)
+    //                 => dv/dt + c * v = 0
+    // Solution      : v(t) = v0 * e^(-c * t)
+    //                 => v(t + dt) = v0 * e^(-c(t + dt))
+    //                              = v0 * e^(-ct) * e^(-c * dt)
+    //                              = v(t) * e^(-c * dt)
+    //                 => v2 = v1 * e^(-c * dt)
+    // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
+    //                              => e^(-x) ~ 1 - x
+    //                 => v2 = v1 * (1 - c * dt)
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+
+        const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i];
+        const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i];
+        const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
+        const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
+        mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping;
+        mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping;
+    }
 }
 
 // Solve the contacts and constraints
@@ -344,40 +306,31 @@ void DynamicsWorld::solveContactsAndConstraints() {
 
     RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler);
 
-    // Set the velocities arrays
-    mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
-    mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
-                                                  mConstrainedAngularVelocities);
-    mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
-                                                     mConstrainedAngularVelocities);
-    mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
-                                                    mConstrainedOrientations);
-
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
     // Initialize the contact solver
-    mContactSolver.init(mIslands, mNbIslands, mTimeStep);
+    mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep);
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
         // If there are constraints to solve
-        if (mIslands[islandIndex]->getNbJoints() > 0) {
+        if (mIslands.joints[islandIndex].size() > 0) {
 
             // Initialize the constraint solver
-            mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+            mConstraintSolver.initializeForIsland(mTimeStep, islandIndex);
         }
     }
 
     // For each iteration of the velocity solver
     for (uint i=0; i<mNbVelocitySolverIterations; i++) {
 
-        for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+        for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
             // Solve the constraints
-            if (mIslands[islandIndex]->getNbJoints() > 0) {
+            if (mIslands.joints[islandIndex].size() > 0) {
 
-                mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+                mConstraintSolver.solveVelocityConstraints(islandIndex);
             }
         }
 
@@ -396,17 +349,17 @@ void DynamicsWorld::solvePositionCorrection() {
     if (mJoints.size() == 0) return;
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
         // ---------- Solve the position error correction for the constraints ---------- //
 
-        if (mIslands[islandIndex]->getNbJoints() > 0) {
+        if (mIslands.joints[islandIndex].size() > 0) {
 
             // For each iteration of the position (error correction) solver
             for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
                 // Solve the position constraints
-                mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
+                mConstraintSolver.solvePositionConstraints(islandIndex);
             }
         }
     }
@@ -429,7 +382,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
     assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
 
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
-    mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero()));
+    mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition()));
 
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
@@ -479,9 +432,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
         destroyJoint(element->joint);
     }
 
-    // Reset the contact manifold list of the body
-    rigidBody->resetContactManifoldsList();
-
     // Destroy the corresponding entity and its components
     mBodyComponents.removeComponent(rigidBody->getEntity());
     mTransformComponents.removeComponent(rigidBody->getEntity());
@@ -672,123 +622,133 @@ uint DynamicsWorld::computeNextAvailableJointId() {
     return jointId;
 }
 
-// Compute the islands of awake bodies.
+// Compute the islands using potential contacts and joints
+/// We compute the islands before creating the actual contacts here because we want all
+/// the contact manifolds and contact points of the same island
+/// to be packed together into linear arrays of manifolds and contacts for better caching.
 /// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
 /// between each other. This method computes the islands at each time step as follows: For each
 /// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body
 /// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to
 /// find all the bodies that are connected with it (the bodies that share joints or contacts with
 /// it). Then, we create an island with this group of connected bodies.
-void DynamicsWorld::computeIslands() {
+void DynamicsWorld::createIslands() {
 
-    RP3D_PROFILE("DynamicsWorld::computeIslands()", mProfiler);
+    // TODO : Check if we handle kinematic bodies correctly in islands creation
 
-    uint nbBodies = mRigidBodies.size();
+    // list of contact pairs involving a non-rigid body
+    List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
 
-    // Allocate and create the array of islands pointer. This memory is allocated
+    RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler);
-    // in the single frame allocator
-    mIslands = static_cast<Island**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                             sizeof(Island*) * nbBodies));
-    mNbIslands = 0;
 
-    int nbContactManifolds = 0;
+    // Reset all the isAlreadyInIsland variables of bodies and joints
+    for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) {
 
-    // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
+        mDynamicsComponents.mIsAlreadyInIsland[b] = false;
-    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-        int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
-        nbContactManifolds += nbBodyManifolds;
     }
     for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
         (*it)->mIsAlreadyInIsland = false;
     }
 
-    // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
+    // Create a stack for the bodies to visit during the Depth First Search
-    size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
+    Stack<Entity> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator());
-    RigidBody** stackBodiesToVisit = static_cast<RigidBody**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                                      nbBytesStack));
 
-    // For each rigid body of the world
+    uint nbTotalManifolds = 0;
-    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
-        RigidBody* body = *it;
+    // For each dynamic component
+    // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should
+    //        not use a dynamic component for a static body.
+    for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
 
         // If the body has already been added to an island, we go to the next body
-        if (body->mIsAlreadyInIsland) continue;
+        if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue;
 
         // If the body is static, we go to the next body
+        // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+        CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
         if (body->getType() == BodyType::STATIC) continue;
 
-        // If the body is sleeping or inactive, we go to the next body
-        if (body->isSleeping() || !body->isActive()) continue;
-
         // Reset the stack of bodies to visit
-        uint stackIndex = 0;
+        bodyEntityIndicesToVisit.clear();
-        stackBodiesToVisit[stackIndex] = body;
+
-        stackIndex++;
+        // Add the body into the stack of bodies to visit
-        body->mIsAlreadyInIsland = true;
+        mDynamicsComponents.mIsAlreadyInIsland[b] = true;
+        bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
 
         // Create the new island
-        void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+        uint32 islandIndex = mIslands.addIsland(nbTotalManifolds);
-                                                              sizeof(Island));
-        mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
-                                                                  mMemoryManager);
 
         // While there are still some bodies to visit in the stack
-        while (stackIndex > 0) {
+        while (bodyEntityIndicesToVisit.size() > 0) {
 
             // Get the next body to visit from the stack
-            stackIndex--;
+            const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop();
-            RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex];
-            assert(bodyToVisit->isActive());
-
-            // Awake the body if it is sleeping
-            bodyToVisit->setIsSleeping(false);
 
             // Add the body into the island
-            mIslands[mNbIslands]->addBody(bodyToVisit);
+            mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity);
 
             // If the current body is static, we do not want to perform the DFS
             // search across that body
-            if (bodyToVisit->getType() == BodyType::STATIC) continue;
+            // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+            RigidBody* rigidBodyToVisit = static_cast<RigidBody*>(mBodyComponents.getBody(bodyToVisitEntity));
 
-            // For each contact manifold in which the current body is involded
+            // Awake the body if it is sleeping
-            ContactManifoldListElement* contactElement;
+            rigidBodyToVisit->setIsSleeping(false);
-            for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
-                 contactElement = contactElement->getNext()) {
 
-                ContactManifold* contactManifold = contactElement->getContactManifold();
+            if (rigidBodyToVisit->getType() == BodyType::STATIC) continue;
 
-                assert(contactManifold->getNbContactPoints() > 0);
+            // If the body is involved in contacts with other bodies
+            auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity);
+            if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) {
 
-                // Check if the current contact manifold has already been added into an island
+                // For each contact pair in which the current body is involded
-                if (contactManifold->isAlreadyInIsland()) continue;
+                List<uint>& contactPairs = itBodyContactPairs->second;
+                for (uint p=0; p < contactPairs.size(); p++) {
 
-                // Get the other body of the contact manifold
+                    ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]];
-                RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
+                    assert(pair.potentialContactManifoldsIndices.size() > 0);
-                RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
 
-                // If the colliding body is a RigidBody (and not a CollisionBody instead)
+                    // Check if the current contact pair has already been added into an island
-                if (body1 != nullptr && body2 != nullptr) {
+                    if (pair.isAlreadyInIsland) continue;
 
-                    // Add the contact manifold into the island
+                    // Get the other body of the contact manifold
-                    mIslands[mNbIslands]->addContactManifold(contactManifold);
+                    // TODO : Maybe avoid those casts here
-                    contactManifold->mIsAlreadyInIsland = true;
+                    RigidBody* body1 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body1Entity));
+                    RigidBody* body2 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body2Entity));
 
-                    RigidBody* otherBody = (body1->getId() == bodyToVisit->getId()) ? body2 : body1;
+                    // If the colliding body is a RigidBody (and not a CollisionBody instead)
+                    if (body1 != nullptr && body2 != nullptr) {
 
-                    // Check if the other body has already been added to the island
+                        nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
-                    if (otherBody->mIsAlreadyInIsland) continue;
 
-                    // Insert the other body into the stack of bodies to visit
+                        // Add the pair into the list of pair to process to create contacts
-                    stackBodiesToVisit[stackIndex] = otherBody;
+                        mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
-                    stackIndex++;
+
-                    otherBody->mIsAlreadyInIsland = true;
+                        // Add the contact manifold into the island
+                        mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
+                        pair.isAlreadyInIsland = true;
+
+                        const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
+
+                        // Check if the other body has already been added to the island
+                        if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
+
+                        // Insert the other body into the stack of bodies to visit
+                        bodyEntityIndicesToVisit.push(otherBodyEntity);
+                        mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
+                    }
+                    else {
+
+                        // Add the contact pair index in the list of contact pairs that won't be part of islands
+                        nonRigidBodiesContactPairs.add(pair.contactPairIndex);
+                        pair.isAlreadyInIsland = true;
+                    }
                 }
             }
 
             // For each joint in which the current body is involved
             JointListElement* jointElement;
-            for (jointElement = bodyToVisit->mJointsList; jointElement != nullptr;
+            for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr;
                  jointElement = jointElement->next) {
 
                 Joint* joint = jointElement->joint;
@@ -797,35 +757,41 @@ void DynamicsWorld::computeIslands() {
                 if (joint->isAlreadyInIsland()) continue;
 
                 // Add the joint into the island
-                mIslands[mNbIslands]->addJoint(joint);
+                mIslands.joints[islandIndex].add(joint);
                 joint->mIsAlreadyInIsland = true;
 
-                // Get the other body of the contact manifold
+                const Entity body1Entity = joint->getBody1()->getEntity();
-                RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
+                const Entity body2Entity = joint->getBody2()->getEntity();
-                RigidBody* body2 = static_cast<RigidBody*>(joint->getBody2());
+                const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity;
-                RigidBody* otherBody = (body1->getId() == bodyToVisit->getId()) ? body2 : body1;
 
                 // Check if the other body has already been added to the island
-                if (otherBody->mIsAlreadyInIsland) continue;
+                if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
 
                 // Insert the other body into the stack of bodies to visit
-                stackBodiesToVisit[stackIndex] = otherBody;
+                bodyEntityIndicesToVisit.push(otherBodyEntity);
-                stackIndex++;
+                mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
-                otherBody->mIsAlreadyInIsland = true;
             }
         }
 
         // Reset the isAlreadyIsland variable of the static bodies so that they
         // can also be included in the other islands
-        for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
+        for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) {
 
-            if (mIslands[mNbIslands]->mBodies[i]->getType() == BodyType::STATIC) {
+            // If the body is static, we go to the next body
-                mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
+            // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+            CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[j]));
+            if (body->getType() == BodyType::STATIC) {
+                mDynamicsComponents.mIsAlreadyInIsland[j] = false;
             }
         }
+    }
 
-        mNbIslands++;
+    // Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations
-     }
+    mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs);
+
+    assert(mCollisionDetection.mCurrentContactPairs->size() == mCollisionDetection.mContactPairsIndicesOrderingForContacts.size());
+
+    mCollisionDetection.mMapBodyToContactPairs.clear(true);
 }
 
 // Put bodies to sleep if needed.
@@ -839,32 +805,36 @@ void DynamicsWorld::updateSleepingBodies() {
     const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
 
     // For each island of the world
-    for (uint i=0; i<mNbIslands; i++) {
+    for (uint i=0; i<mIslands.getNbIslands(); i++) {
 
         decimal minSleepTime = DECIMAL_LARGEST;
 
         // For each body of the island
-        RigidBody** bodies = mIslands[i]->getBodies();
+        for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+
+            const Entity bodyEntity = mIslands.bodyEntities[i][b];
+
+            // TODO : We should not have to do this cast here to get type of body
+            CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
 
             // Skip static bodies
-            if (bodies[b]->getType() == BodyType::STATIC) continue;
+            if (body->getType() == BodyType::STATIC) continue;
 
             // If the body is velocity is large enough to stay awake
-            if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
+            if (mDynamicsComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare ||
-                bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
+                mDynamicsComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare ||
-                !bodies[b]->isAllowedToSleep()) {
+                !body->isAllowedToSleep()) {
 
                 // Reset the sleep time of the body
-                bodies[b]->mSleepTime = decimal(0.0);
+                body->mSleepTime = decimal(0.0);
                 minSleepTime = decimal(0.0);
             }
-            else {  // If the body velocity is bellow the sleeping velocity threshold
+            else {  // If the body velocity is below the sleeping velocity threshold
 
                 // Increase the sleep time
-                bodies[b]->mSleepTime += mTimeStep;
+                body->mSleepTime += mTimeStep;
-                if (bodies[b]->mSleepTime < minSleepTime) {
+                if (body->mSleepTime < minSleepTime) {
-                    minSleepTime = bodies[b]->mSleepTime;
+                    minSleepTime = body->mSleepTime;
                 }
             }
         }
@@ -875,8 +845,11 @@ void DynamicsWorld::updateSleepingBodies() {
         if (minSleepTime >= mTimeBeforeSleep) {
 
             // Put all the bodies of the island to sleep
-            for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+            for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
-                bodies[b]->setIsSleeping(true);
+
+                const Entity bodyEntity = mIslands.bodyEntities[i][b];
+                CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
+                body->setIsSleeping(true);
             }
         }
     }
@@ -906,33 +879,3 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::World,
              "Dynamics World: isSleepingEnabled=" + (isSleepingEnabled ? std::string("true") : std::string("false")) );
 }
-
-// Return the list of all contacts of the world
-/**
- * @return A pointer to the first contact manifold in the linked-list of manifolds
- */
-List<const ContactManifold*> DynamicsWorld::getContactsList() {
-
-    List<const ContactManifold*> contactManifolds(mMemoryManager.getPoolAllocator());
-
-    // For each currently overlapping pair of bodies
-    for (auto it = mCollisionDetection.mOverlappingPairs.begin();
-         it != mCollisionDetection.mOverlappingPairs.end(); ++it) {
-
-        OverlappingPair* pair = it->second;
-
-        // For each contact manifold of the pair
-        const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
-        ContactManifold* manifold = manifoldSet.getContactManifolds();
-        while (manifold != nullptr) {
-
-            // Get the contact manifold
-            contactManifolds.add(manifold);
-
-            manifold = manifold->getNext();
-        }
-    }
-
-    // Return all the contact manifold
-    return contactManifolds;
-}

src/engine/DynamicsWorld.h

@@ -33,6 +33,7 @@
 #include "utils/Logger.h"
 #include "engine/ContactSolver.h"
 #include "components/DynamicsComponents.h"
+#include "engine/Islands.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -54,6 +55,9 @@ class DynamicsWorld : public CollisionWorld {
 
         // -------------------- Attributes -------------------- //
 
+        /// All the islands of bodies of the current frame
+        Islands mIslands;
+
         /// Contact solver
         ContactSolver mContactSolver;
 
@@ -84,32 +88,6 @@ class DynamicsWorld : public CollisionWorld {
         /// True if the gravity force is on
         bool mIsGravityEnabled;
 
-        /// Array of constrained linear velocities (state of the linear velocities
-        /// after solving the constraints)
-        Vector3* mConstrainedLinearVelocities;
-
-        /// Array of constrained angular velocities (state of the angular velocities
-        /// after solving the constraints)
-        Vector3* mConstrainedAngularVelocities;
-
-        /// Split linear velocities for the position contact solver (split impulse)
-        Vector3* mSplitLinearVelocities;
-
-        /// Split angular velocities for the position contact solver (split impulse)
-        Vector3* mSplitAngularVelocities;
-
-        /// Array of constrained rigid bodies position (for position error correction)
-        Vector3* mConstrainedPositions;
-
-        /// Array of constrained rigid bodies orientation (for position error correction)
-        Quaternion* mConstrainedOrientations;
-
-        /// Number of islands in the world
-        uint mNbIslands;
-
-        /// Array with all the islands of awaken bodies
-        Island** mIslands;
-
         /// Sleep linear velocity threshold
         decimal mSleepLinearVelocity;
 
@@ -134,8 +112,8 @@ class DynamicsWorld : public CollisionWorld {
         /// Reset the external force and torque applied to the bodies
         void resetBodiesForceAndTorque();
 
-        /// Initialize the bodies velocities arrays for the next simulation step.
+        /// Reset the split velocities of the bodies
-        void initVelocityArrays();
+        void resetSplitVelocities();
 
         /// Integrate the velocities of rigid bodies.
         void integrateRigidBodiesVelocities();
@@ -149,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute the islands of awake bodies.
         void computeIslands();
 
+        /// Compute the islands using potential contacts and joints and create the actual contacts.
+        void createIslands();
+
         /// Update the postion/orientation of the bodies
         void updateBodiesState();
 
@@ -259,9 +240,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Set an event listener object to receive events callbacks.
         void setEventListener(EventListener* eventListener);
 
-        /// Return the list of all contacts of the world
-        List<const ContactManifold*> getContactsList();
-
         // -------------------- Friendship -------------------- //
 
         friend class RigidBody;
@@ -271,10 +249,9 @@ class DynamicsWorld : public CollisionWorld {
 inline void DynamicsWorld::resetBodiesForceAndTorque() {
 
     // For each body of the world
-    List<RigidBody*>::Iterator it;
+    for (uint32 i=0; i < mDynamicsComponents.getNbComponents(); i++) {
-    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        mDynamicsComponents.mExternalForces[i].setToZero();
-        (*it)->mExternalForce.setToZero();
+        mDynamicsComponents.mExternalTorques[i].setToZero();
-        (*it)->mExternalTorque.setToZero();
     }
 }
 

src/engine/EventListener.h

@@ -39,7 +39,7 @@ namespace reactphysics3d {
  * new event listener class to the physics world using the DynamicsWorld::setEventListener()
  * method.
  */
-class EventListener {
+class EventListener : public CollisionCallback {
 
     public :
 
@@ -47,20 +47,22 @@ class EventListener {
         EventListener() = default;
 
         /// Destructor
-        virtual ~EventListener() = default;
+        virtual ~EventListener() override = default;
 
-        /// Called when a new contact point is found between two bodies
+        /// Called when some contacts occur
         /**
-         * @param contact Information about the contact
+         * @param collisionInfo Information about the contacts
          */
-        virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {}
+        virtual void onContact(const CollisionCallback::CallbackData& callbackData) override {}
 
+        // TODO : Remove this method (no internal steps anymore)
         /// Called at the beginning of an internal tick of the simulation step.
         /// Each time the DynamicsWorld::update() method is called, the physics
         /// engine will do several internal simulation steps. This method is
         /// called at the beginning of each internal simulation step.
         virtual void beginInternalTick() {}
 
+        // TODO : Remove this method (no internal steps anymore)
         /// Called at the end of an internal tick of the simulation step.
         /// Each time the DynamicsWorld::update() metho is called, the physics
         /// engine will do several internal simulation steps. This method is

src/engine/Islands.h

@@ -0,0 +1,120 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_ISLANDS_H
+#define REACTPHYSICS3D_ISLANDS_H
+
+// Libraries
+#include "configuration.h"
+#include "containers/List.h"
+#include "engine/Entity.h"
+#include "constraint/Joint.h"
+
+namespace reactphysics3d {
+
+// Declarations
+
+// Structure Islands
+/**
+ * This class contains all the islands of bodies during a frame.
+ * An island represent an isolated group of awake bodies that are connected with each other by
+ * some contraints (contacts or joints).
+ */
+struct Islands {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Reference to the memory allocator
+        MemoryAllocator& memoryAllocator;
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+
+        /// For each island, index of the first contact manifold of the island in the array of contact manifolds
+        List<uint> contactManifoldsIndices;
+
+        /// 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<List<Entity>> bodyEntities;
+
+        // TODO : Use joints entities here and not pointers
+        /// For each island, list of the joints that are part of the island
+        List<List<Joint*>> joints;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Islands(MemoryAllocator& allocator)
+            :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), bodyEntities(allocator),
+             joints(allocator) {
+
+        }
+
+        /// Destructor
+        ~Islands() = default;
+
+        /// Assignment operator
+        Islands& operator=(const Islands& island) = default;
+
+        /// Copy-constructor
+        Islands(const Islands& island) = default;
+
+        /// Return the number of islands
+        uint32 getNbIslands() const {
+            return static_cast<uint32>(contactManifoldsIndices.size());
+        }
+
+        /// Add an island and return its index
+        uint32 addIsland(uint32 contactManifoldStartIndex) {
+
+            uint32 islandIndex = contactManifoldsIndices.size();
+
+            contactManifoldsIndices.add(contactManifoldStartIndex);
+            nbContactManifolds.add(0);
+            bodyEntities.add(List<Entity>(memoryAllocator));
+            joints.add(List<Joint*>(memoryAllocator));
+
+            return islandIndex;
+        }
+
+        /// Clear all the islands
+        void clear() {
+
+            contactManifoldsIndices.clear(true);
+            nbContactManifolds.clear(true);
+            bodyEntities.clear(true);
+            joints.clear(true);
+        }
+};
+
+}
+
+#endif

src/engine/OverlappingPair.cpp

@@ -35,7 +35,7 @@ using namespace reactphysics3d;
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator,
                                  const WorldSettings& worldSettings)
-                : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings),
+                : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mProxyShape1(shape1), mProxyShape2(shape2),
                   mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
                   mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings) {
     

src/engine/OverlappingPair.h

@@ -27,7 +27,6 @@
 #define	REACTPHYSICS3D_OVERLAPPING_PAIR_H
 
 // Libraries
-#include "collision/ContactManifoldSet.h"
 #include "collision/ProxyShape.h"
 #include "containers/Map.h"
 #include "containers/Pair.h"
@@ -110,8 +109,9 @@ class OverlappingPair {
         /// Pair ID
         OverlappingPairId mPairID;
 
-        /// Set of persistent contact manifolds
+        // TODO : Replace this by entities
-        ContactManifoldSet mContactManifoldSet;
+        ProxyShape* mProxyShape1;
+        ProxyShape* mProxyShape2;
 
         /// Persistent memory allocator
         MemoryAllocator& mPersistentAllocator;
@@ -146,6 +146,9 @@ class OverlappingPair {
         /// Deleted assignment operator
         OverlappingPair& operator=(const OverlappingPair& pair) = delete;
 
+        /// Return the Id of the pair
+        OverlappingPairId getId() const;
+
         /// Return the pointer to first proxy collision shape
         ProxyShape* getShape1() const;
 
@@ -155,30 +158,12 @@ class OverlappingPair {
         /// Return the last frame collision info
         LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds);
 
-        /// Return the a reference to the contact manifold set
-        const ContactManifoldSet& getContactManifoldSet();
-
-        /// Add potential contact-points from narrow-phase into potential contact manifolds
-        void addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
-
         /// Return a reference to the temporary memory allocator
         MemoryAllocator& getTemporaryAllocator();
 
         /// Return true if one of the shapes of the pair is a concave shape
         bool hasConcaveShape() const;
 
-		/// Return true if the overlapping pair has contact manifolds with contacts
-		bool hasContacts() const;
-
-        /// Make the contact manifolds and contact points obsolete
-        void makeContactsObsolete();
-
-        /// Clear the obsolete contact manifold and contact points
-        void clearObsoleteManifoldsAndContactPoints();
-
-        /// Reduce the contact manifolds that have too many contact points
-        void reduceContactManifolds();
-
         /// Add a new last frame collision info if it does not exist for the given shapes already
         LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2);
 
@@ -202,14 +187,19 @@ class OverlappingPair {
         friend class DynamicsWorld;
 };
 
+// Return the Id of the pair
+inline OverlappingPair::OverlappingPairId OverlappingPair::getId() const {
+    return mPairID;
+}
+
 // Return the pointer to first body
 inline ProxyShape* OverlappingPair::getShape1() const {
-    return mContactManifoldSet.getShape1();
+    return mProxyShape1;
 }
 
 // Return the pointer to second body
 inline ProxyShape* OverlappingPair::getShape2() const {
-    return mContactManifoldSet.getShape2();
+    return mProxyShape2;
 }                
 
 // Return the last frame collision info for a given shape id or nullptr if none is found
@@ -222,17 +212,6 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeI
     return nullptr;
 }
 
-// Return the contact manifold
-inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
-    return mContactManifoldSet;
-}
-
-// Make the contact manifolds and contact points obsolete
-inline void OverlappingPair::makeContactsObsolete() {
-
-    mContactManifoldSet.makeContactsObsolete();
-}
-
 // Return the pair of bodies index
 inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) {
     assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0);
@@ -268,31 +247,11 @@ inline bool OverlappingPair::hasConcaveShape() const {
            !getShape2()->getCollisionShape()->isConvex();
 }
 
-// Return true if the overlapping pair has contact manifolds with contacts
-inline bool OverlappingPair::hasContacts() const {
-	return mContactManifoldSet.getContactManifolds() != nullptr;
-}
-
-// Clear the obsolete contact manifold and contact points
-inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
-    mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
-}
-
-// Reduce the contact manifolds that have too many contact points
-inline void OverlappingPair::reduceContactManifolds() {
-   mContactManifoldSet.reduce();
-}
-
 // Return the last frame collision info for a given pair of shape ids
 inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const {
     return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)];
 }
 
-// Create a new potential contact manifold using contact-points from narrow-phase
-inline void OverlappingPair::addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
-    mContactManifoldSet.addContactPoints(narrowPhaseInfoBatch, batchIndex);
-}
-
 }
 
 #endif

src/memory/DefaultAllocator.h

@@ -50,6 +50,10 @@ class DefaultAllocator : public MemoryAllocator {
         /// Allocate memory of a given size (in bytes) and return a pointer to the
         /// allocated memory.
         virtual void* allocate(size_t size) override {
+
+            // TODO : Make sure to reduce the calls to default allocator is not called within a frame. For instance by a call
+            //        to a pool allocator with size too large
+
             return malloc(size);
         }
 

src/systems/BroadPhaseSystem.cpp

@@ -52,8 +52,8 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, Proxy
 }
 
 // Return true if the two broad-phase collision shapes are overlapping
-bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1,
+// TODO : Use proxy-shape entities in parameters
-                                                       const ProxyShape* shape2) const {
+bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const {
 
     if (shape1->getBroadPhaseId() == -1 || shape2->getBroadPhaseId() == -1) return false;
 

src/systems/BroadPhaseSystem.h

@@ -186,7 +186,7 @@ class BroadPhaseSystem {
         void reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int>& overlappingNodes) const;
 
         /// Compute all the overlapping pairs of collision shapes
-        void computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int> >& overlappingNodes);
+        void computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int>>& overlappingNodes);
 
         /// Return the proxy shape corresponding to the broad-phase node id in parameter
         ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;

testbed/scenes/collisiondetection/CollisionDetectionScene.cpp

@@ -35,7 +35,7 @@ using namespace collisiondetectionscene;
 // Constructor
 CollisionDetectionScene::CollisionDetectionScene(const std::string& name, EngineSettings& settings)
        : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
-         mContactManager(mPhongShader, mMeshFolderPath),
+         mContactManager(mPhongShader, mMeshFolderPath, mContactPoints),
          mAreNormalsDisplayed(false) {
 
     mSelectedShapeIndex = 0;
@@ -160,6 +160,8 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
 // Reset the scene
 void CollisionDetectionScene::reset() {
 
+    SceneDemo::reset();
+
     mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(15, 5, 0), rp3d::Quaternion::identity()));
     mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(0, 6, 0), rp3d::Quaternion::identity()));
     mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-8, 7, 0), rp3d::Quaternion::identity()));
@@ -206,8 +208,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
     mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody());
 	delete mHeightField;
 
-    mContactManager.resetPoints();
-
     // Destroy the static data for the visual contact points
     VisualContactPoint::destroyStaticData();
 
@@ -218,9 +218,9 @@ CollisionDetectionScene::~CollisionDetectionScene() {
 // Take a step for the simulation
 void CollisionDetectionScene::update() {
 
-    mContactManager.resetPoints();
+    mContactPoints.clear();
 
-    mPhysicsWorld->testCollision(&mContactManager);
+    mPhysicsWorld->testCollision(mContactManager);
 
     SceneDemo::update();
 }
@@ -313,38 +313,33 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i
     return false;
 }
 
-// This method will be called for each reported contact point
+// This method is called when some contacts occur
-void ContactManager::notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) {
+void ContactManager::onContact(const CallbackData& callbackData) {
 
-    // For each contact manifold
+    // For each contact pair
-    rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements;
+    for (uint p=0; p < callbackData.getNbContactPairs(); p++) {
-    while (manifoldElement != nullptr) {
 
-    // Get the contact manifold
+        ContactPair contactPair = callbackData.getContactPair(p);
-    rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold();
 
-    // For each contact point
+        // For each contact point of the contact pair
-    rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints();
+        for (uint c=0; c < contactPair.getNbContactPoints(); c++) {
-    while (contactPoint != nullptr) {
 
-        // Contact normal
+            ContactPoint contactPoint = contactPair.getContactPoint(c);
-        rp3d::Vector3 normal = contactPoint->getNormal();
-        openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
 
-        rp3d::Vector3 point1 = contactPoint->getLocalPointOnShape1();
+            // Contact normal
-        point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+            rp3d::Vector3 normal = contactPoint.getWorldNormal();
+            openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
 
-        openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+            rp3d::Vector3 point1 = contactPoint.getLocalPointOnShape1();
-        mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
+            point1 = contactPair.getProxyShape1()->getLocalToWorldTransform() * point1;
 
-        rp3d::Vector3 point2 = contactPoint->getLocalPointOnShape2();
+            openglframework::Vector3 position1(point1.x, point1.y, point1.z);
-        point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
+            mContactPoints.push_back(SceneContactPoint(position1, contactNormal, openglframework::Color::red()));
-        openglframework::Vector3 position2(point2.x, point2.y, point2.z);
-        mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
 
-        contactPoint = contactPoint->getNext();
+            rp3d::Vector3 point2 = contactPoint.getLocalPointOnShape2();
-    }
+            point2 = contactPair.getProxyShape2()->getLocalToWorldTransform() * point2;
-
+            openglframework::Vector3 position2(point2.x, point2.y, point2.z);
-            manifoldElement = manifoldElement->getNext();
+            mContactPoints.push_back(SceneContactPoint(position2, contactNormal, openglframework::Color::blue()));
+        }
     }
 }

testbed/scenes/collisiondetection/CollisionDetectionScene.h

@@ -63,30 +63,22 @@ class ContactManager : public rp3d::CollisionCallback {
 
     private:
 
-        /// All the visual contact points
-        std::vector<ContactPoint> mContactPoints;
-
         /// Contact point mesh folder path
         std::string mMeshFolderPath;
 
+        /// Reference to the list of all the visual contact points
+        std::vector<SceneContactPoint>& mContactPoints;
+
    public:
 
-        ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath)
+        ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath,
-            : mMeshFolderPath(meshFolderPath) {
+                       std::vector<SceneContactPoint>& contactPoints)
+            : mMeshFolderPath(meshFolderPath), mContactPoints(contactPoints) {
 
         }
 
-        /// This method will be called for each reported contact point
+        /// This method is called when some contacts occur
-        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override;
+        virtual void onContact(const CallbackData& callbackData) override;
-
-        void resetPoints() {
-
-            mContactPoints.clear();
-        }
-
-        std::vector<ContactPoint> getContactPoints() const {
-            return mContactPoints;
-        }
 };
 
 // Class CollisionDetectionScene
@@ -151,9 +143,6 @@ class CollisionDetectionScene : public SceneDemo {
 
         /// Display/Hide the contact points
         virtual void setIsContactPointsDisplayed(bool display) override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Display or not the surface normals at hit points
@@ -171,11 +160,6 @@ inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
     SceneDemo::setIsContactPointsDisplayed(true);
 }
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() {
-    return mContactManager.getContactPoints();
-}
-
 }
 
 #endif

testbed/scenes/collisionshapes/CollisionShapesScene.cpp

@@ -49,7 +49,9 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
     for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
 
@@ -199,6 +201,8 @@ CollisionShapesScene::~CollisionShapesScene() {
 /// Reset the scene
 void CollisionShapesScene::reset() {
 
+    SceneDemo::reset();
+
     const float radius = 3.0f;
 
     for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {

testbed/scenes/collisionshapes/CollisionShapesScene.h

@@ -99,16 +99,8 @@ class CollisionShapesScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionShapesScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/concavemesh/ConcaveMeshScene.cpp

@@ -49,7 +49,9 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& sett
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
     // ---------- Create the boxes ----------- //
     for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
@@ -206,6 +208,8 @@ ConcaveMeshScene::~ConcaveMeshScene() {
 // Reset the scene
 void ConcaveMeshScene::reset() {
 
+    SceneDemo::reset();
+
     const float radius = 15.0f;
 
     for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {

testbed/scenes/concavemesh/ConcaveMeshScene.h

@@ -96,16 +96,8 @@ class ConcaveMeshScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> ConcaveMeshScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/cubes/CubesScene.cpp

@@ -47,7 +47,9 @@ CubesScene::CubesScene(const std::string& name, EngineSettings& settings)
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
     // Create all the cubes of the scene
     for (int i=0; i<NB_CUBES; i++) {
@@ -117,6 +119,8 @@ CubesScene::~CubesScene() {
 // Reset the scene
 void CubesScene::reset() {
 
+    SceneDemo::reset();
+
     float radius = 2.0f;
 
     // Create all the cubes of the scene

testbed/scenes/cubes/CubesScene.h

@@ -67,16 +67,8 @@ class CubesScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CubesScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/cubestack/CubeStackScene.cpp

@@ -47,7 +47,9 @@ CubeStackScene::CubeStackScene(const std::string& name, EngineSettings& settings
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
     // Create all the cubes of the scene
     for (int i=1; i<=NB_FLOORS; i++) {
@@ -120,6 +122,8 @@ CubeStackScene::~CubeStackScene() {
 // Reset the scene
 void CubeStackScene::reset() {
 
+    SceneDemo::reset();
+
     int index = 0;
     for (int i=NB_FLOORS; i > 0; i--) {
 

testbed/scenes/cubestack/CubeStackScene.h

@@ -67,16 +67,8 @@ class CubeStackScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CubeStackScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/heightfield/HeightFieldScene.cpp

@@ -49,7 +49,9 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
 	for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
 
@@ -205,6 +207,8 @@ HeightFieldScene::~HeightFieldScene() {
 // Reset the scene
 void HeightFieldScene::reset() {
 
+    SceneDemo::reset();
+
     const float radius = 3.0f;
 
     for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {

testbed/scenes/heightfield/HeightFieldScene.h

@@ -98,16 +98,8 @@ class HeightFieldScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override ;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override ;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> HeightFieldScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/joints/JointsScene.cpp

@@ -48,7 +48,9 @@ JointsScene::JointsScene(const std::string& name, EngineSettings& settings)
     worldSettings.worldName = name;
 
     // Create the dynamics world for the physics simulation
-    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+    dynamicsWorld->setEventListener(this);
+    mPhysicsWorld = dynamicsWorld;
 
     // Create the Ball-and-Socket joint
     createBallAndSocketJoints();
@@ -129,6 +131,8 @@ void JointsScene::updatePhysics() {
 // Reset the scene
 void JointsScene::reset() {
 
+    SceneDemo::reset();
+
     openglframework::Vector3 positionBox(0, 15, 5);
     openglframework::Vector3 boxDimension(1, 1, 1);
 

testbed/scenes/joints/JointsScene.h

@@ -125,16 +125,8 @@ class JointsScene : public SceneDemo {
 
         /// Reset the scene
         virtual void reset() override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> JointsScene::getContactPoints() {
-    return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
 }
 
 #endif

testbed/scenes/raycast/RaycastScene.cpp

@@ -33,7 +33,7 @@ using namespace raycastscene;
 // Constructor
 RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings)
        : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
-         mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1),
+         mRaycastManager(mPhongShader, mMeshFolderPath, mContactPoints), mCurrentBodyIndex(-1),
          mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
 
     mIsContactPointsDisplayed = true;
@@ -196,6 +196,8 @@ void RaycastScene::changeBody() {
 // Reset the scene
 void RaycastScene::reset() {
 
+    SceneDemo::reset();
+
     std::vector<PhysicsObject*>::iterator it;
     for (it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
         (*it)->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));

testbed/scenes/raycast/RaycastScene.h

@@ -64,17 +64,17 @@ class RaycastManager : public rp3d::RaycastCallback {
 
     private:
 
-        /// All the visual contact points
+        /// Reference to the list of contact points of the scene
-        std::vector<ContactPoint> mHitPoints;
+        std::vector<SceneContactPoint>& mHitPoints;
 
         /// Contact point mesh folder path
         std::string mMeshFolderPath;
 
    public:
 
-        RaycastManager(openglframework::Shader& shader,
+        RaycastManager(openglframework::Shader& shader, const std::string& meshFolderPath,
-                       const std::string& meshFolderPath)
+                       std::vector<SceneContactPoint>& hitPoints)
-            : mMeshFolderPath(meshFolderPath) {
+            : mMeshFolderPath(meshFolderPath), mHitPoints(hitPoints) {
 
         }
 
@@ -85,7 +85,7 @@ class RaycastManager : public rp3d::RaycastCallback {
 
             rp3d::Vector3 hitPos = raycastInfo.worldPoint;
             openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z);
-            mHitPoints.push_back(ContactPoint(position, normal, openglframework::Color::red()));
+            mHitPoints.push_back(SceneContactPoint(position, normal, openglframework::Color::red()));
 
             return raycastInfo.hitFraction;
         }
@@ -95,7 +95,7 @@ class RaycastManager : public rp3d::RaycastCallback {
             mHitPoints.clear();
         }
 
-        std::vector<ContactPoint> getHitPoints() const {
+        std::vector<SceneContactPoint> getHitPoints() const {
             return mHitPoints;
         }
 };
@@ -181,9 +181,6 @@ class RaycastScene : public SceneDemo {
 
         /// Display/Hide the contact points
         virtual void setIsContactPointsDisplayed(bool display) override;
-
-        /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Display or not the surface normals at hit points
@@ -201,11 +198,6 @@ inline void RaycastScene::setIsContactPointsDisplayed(bool display) {
     SceneDemo::setIsContactPointsDisplayed(true);
 }
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> RaycastScene::getContactPoints() {
-    return mRaycastManager.getHitPoints();
-}
-
 }
 
 #endif

testbed/src/Scene.cpp

@@ -185,3 +185,25 @@ void Scene::rotate(int xMouse, int yMouse) {
         }
     }
 }
+
+// Called when some contacts occur
+void Scene::onContact(const rp3d::CollisionCallback::CallbackData& callbackData) {
+
+    // For each contact pair
+    for (uint p=0; p < callbackData.getNbContactPairs(); p++) {
+
+        rp3d::CollisionCallback::ContactPair contactPair = callbackData.getContactPair(p);
+
+        // For each contact point of the contact pair
+        for (uint c=0; c < contactPair.getNbContactPoints(); c++) {
+
+            rp3d::CollisionCallback::ContactPoint contactPoint = contactPair.getContactPoint(c);
+
+            rp3d::Vector3 point = contactPair.getProxyShape1()->getLocalToWorldTransform() * contactPoint.getLocalPointOnShape1();
+            rp3d::Vector3 normalWorld = contactPoint.getWorldNormal();
+            openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
+            SceneContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
+            mContactPoints.push_back(contact);
+        }
+    }
+}

testbed/src/Scene.h

@@ -31,7 +31,7 @@
 #include "reactphysics3d.h"
 
 // Structure ContactPoint
-struct ContactPoint {
+struct SceneContactPoint {
 
     public:
         openglframework::Vector3 point;
@@ -39,7 +39,7 @@ struct ContactPoint {
 		openglframework::Color color;
 
         /// Constructor
-        ContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint)
+        SceneContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint)
 			        : point(pointWorld), normal(normalWorld), color(colorPoint) {
 
         }
@@ -88,7 +88,7 @@ struct EngineSettings {
 
 // Class Scene
 // Abstract class that represents a 3D scene.
-class Scene {
+class Scene : public rp3d::EventListener {
 
     protected:
 
@@ -130,12 +130,15 @@ class Scene {
         /// True if contact points are displayed
         bool mIsContactPointsDisplayed;
 
-        /// True if the AABBs of the phycis objects are displayed
+        /// True if the AABBs of the physics objects are displayed
         bool mIsAABBsDisplayed;
 
         /// True if we render shapes in wireframe mode
         bool mIsWireframeEnabled;
 
+        /// Contact points
+        std::vector<SceneContactPoint> mContactPoints;
+
         // -------------------- Methods -------------------- //
 
         /// Set the scene position (where the camera needs to look at)
@@ -165,7 +168,7 @@ class Scene {
         Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled = false);
 
         /// Destructor
-        virtual ~Scene();
+        virtual ~Scene() override;
 
         /// Reshape the view
         virtual void reshape(int width, int height);
@@ -181,7 +184,7 @@ class Scene {
         virtual void render()=0;
 
         /// Reset the scene
-        virtual void reset()=0;
+        virtual void reset();
 
         /// Called when a keyboard event occurs
         virtual bool keyboardEvent(int key, int scancode, int action, int mods);
@@ -230,11 +233,11 @@ class Scene {
         /// Enable/disbale wireframe rendering
         void setIsWireframeEnabled(bool isEnabled);
 
-        /// Return all the contact points of the scene
-        std::vector<ContactPoint> virtual getContactPoints();
-
         /// Update the engine settings
         virtual void updateEngineSettings() = 0;
+
+        /// Called when some contacts occur
+        virtual void onContact(const rp3d::CollisionCallback::CallbackData& callbackData) override;
 };
 
 // Called when a keyboard event occurs
@@ -247,6 +250,11 @@ inline void Scene::reshape(int width, int height) {
     mCamera.setDimensions(width, height);
 }
 
+// Reset the scene
+inline void Scene::reset() {
+    mContactPoints.clear();
+}
+
 // Return a reference to the camera
 inline const openglframework::Camera& Scene::getCamera() const  {
     return mCamera;
@@ -306,11 +314,4 @@ inline void Scene::setIsWireframeEnabled(bool isEnabled) {
     mIsWireframeEnabled = isEnabled;
 }
 
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> Scene::getContactPoints() {
-
-    // Return an empty list of contact points
-    return std::vector<ContactPoint>();
-}
-
 #endif

testbed/src/SceneDemo.cpp

@@ -98,7 +98,7 @@ SceneDemo::~SceneDemo() {
 	mColorShader.destroy();
 
     // Destroy the contact points
-    removeAllContactPoints();
+    removeAllVisualContactPoints();
 
     // Destroy rendering data for the AABB
     AABB::destroy();
@@ -124,6 +124,9 @@ void SceneDemo::update() {
 // Can be called several times per frame
 void SceneDemo::updatePhysics() {
 
+    // Clear contacts points
+    mContactPoints.clear();
+
     if (getDynamicsWorld() != nullptr) {
 
         // Take a simulation step
@@ -345,20 +348,17 @@ void SceneDemo::drawTextureQuad() {
 void SceneDemo::updateContactPoints() {
 
     // Remove the previous contact points
-    removeAllContactPoints();
+    removeAllVisualContactPoints();
 
     if (mIsContactPointsDisplayed) {
 
-        // Get the current contact points of the scene
-        std::vector<ContactPoint> contactPoints = getContactPoints();
-
         // For each contact point
-        std::vector<ContactPoint>::const_iterator it;
+        std::vector<SceneContactPoint>::const_iterator it;
-        for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+        for (it = mContactPoints.begin(); it != mContactPoints.end(); ++it) {
 
             // Create a visual contact point for rendering
             VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath, it->point + it->normal, it->color);
-            mContactPoints.push_back(point);
+            mVisualContactPoints.push_back(point);
         }
     }
 }
@@ -367,8 +367,8 @@ void SceneDemo::updateContactPoints() {
 void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
 
     // Render all the contact points
-    for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
+    for (std::vector<VisualContactPoint*>::iterator it = mVisualContactPoints.begin();
-         it != mContactPoints.end(); ++it) {
+         it != mVisualContactPoints.end(); ++it) {
         (*it)->render(mColorShader, worldToCameraMatrix);
     }
 }
@@ -397,46 +397,14 @@ void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix)
     }
 }
 
-void SceneDemo::removeAllContactPoints() {
+void SceneDemo::removeAllVisualContactPoints() {
 
     // Destroy all the visual contact points
-    for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
+    for (std::vector<VisualContactPoint*>::iterator it = mVisualContactPoints.begin();
-         it != mContactPoints.end(); ++it) {
+         it != mVisualContactPoints.end(); ++it) {
         delete (*it);
     }
-    mContactPoints.clear();
+    mVisualContactPoints.clear();
-}
-
-// Return all the contact points of the scene
-std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(rp3d::DynamicsWorld* world) {
-
-    std::vector<ContactPoint> contactPoints;
-
-    // Get the list of contact manifolds from the world
-    rp3d::List<const rp3d::ContactManifold*> manifolds = world->getContactsList();
-
-    // For each contact manifold
-    rp3d::List<const rp3d::ContactManifold*>::Iterator it;
-    for (it = manifolds.begin(); it != manifolds.end(); ++it) {
-
-        const rp3d::ContactManifold* manifold = *it;
-
-        // For each contact point of the manifold
-        rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
-        while (contactPoint != nullptr) {
-
-            rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnShape1();
-			rp3d::Vector3 normalWorld = contactPoint->getNormal();
-			openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
-            ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
-            contactPoints.push_back(contact);
-
-            contactPoint = contactPoint->getNext();
-        }
-
-    }
-
-    return contactPoints;
 }
 
 // Update the engine settings

testbed/src/SceneDemo.h

@@ -60,7 +60,7 @@ class SceneDemo : public Scene {
         static int shadowMapTextureLevel;
 
         /// All the visual contact points
-        std::vector<VisualContactPoint*> mContactPoints;
+        std::vector<VisualContactPoint*> mVisualContactPoints;
 
         /// Shadow map bias matrix
         openglframework::Matrix4 mShadowMapBiasMatrix;
@@ -123,7 +123,7 @@ class SceneDemo : public Scene {
         void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix);
 
         /// Remove all contact points
-        void removeAllContactPoints();
+        void removeAllVisualContactPoints();
 
         /// Return a reference to the dynamics world
         rp3d::DynamicsWorld* getDynamicsWorld();
@@ -144,6 +144,9 @@ class SceneDemo : public Scene {
         /// Update the scene
         virtual void update() override;
 
+        /// Reset the scene
+        virtual void reset() override;
+
 		/// Update the physics world (take a simulation step)
 		/// Can be called several times per frame
 		virtual void updatePhysics() override;
@@ -159,9 +162,6 @@ class SceneDemo : public Scene {
 
         /// Enabled/Disable the shadow mapping
         virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
-
-        /// Return all the contact points of the scene
-        std::vector<ContactPoint> computeContactPointsOfWorld(reactphysics3d::DynamicsWorld *world);
 };
 
 // Enabled/Disable the shadow mapping
@@ -184,6 +184,14 @@ inline const rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() const {
     return dynamic_cast<rp3d::DynamicsWorld*>(mPhysicsWorld);
 }
 
+// Reset the scene
+inline void SceneDemo::reset() {
+
+    Scene::reset();
+
+    removeAllVisualContactPoints();
+}
+
 #endif