Iterate over the islands to solve the contacts and joints

src/body/RigidBody.cpp

@@ -70,6 +70,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
                 memoryAllocator.release(elementToRemove, sizeof(JointListElement));
                 break;
             }
+            currentElement = currentElement->next;
         }
     }
 }

src/engine/ConstraintSolver.cpp

@@ -30,18 +30,14 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
+ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
-                                   std::vector<Vector3>& linearVelocities,
-                                   std::vector<Vector3>& angularVelocities,
-                                   std::vector<Vector3>& positions,
                                    std::vector<Quaternion>& orientations,
                                    const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                 : mJoints(joints), mLinearVelocities(linearVelocities),
+                 : mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
-                   mAngularVelocities(angularVelocities), mPositions(positions),
                    mOrientations(orientations),
                    mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
+                   mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
-                   angularVelocities, positions, orientations, mapBodyToVelocityIndex){
+                                                                      mapBodyToVelocityIndex){
 
 }
 
@@ -50,10 +46,16 @@ ConstraintSolver::~ConstraintSolver() {
 
 }
 
-// Initialize the constraint solver
+// Initialize the constraint solver for a given island
-void ConstraintSolver::initialize(decimal dt) {
+void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
 
-    PROFILE("ConstraintSolver::initialize()");
+    PROFILE("ConstraintSolver::initializeForIsland()");
+
+    assert(mLinearVelocities != NULL);
+    assert(mAngularVelocities != NULL);
+    assert(island != NULL);
+    assert(island->getNbBodies() > 0);
+    assert(island->getNbJoints() > 0);
 
     // Set the current time step
     mTimeStep = dt;
@@ -62,54 +64,50 @@ void ConstraintSolver::initialize(decimal dt) {
     mConstraintSolverData.timeStep = mTimeStep;
     mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
 
-    // For each joint
+    // For each joint of the island
-    std::set<Constraint*>::iterator it;
+    Constraint** joints = island->getJoints();
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+    for (uint i=0; i<island->getNbJoints(); i++) {
-
-        Constraint* joint = (*it);
-
-        // Get the rigid bodies of the joint
-        RigidBody* body1 = joint->getBody1();
-        RigidBody* body2 = joint->getBody2();
-
-        // Add the bodies to the set of constrained bodies
-        mConstraintBodies.insert(body1);
-        mConstraintBodies.insert(body2);
 
         // Initialize the constraint before solving it
-        joint->initBeforeSolve(mConstraintSolverData);
+        joints[i]->initBeforeSolve(mConstraintSolverData);
 
         // Warm-start the constraint if warm-starting is enabled
         if (mIsWarmStartingActive) {
-            joint->warmstart(mConstraintSolverData);
+            joints[i]->warmstart(mConstraintSolverData);
         }
     }
 }
 
 // Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints() {
+void ConstraintSolver::solveVelocityConstraints(Island* island) {
 
     PROFILE("ConstraintSolver::solveVelocityConstraints()");
 
-    // For each joint
+    assert(island != NULL);
-    std::set<Constraint*>::iterator it;
+    assert(island->getNbJoints() > 0);
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+
+    // For each joint of the island
+    Constraint** joints = island->getJoints();
+    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Solve the constraint
-        (*it)->solveVelocityConstraint(mConstraintSolverData);
+        joints[i]->solveVelocityConstraint(mConstraintSolverData);
     }
 }
 
 // Solve the position constraints
-void ConstraintSolver::solvePositionConstraints() {
+void ConstraintSolver::solvePositionConstraints(Island* island) {
 
     PROFILE("ConstraintSolver::solvePositionConstraints()");
 
-    // For each joint
+    assert(island != NULL);
-    std::set<Constraint*>::iterator it;
+    assert(island->getNbJoints() > 0);
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+
+    // For each joint of the island
+    Constraint** joints = island->getJoints();
+    for (uint i=0; i < island->getNbJoints(); i++) {
 
         // Solve the constraint
-        (*it)->solvePositionConstraint(mConstraintSolverData);
+        joints[i]->solvePositionConstraint(mConstraintSolverData);
     }
 }

src/engine/ConstraintSolver.h

@@ -30,6 +30,7 @@
 #include "../configuration.h"
 #include "mathematics/mathematics.h"
 #include "../constraint/Constraint.h"
+#include "Island.h"
 #include <map>
 #include <set>
 
@@ -47,11 +48,11 @@ struct ConstraintSolverData {
         /// Current time step of the simulation
         decimal timeStep;
 
-        /// Reference to the bodies linear velocities
+        /// Array with the bodies linear velocities
-        std::vector<Vector3>& linearVelocities;
+        Vector3* linearVelocities;
 
-        /// Reference to the bodies angular velocities
+        /// Array with the bodies angular velocities
-        std::vector<Vector3>& angularVelocities;
+        Vector3* angularVelocities;
 
         /// Reference to the bodies positions
         std::vector<Vector3>& positions;
@@ -67,13 +68,11 @@ struct ConstraintSolverData {
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
+        ConstraintSolverData(std::vector<Vector3>& refPositions,
-                             std::vector<Vector3>& refAngularVelocities,
-                             std::vector<Vector3>& refPositions,
                              std::vector<Quaternion>& refOrientations,
                              const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
-                           :linearVelocities(refLinearVelocities),
+                           :linearVelocities(NULL),
-                            angularVelocities(refAngularVelocities),
+                            angularVelocities(NULL),
                             positions(refPositions), orientations(refOrientations),
                             mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
 
@@ -156,19 +155,13 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to all the joints of the world
+        /// Array of constrained linear velocities (state of the linear velocities
-        std::set<Constraint*>& mJoints;
-
-        /// Constrained bodies
-        std::set<RigidBody*> mConstraintBodies;
-
-        /// Reference to the array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
-        std::vector<Vector3>& mLinearVelocities;
+        Vector3* mLinearVelocities;
 
-        /// Reference to the array of constrained angular velocities (state of the angular velocities
+        /// Array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
-        std::vector<Vector3>& mAngularVelocities;
+        Vector3* mAngularVelocities;
 
         /// Reference to the array of bodies positions (for position error correction)
         std::vector<Vector3>& mPositions;
@@ -194,24 +187,20 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(std::set<Constraint*>& joints,
+        ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
-                         std::vector<Vector3>& linearVelocities,
-                         std::vector<Vector3>& angularVelocities,
-                         std::vector<Vector3>& positions,
-                         std::vector<Quaternion>& orientations,
                          const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         ~ConstraintSolver();
 
-        /// Initialize the constraint solver
+        /// Initialize the constraint solver for a given island
-        void initialize(decimal dt);
+        void initializeForIsland(decimal dt, Island* island);
 
         /// Solve the constraints
-        void solveVelocityConstraints();
+        void solveVelocityConstraints(Island* island);
 
         /// Solve the position constraints
-        void solvePositionConstraints();
+        void solvePositionConstraints(Island* island);
 
         /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
         bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
@@ -219,13 +208,20 @@ class ConstraintSolver {
         /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
         void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
 
-        /// Return true if the body is in at least one constraint
+        /// Set the constrained velocities arrays
-        bool isConstrainedBody(RigidBody* body) const;
+        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
+                                            Vector3* constrainedAngularVelocities);
 };
 
-// Return true if the body is in at least one constraint
+// Set the constrained velocities arrays
-inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
+inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-    return mConstraintBodies.count(body) == 1;
+                                                            Vector3* constrainedAngularVelocities) {
+    assert(constrainedLinearVelocities != NULL);
+    assert(constrainedAngularVelocities != NULL);
+    mLinearVelocities = constrainedLinearVelocities;
+    mAngularVelocities = constrainedAngularVelocities;
+    mConstraintSolverData.linearVelocities = mLinearVelocities;
+    mConstraintSolverData.angularVelocities = mAngularVelocities;
 }
 
 }

src/engine/ContactSolver.cpp

@@ -36,18 +36,12 @@ using namespace std;
 // Constants initialization
 const decimal ContactSolver::BETA = decimal(0.2);
 const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP = decimal(0.01);
+const decimal ContactSolver::SLOP= decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                             std::vector<Vector3>& constrainedLinearVelocities,
+              :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
-                             std::vector<Vector3>& constrainedAngularVelocities,
+               mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
-                             const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-              :mContactManifolds(contactManifolds),
-               mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
-               mContactConstraints(NULL),
-               mLinearVelocities(constrainedLinearVelocities),
-               mAngularVelocities(constrainedAngularVelocities),
                mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
                mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
                mIsSolveFrictionAtContactManifoldCenterActive(true) {
@@ -59,26 +53,32 @@ ContactSolver::~ContactSolver() {
 
 }
 
-// Initialize the constraint solver
+// Initialize the constraint solver for a given island
-void ContactSolver::initialize(decimal dt) {
+void ContactSolver::initializeForIsland(decimal dt, Island* island) {
 
-    PROFILE("ContactSolver::initialize()");
+    PROFILE("ContactSolver::initializeForIsland()");
+
+    assert(island != NULL);
+    assert(island->getNbBodies() > 0);
+    assert(island->getNbContactManifolds() > 0);
+    assert(mSplitLinearVelocities != NULL);
+    assert(mSplitAngularVelocities != NULL);
 
     // Set the current time step
     mTimeStep = dt;
 
-    // TODO : Use better memory allocation here
+    mNbContactManifolds = island->getNbContactManifolds();
-    mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
 
-    mNbContactManifolds = 0;
+    mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
+    assert(mContactConstraints != NULL);
 
-    // For each contact manifold of the world
+    // For each contact manifold of the island
-    vector<ContactManifold*>::iterator it;
+    ContactManifold** contactManifolds = island->getContactManifold();
-    for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
+    for (uint i=0; i<mNbContactManifolds; i++) {
 
-        ContactManifold* externalManifold = *it;
+        ContactManifold* externalManifold = contactManifolds[i];
 
-        ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
+        ContactManifoldSolver& internalManifold = mContactConstraints[i];
 
         assert(externalManifold->getNbContactPoints() > 0);
 
@@ -86,10 +86,6 @@ void ContactSolver::initialize(decimal dt) {
         RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
         RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
 
-        // Add the two bodies of the constraint in the constraintBodies list
-        mConstraintBodies.insert(body1);
-        mConstraintBodies.insert(body2);
-
         // Get the position of the two bodies
         Vector3 x1 = body1->getTransform().getPosition();
         Vector3 x2 = body2->getTransform().getPosition();
@@ -173,46 +169,12 @@ void ContactSolver::initialize(decimal dt) {
                 internalManifold.frictionTwistImpulse = 0.0;
             }
         }
-
-        mNbContactManifolds++;
     }
 
-    // Allocated memory for split impulse velocities
-    // TODO : Use better memory allocation here
-    mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
-    mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
-    assert(mSplitLinearVelocities != NULL);
-    assert(mSplitAngularVelocities != NULL);
-
-    assert(mConstraintBodies.size() > 0);
-    assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
-    assert(mLinearVelocities.size() >= mConstraintBodies.size());
-    assert(mAngularVelocities.size() >= mConstraintBodies.size());
-
-    // Initialize the split impulse velocities
-    initializeSplitImpulseVelocities();
-
     // Fill-in all the matrices needed to solve the LCP problem
     initializeContactConstraints();
 }
 
-// Initialize the split impulse velocities
-void ContactSolver::initializeSplitImpulseVelocities() {
-
-    // For each current body that is implied in some constraint
-    set<RigidBody*>::iterator it;
-    for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
-        RigidBody* rigidBody = *it;
-        assert(rigidBody);
-
-        uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
-
-        // Initialize the split impulse velocities to zero
-        mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
-        mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
-    }
-}
-
 // Initialize the contact constraints before solving the system
 void ContactSolver::initializeContactConstraints() {
     
@@ -884,18 +846,8 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
 // Clean up the constraint solver
 void ContactSolver::cleanup() {
 
-    mConstraintBodies.clear();
-
     if (mContactConstraints != NULL) {
         delete[] mContactConstraints;
         mContactConstraints = NULL;
     }
-    if (mSplitLinearVelocities != NULL) {
-        delete[] mSplitLinearVelocities;
-        mSplitLinearVelocities = NULL;
-    }
-    if (mSplitAngularVelocities != NULL) {
-        delete[] mSplitAngularVelocities;
-        mSplitAngularVelocities = NULL;
-    }
 }

src/engine/ContactSolver.h

@@ -31,6 +31,7 @@
 #include "../configuration.h"
 #include "../constraint/Constraint.h"
 #include "ContactManifold.h"
+#include "Island.h"
 #include "Impulse.h"
 #include <map>
 #include <set>
@@ -311,9 +312,6 @@ class ContactSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to all the contact manifold of the world
-        std::vector<ContactManifold*>& mContactManifolds;
-
         /// Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
@@ -329,14 +327,11 @@ class ContactSolver {
         /// Number of contact constraints
         uint mNbContactManifolds;
 
-        /// Constrained bodies
+        /// Array of linear velocities
-        std::set<RigidBody*> mConstraintBodies;
+        Vector3* mLinearVelocities;
 
-        /// Reference to the array of linear velocities
+        /// Array of angular velocities
-        std::vector<Vector3>& mLinearVelocities;
+        Vector3* mAngularVelocities;
-
-        /// Reference to the array of angular velocities
-        std::vector<Vector3>& mAngularVelocities;
 
         /// Reference to the map of rigid body to their index in the constrained velocities array
         const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@@ -353,9 +348,6 @@ class ContactSolver {
 
         // -------------------- Methods -------------------- //
 
-        /// Initialize the split impulse velocities
-        void initializeSplitImpulseVelocities();
-
         /// Initialize the contact constraints before solving the system
         void initializeContactConstraints();
 
@@ -403,16 +395,21 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+        ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
-                      std::vector<Vector3>& constrainedLinearVelocities,
-                      std::vector<Vector3>& constrainedAngularVelocities,
-                      const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         virtual ~ContactSolver();
 
-        /// Initialize the constraint solver
+        /// Initialize the constraint solver for a given island
-        void initialize(decimal dt);
+        void initializeForIsland(decimal dt, Island* island);
+
+        /// Set the split velocities arrays
+        void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
+                                      Vector3* splitAngularVelocities);
+
+        /// Set the constrained velocities arrays
+        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
+                                            Vector3* constrainedAngularVelocities);
 
         /// Warm start the solver.
         void warmStart();
@@ -424,24 +421,6 @@ class ContactSolver {
         /// Solve the contacts
         void solve();
 
-        /// Return true if the body is in at least one constraint
-        bool isConstrainedBody(RigidBody* body) const;
-
-        /// Return the constrained linear velocity of a body after solving the constraints
-        Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
-
-        /// Return the split linear velocity
-        Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
-
-        /// Return the constrained angular velocity of a body after solving the constraints
-        Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
-
-        /// Return the split angular velocity
-        Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
-
-        /// Clean up the constraint solver
-        void cleanup();
-
         /// Return true if the split impulses position correction technique is used for contacts
         bool isSplitImpulseActive() const;
 
@@ -451,25 +430,27 @@ class ContactSolver {
         /// Activate or deactivate the solving of friction constraints at the center of
         /// the contact manifold instead of solving them at each contact point
         void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
+
+        /// Clean up the constraint solver
+        void cleanup();
 };
 
-// Return true if the body is in at least one constraint
+// Set the split velocities arrays
-inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
+inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
-    return mConstraintBodies.count(body) == 1;
+                                                    Vector3* splitAngularVelocities) {
+    assert(splitLinearVelocities != NULL);
+    assert(splitAngularVelocities != NULL);
+    mSplitLinearVelocities = splitLinearVelocities;
+    mSplitAngularVelocities = splitAngularVelocities;
 }
 
-// Return the split linear velocity
+// Set the constrained velocities arrays
-inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
+inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-    assert(isConstrainedBody(body));
+                                                          Vector3* constrainedAngularVelocities) {
-    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
+    assert(constrainedLinearVelocities != NULL);
-    return mSplitLinearVelocities[indexBody];
+    assert(constrainedAngularVelocities != NULL);
-}
+    mLinearVelocities = constrainedLinearVelocities;
-
+    mAngularVelocities = constrainedAngularVelocities;
-// Return the split angular velocity
-inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
-    assert(isConstrainedBody(body));
-    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
-    return mSplitAngularVelocities[indexBody];
 }
 
 // Return true if the split impulses position correction technique is used for contacts

src/engine/DynamicsWorld.cpp

@@ -37,15 +37,15 @@ using namespace std;
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
               : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
-                mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
-                               mMapBodyToConstrainedVelocityIndex),
+                mContactSolver(mMapBodyToConstrainedVelocityIndex),
-                mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
-                                  mConstrainedPositions, mConstrainedOrientations,
                                   mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
-                mIsSleepingEnabled(SPLEEPING_ENABLED), mNbIslands(0), mNbIslandsCapacity(0),
+                mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
-                mIslands(NULL) {
+                mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0),
+                mIslands(NULL), mNbBodiesCapacity(0) {
 
 }
 
@@ -73,8 +73,13 @@ DynamicsWorld::~DynamicsWorld() {
         mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
     }
 
-    // Free the allocated memory for the constrained velocities
+    // Release the memory allocated for the bodies velocity arrays
-    cleanupConstrainedVelocitiesArray();
+    if (mNbBodiesCapacity > 0) {
+        delete[] mSplitLinearVelocities;
+        delete[] mSplitAngularVelocities;
+        delete[] mConstrainedLinearVelocities;
+        delete[] mConstrainedAngularVelocities;
+    }
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -137,12 +142,6 @@ void DynamicsWorld::update() {
 
         // Update the AABBs of the bodies
         updateRigidBodiesAABB();
-
-        // Cleanup of the contact solver
-        mContactSolver.cleanup();
-
-        // Cleanup the constrained velocities
-        cleanupConstrainedVelocitiesArray();
     }
 
     // Compute and set the interpolation factor to all the bodies
@@ -178,11 +177,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             rigidBody->setAngularVelocity(newAngVelocity);
 
             // Add the split impulse velocity from Contact Solver (only used to update the position)
-            if (mContactSolver.isConstrainedBody(rigidBody) &&
+            if (mContactSolver.isSplitImpulseActive()) {
-                mContactSolver.isSplitImpulseActive()) {
 
-                newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
+                newLinVelocity += mSplitLinearVelocities[indexArray];
-                newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
+                newAngVelocity += mSplitAngularVelocities[indexArray];
             }
 
             // Get current position and orientation of the body
@@ -239,6 +237,34 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
     }
 }
 
+// Initialize the bodies velocities arrays for the next simulation step.
+void DynamicsWorld::initVelocityArrays() {
+
+    // Allocate memory for the bodies velocity arrays
+    uint nbBodies = mRigidBodies.size();
+    if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
+        if (mNbBodiesCapacity > 0) {
+            delete[] mSplitLinearVelocities;
+            delete[] mSplitAngularVelocities;
+        }
+        mNbBodiesCapacity = nbBodies;
+        mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
+        mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
+        mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
+        mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
+        assert(mSplitLinearVelocities != NULL);
+        assert(mSplitAngularVelocities != NULL);
+        assert(mConstrainedLinearVelocities != NULL);
+        assert(mConstrainedAngularVelocities != NULL);
+    }
+
+    // Reset the velocities arrays
+    for (uint i=0; i<mNbBodiesCapacity; i++) {
+        mSplitLinearVelocities[i].setToZero();
+        mSplitAngularVelocities[i].setToZero();
+    }
+}
+
 // Integrate the velocities of rigid bodies.
 /// This method only set the temporary velocities but does not update
 /// the actual velocitiy of the bodies. The velocities updated in this method
@@ -248,19 +274,22 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
 
-    // TODO : Use better memory allocation here
+    // Initialize the bodies velocity arrays
-    mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+    initVelocityArrays();
-    mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
 
     decimal dt = static_cast<decimal>(mTimer.getTimeStep());
 
     // Fill in the mapping of rigid body to their index in the constrained
     // velocities arrays
     uint i = 0;
+    mMapBodyToConstrainedVelocityIndex.clear();
     for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
         RigidBody* rigidBody = *it;
         mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
 
+        assert(mSplitLinearVelocities[i] == Vector3(0, 0, 0));
+        assert(mSplitAngularVelocities[i] == Vector3(0, 0, 0));
+
         // If the body is allowed to move
         if (rigidBody->getIsMotionEnabled()) {
 
@@ -318,42 +347,59 @@ void DynamicsWorld::solveContactsAndConstraints() {
     // Get the current time step
     decimal dt = static_cast<decimal>(mTimer.getTimeStep());
 
-    // Check if there are contacts and constraints to solve
+    // Set the velocities arrays
-    bool isConstraintsToSolve = !mJoints.empty();
+    mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
-    bool isContactsToSolve = !mContactManifolds.empty();
+    mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
-    if (!isConstraintsToSolve && !isContactsToSolve) return;
+                                                  mConstrainedAngularVelocities);
+    mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
+                                                     mConstrainedAngularVelocities);
 
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
-    // If there are contacts
+    // For each island of the world
-    if (isContactsToSolve) {
+    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-        // Initialize the solver
+        // Check if there are contacts and constraints to solve
-        mContactSolver.initialize(dt);
+        bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
+        bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+        if (!isConstraintsToSolve && !isContactsToSolve) continue;
 
-        // Warm start the contact solver
+        // If there are contacts in the current island
-        mContactSolver.warmStart();
+        if (isContactsToSolve) {
+
+            // Initialize the solver
+            mContactSolver.initializeForIsland(dt, mIslands[islandIndex]);
+
+            // Warm start the contact solver
+            mContactSolver.warmStart();
+        }
+
+        // If there are constraints
+        if (isConstraintsToSolve) {
+
+            // Initialize the constraint solver
+            mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]);
+        }
+
+        // For each iteration of the velocity solver
+        for (uint i=0; i<mNbVelocitySolverIterations; i++) {
+
+            // Solve the constraints
+            if (isConstraintsToSolve) {
+                mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+            }
+
+            // Solve the contacts
+            if (isContactsToSolve) mContactSolver.solve();
+        }        
+
+        // Cache the lambda values in order to use them in the next
+        // step and cleanup the contact solver
+        if (isContactsToSolve) {
+            mContactSolver.storeImpulses();
+            mContactSolver.cleanup();
+        }
     }
-
-    // If there are constraints
-    if (isConstraintsToSolve) {
-
-        // Initialize the constraint solver
-        mConstraintSolver.initialize(dt);
-    }
-
-    // For each iteration of the velocity solver
-    for (uint i=0; i<mNbVelocitySolverIterations; i++) {
-
-        // Solve the constraints
-        if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
-
-        // Solve the contacts
-        if (isContactsToSolve) mContactSolver.solve();
-    }
-
-    // Cache the lambda values in order to use them in the next step
-    if (isContactsToSolve) mContactSolver.storeImpulses();
 }
 
 // Solve the position error correction of the constraints
@@ -369,37 +415,36 @@ void DynamicsWorld::solvePositionCorrection() {
     // TODO : Use better memory allocation here
     mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
     mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
-        // If it is a constrained bodies (by a joint)
+    // For each island of the world
-        if (mConstraintSolver.isConstrainedBody(*it)) {
+    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-            uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
+        // For each body of the island
+        RigidBody** bodies = mIslands[islandIndex]->getBodies();
+        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
+
+            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
 
             // Get the position/orientation of the rigid body
-            const Transform& transform = (*it)->getTransform();
+            const Transform& transform = bodies[b]->getTransform();
             mConstrainedPositions[index] = transform.getPosition();
             mConstrainedOrientations[index]= transform.getOrientation();
         }
-    }
 
-    // ---------- Solve the position error correction for the constraints ---------- //
+        // ---------- Solve the position error correction for the constraints ---------- //
 
-    // For each iteration of the position (error correction) solver
+        // For each iteration of the position (error correction) solver
-    for (uint i=0; i<mNbPositionSolverIterations; i++) {
+        for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
-        // Solve the position constraints
+            // Solve the position constraints
-        mConstraintSolver.solvePositionConstraints();
+            mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
-    }
+        }
 
-    // ---------- Update the position/orientation of the rigid bodies ---------- //
+        // ---------- Update the position/orientation of the rigid bodies ---------- //
 
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
 
-        // If it is a constrained bodies (by a joint)
+            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-        if (mConstraintSolver.isConstrainedBody(*it)) {
-
-            uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
 
             // Get the new position/orientation of the body
             const Vector3& newPosition = mConstrainedPositions[index];
@@ -407,22 +452,11 @@ void DynamicsWorld::solvePositionCorrection() {
 
             // Update the Transform of the body
             Transform newTransform(newPosition, newOrientation.getUnit());
-            (*it)->setTransform(newTransform);
+            bodies[b]->setTransform(newTransform);
         }
     }
 }
 
-// Cleanup the constrained velocities array at each step
-void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
-
-    // Clear the constrained velocites
-    mConstrainedLinearVelocities.clear();
-    mConstrainedAngularVelocities.clear();
-
-    // Clear the rigid body to velocities array index mapping
-    mMapBodyToConstrainedVelocityIndex.clear();
-}
-
 // Create a rigid body into the physics world
 RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
                                           const Matrix3x3& inertiaTensorLocal,
@@ -579,11 +613,13 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
     joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
     joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
 
+    size_t nbBytes = joint->getSizeInBytes();
+
     // Call the destructor of the joint
     joint->Constraint::~Constraint();
 
     // Release the allocated memory
-    mMemoryAllocator.release(joint, joint->getSizeInBytes());
+    mMemoryAllocator.release(joint, nbBytes);
 }
 
 // Add the joint to the list of joints of the two bodies involved in the joint

src/engine/DynamicsWorld.h

@@ -87,11 +87,17 @@ class DynamicsWorld : public CollisionWorld {
 
         /// Array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
-        std::vector<Vector3> mConstrainedLinearVelocities;
+        Vector3* mConstrainedLinearVelocities;
 
         /// Array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
-        std::vector<Vector3> mConstrainedAngularVelocities;
+        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)
         std::vector<Vector3> mConstrainedPositions;
@@ -111,6 +117,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Array with all the islands of awaken bodies
         Island** mIslands;
 
+        /// Current allocated capacity for the bodies
+        uint mNbBodiesCapacity;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -132,6 +141,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
+        /// Initialize the bodies velocities arrays for the next simulation step.
+        void initVelocityArrays();
+
         /// Integrate the velocities of rigid bodies.
         void integrateRigidBodiesVelocities();