Modify the contact solver so that its main loop is outside the solver

examples/fallingcubes/FallingCubes.cpp

@@ -66,12 +66,11 @@ int main(int argc, char** argv) {
     glutMouseFunc(mouseButton);
     glutMotionFunc(mouseMotion);
     glutKeyboardFunc(keyboard);
+    glutCloseFunc(finish);
 
     // Glut main looop
     glutMainLoop();
 
-    finish();
-
     return 0;
 }
 
@@ -115,8 +114,7 @@ void keyboard(unsigned char key, int x, int y) {
 
         // Escape key
         case 27:
-            finish();
+            glutLeaveMainLoop();
-            exit(0);
             break;
 
         // Space bar

src/engine/CollisionWorld.cpp

@@ -154,6 +154,7 @@ CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& colli
     // A similar collision shape does not already exist in the world, so we create a
     // new one and add it to the world
     void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
+    size_t test = collisionShape.getSizeInBytes();
     CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
     mCollisionShapes.push_back(newCollisionShape);
 
@@ -181,11 +182,14 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
         // Remove the shape from the set of shapes in the world
         mCollisionShapes.remove(collisionShape);
 
+        // Compute the size (in bytes) of the collision shape
+        size_t nbBytesShape = collisionShape->getSizeInBytes();
+
         // Call the destructor of the collision shape
         collisionShape->CollisionShape::~CollisionShape();
 
         // Deallocate the memory used by the collision shape
-        mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
+        mMemoryAllocator.release(collisionShape, nbBytesShape);
     }
 }
 

src/engine/ConstraintSolver.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "ConstraintSolver.h"
+#include "Profiler.h"
 
 using namespace reactphysics3d;
 
@@ -43,3 +44,19 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
 ConstraintSolver::~ConstraintSolver() {
 
 }
+
+// Initialize the constraint solver
+void ConstraintSolver::initialize(decimal dt) {
+
+    PROFILE("ConstraintSolver::initialize()");
+
+    // Set the current time step
+    mTimeStep = dt;
+}
+
+// Solve the constraints
+void ConstraintSolver::solve() {
+
+    PROFILE("ConstraintSolver::solve()");
+
+}

src/engine/ConstraintSolver.h

@@ -142,6 +142,12 @@ class ConstraintSolver {
 
         /// Destructor
         ~ConstraintSolver();
+
+        /// Initialize the constraint solver
+        void initialize(decimal dt);
+
+        /// Solve the constraints
+        void solve();
 };
 
 }

src/engine/ContactSolver.cpp

@@ -44,7 +44,6 @@ ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
                              std::vector<Vector3>& constrainedAngularVelocities,
                              const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
               :mContactManifolds(contactManifolds),
-               mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
                mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
                mContactConstraints(NULL),
                mConstrainedLinearVelocities(constrainedLinearVelocities),
@@ -61,7 +60,12 @@ ContactSolver::~ContactSolver() {
 }
 
 // Initialize the constraint solver
-void ContactSolver::initialize() {
+void ContactSolver::initialize(decimal dt) {
+
+    PROFILE("ContactSolver::initialize()");
+
+    // Set the current time step
+    mTimeStep = dt;
 
     // TODO : Use better memory allocation here
     mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
@@ -187,6 +191,9 @@ void ContactSolver::initialize() {
 
     // Initialize the split impulse velocities
     initializeSplitImpulseVelocities();
+
+    // Fill-in all the matrices needed to solve the LCP problem
+    initializeContactConstraints();
 }
 
 // Initialize the split impulse velocities
@@ -380,6 +387,9 @@ void ContactSolver::initializeContactConstraints() {
 /// the solution of the linear system
 void ContactSolver::warmStart() {
 
+    // Check that warm starting is active
+    if (!mIsWarmStartingActive) return;
+
     // For each constraint
     for (uint c=0; c<mNbContactManifolds; c++) {
 
@@ -519,260 +529,231 @@ void ContactSolver::warmStart() {
     }
 }
 
-// Solve the contact constraints by applying sequential impulses
+// Solve the contacts
-void ContactSolver::solveContactConstraints() {
+void ContactSolver::solve() {
+
+    PROFILE("ContactSolver::solve()");
 
     decimal deltaLambda;
     decimal lambdaTemp;
-    uint iter;
 
-    // For each iteration of the contact solver
+    // For each contact manifold
-    for(iter=0; iter<mNbIterations; iter++) {
+    for (uint c=0; c<mNbContactManifolds; c++) {
 
-        // For each contact manifold
+        ContactManifoldSolver& contactManifold = mContactConstraints[c];
-        for (uint c=0; c<mNbContactManifolds; c++) {
 
-            ContactManifoldSolver& contactManifold = mContactConstraints[c];
+        decimal sumPenetrationImpulse = 0.0;
 
-            decimal sumPenetrationImpulse = 0.0;
+        // Get the constrained velocities
+        const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
+        const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
+        const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
+        const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
 
-            // Get the constrained velocities
+        for (uint i=0; i<contactManifold.nbContacts; i++) {
-            const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
-            const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
-            const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
-            const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
 
-            for (uint i=0; i<contactManifold.nbContacts; i++) {
+            ContactPointSolver& contactPoint = contactManifold.contacts[i];
 
-                ContactPointSolver& contactPoint = contactManifold.contacts[i];
+            // --------- Penetration --------- //
 
-                // --------- Penetration --------- //
+            // Compute J*v
+            Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+            decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+            decimal Jv = deltaVDotN;
 
-                // Compute J*v
+            // Compute the bias "b" of the constraint
-                Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+            decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
-                decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+            decimal biasPenetrationDepth = 0.0;
-                decimal Jv = deltaVDotN;
+            if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+                    max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+            decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
 
-                // Compute the bias "b" of the constraint
+            // Compute the Lagrange multiplier lambda
-                decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+            if (mIsSplitImpulseActive) {
-                decimal biasPenetrationDepth = 0.0;
+                deltaLambda = - (Jv + contactPoint.restitutionBias) *
-                if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+                        contactPoint.inversePenetrationMass;
-                        max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+            }
-                decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
+            else {
+                deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+            }
+            lambdaTemp = contactPoint.penetrationImpulse;
+            contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
+                                                       deltaLambda, decimal(0.0));
+            deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
 
-                // Compute the Lagrange multiplier lambda
+            // Compute the impulse P=J^T * lambda
-                if (mIsSplitImpulseActive) {
+            const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
-                    deltaLambda = - (Jv + contactPoint.restitutionBias) *
+                                                                         contactPoint);
-                                     contactPoint.inversePenetrationMass;
+
-                }
+            // Apply the impulse to the bodies of the constraint
-                else {
+            applyImpulse(impulsePenetration, contactManifold);
-                    deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+
-                }
+            sumPenetrationImpulse += contactPoint.penetrationImpulse;
-                lambdaTemp = contactPoint.penetrationImpulse;
+
-                contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
+            // If the split impulse position correction is active
-                                                           deltaLambda, decimal(0.0));
+            if (mIsSplitImpulseActive) {
-                deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
+
+                // Split impulse (position correction)
+                const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
+                const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
+                const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
+                const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
+                Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
+                        v1Split - w1Split.cross(contactPoint.r1);
+                decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
+                decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
+                        contactPoint.inversePenetrationMass;
+                decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
+                contactPoint.penetrationSplitImpulse = std::max(
+                            contactPoint.penetrationSplitImpulse +
+                            deltaLambdaSplit, decimal(0.0));
+                deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
 
                 // Compute the impulse P=J^T * lambda
-                const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
+                const Impulse splitImpulsePenetration = computePenetrationImpulse(
-                                                                             contactPoint);
+                            deltaLambdaSplit, contactPoint);
 
-                // Apply the impulse to the bodies of the constraint
+                applySplitImpulse(splitImpulsePenetration, contactManifold);
-                applyImpulse(impulsePenetration, contactManifold);
-
-                sumPenetrationImpulse += contactPoint.penetrationImpulse;
-
-                // If the split impulse position correction is active
-                if (mIsSplitImpulseActive) {
-
-                    // Split impulse (position correction)
-                    const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
-                    const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
-                    const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
-                    const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
-                    Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
-                                          v1Split - w1Split.cross(contactPoint.r1);
-                    decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
-                    decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
-                                                  contactPoint.inversePenetrationMass;
-                    decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
-                    contactPoint.penetrationSplitImpulse = std::max(
-                                                             contactPoint.penetrationSplitImpulse +
-                                                             deltaLambdaSplit, decimal(0.0));
-                    deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
-
-                    // Compute the impulse P=J^T * lambda
-                    const Impulse splitImpulsePenetration = computePenetrationImpulse(
-                                                               deltaLambdaSplit, contactPoint);
-
-                    applySplitImpulse(splitImpulsePenetration, contactManifold);
-                }
-
-                // If we do not solve the friction constraints at the center of the contact manifold
-                if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
-                    // --------- Friction 1 --------- //
-
-                    // Compute J*v
-                    deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-                    Jv = deltaV.dot(contactPoint.frictionVector1);
-
-                    // Compute the Lagrange multiplier lambda
-                    deltaLambda = -Jv;
-                    deltaLambda *= contactPoint.inverseFriction1Mass;
-                    decimal frictionLimit = contactManifold.frictionCoefficient *
-                                            contactPoint.penetrationImpulse;
-                    lambdaTemp = contactPoint.friction1Impulse;
-                    contactPoint.friction1Impulse = std::max(-frictionLimit,
-                                                             std::min(contactPoint.friction1Impulse
-                                                             + deltaLambda, frictionLimit));
-                    deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
-
-                    // Compute the impulse P=J^T * lambda
-                    const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
-                                                                             contactPoint);
-
-                    // Apply the impulses to the bodies of the constraint
-                    applyImpulse(impulseFriction1, contactManifold);
-
-                    // --------- Friction 2 --------- //
-
-                    // Compute J*v
-                    deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-                    Jv = deltaV.dot(contactPoint.frictionVector2);
-
-                    // Compute the Lagrange multiplier lambda
-                    deltaLambda = -Jv;
-                    deltaLambda *= contactPoint.inverseFriction2Mass;
-                    frictionLimit = contactManifold.frictionCoefficient *
-                                    contactPoint.penetrationImpulse;
-                    lambdaTemp = contactPoint.friction2Impulse;
-                    contactPoint.friction2Impulse = std::max(-frictionLimit,
-                                                             std::min(contactPoint.friction2Impulse
-                                                             + deltaLambda, frictionLimit));
-                    deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
-
-                    // Compute the impulse P=J^T * lambda
-                    const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
-                                                                             contactPoint);
-
-                    // Apply the impulses to the bodies of the constraint
-                    applyImpulse(impulseFriction2, contactManifold);
-                }
             }
 
-            // If we solve the friction constraints at the center of the contact manifold
+            // If we do not solve the friction constraints at the center of the contact manifold
-            if (mIsSolveFrictionAtContactManifoldCenterActive) {
+            if (!mIsSolveFrictionAtContactManifoldCenterActive) {
 
-                // ------ First friction constraint at the center of the contact manifol ------ //
+                // --------- Friction 1 --------- //
 
                 // Compute J*v
-                Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
+                deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-                                 - v1 - w1.cross(contactManifold.r1Friction);
+                Jv = deltaV.dot(contactPoint.frictionVector1);
-                decimal Jv = deltaV.dot(contactManifold.frictionVector1);
 
                 // Compute the Lagrange multiplier lambda
-                decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+                deltaLambda = -Jv;
-                decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+                deltaLambda *= contactPoint.inverseFriction1Mass;
-                lambdaTemp = contactManifold.friction1Impulse;
+                decimal frictionLimit = contactManifold.frictionCoefficient *
-                contactManifold.friction1Impulse = std::max(-frictionLimit,
+                        contactPoint.penetrationImpulse;
-                                                        std::min(contactManifold.friction1Impulse +
+                lambdaTemp = contactPoint.friction1Impulse;
-                                                                 deltaLambda, frictionLimit));
+                contactPoint.friction1Impulse = std::max(-frictionLimit,
-                deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+                                                         std::min(contactPoint.friction1Impulse
+                                                                  + deltaLambda, frictionLimit));
+                deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
 
                 // Compute the impulse P=J^T * lambda
-                Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+                const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
-                Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+                                                                         contactPoint);
-                Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
-                Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
-                const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
-                                               linearImpulseBody2, angularImpulseBody2);
 
                 // Apply the impulses to the bodies of the constraint
                 applyImpulse(impulseFriction1, contactManifold);
 
-                // ------ Second friction constraint at the center of the contact manifol ----- //
+                // --------- Friction 2 --------- //
 
                 // Compute J*v
-                deltaV = v2 + w2.cross(contactManifold.r2Friction)
+                deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-                        - v1 - w1.cross(contactManifold.r1Friction);
+                Jv = deltaV.dot(contactPoint.frictionVector2);
-                Jv = deltaV.dot(contactManifold.frictionVector2);
 
                 // Compute the Lagrange multiplier lambda
-                deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+                deltaLambda = -Jv;
-                frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+                deltaLambda *= contactPoint.inverseFriction2Mass;
-                lambdaTemp = contactManifold.friction2Impulse;
+                frictionLimit = contactManifold.frictionCoefficient *
-                contactManifold.friction2Impulse = std::max(-frictionLimit,
+                        contactPoint.penetrationImpulse;
-                                                        std::min(contactManifold.friction2Impulse +
+                lambdaTemp = contactPoint.friction2Impulse;
-                                                                 deltaLambda, frictionLimit));
+                contactPoint.friction2Impulse = std::max(-frictionLimit,
-                deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+                                                         std::min(contactPoint.friction2Impulse
+                                                                  + deltaLambda, frictionLimit));
+                deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
 
                 // Compute the impulse P=J^T * lambda
-                linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
+                const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
-                angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+                                                                         contactPoint);
-                linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
-                angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
-                const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
-                                               linearImpulseBody2, angularImpulseBody2);
 
                 // Apply the impulses to the bodies of the constraint
                 applyImpulse(impulseFriction2, contactManifold);
-
-                // ------ Twist friction constraint at the center of the contact manifol ------ //
-
-                // Compute J*v
-                deltaV = w2 - w1;
-                Jv = deltaV.dot(contactManifold.normal);
-
-                deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
-                frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
-                lambdaTemp = contactManifold.frictionTwistImpulse;
-                contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
-                                                       std::min(contactManifold.frictionTwistImpulse
-                                                       + deltaLambda, frictionLimit));
-                deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
-
-                // Compute the impulse P=J^T * lambda
-                linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
-                angularImpulseBody1 = -contactManifold.normal * deltaLambda;
-                linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
-                angularImpulseBody2 = contactManifold.normal * deltaLambda;
-                const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
-                                                   linearImpulseBody2, angularImpulseBody2);
-
-                // Apply the impulses to the bodies of the constraint
-                applyImpulse(impulseTwistFriction, contactManifold);
             }
         }
+
+        // If we solve the friction constraints at the center of the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+            // ------ First friction constraint at the center of the contact manifol ------ //
+
+            // Compute J*v
+            Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
+                    - v1 - w1.cross(contactManifold.r1Friction);
+            decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+
+            // Compute the Lagrange multiplier lambda
+            decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+            decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+            lambdaTemp = contactManifold.friction1Impulse;
+            contactManifold.friction1Impulse = std::max(-frictionLimit,
+                                                        std::min(contactManifold.friction1Impulse +
+                                                                 deltaLambda, frictionLimit));
+            deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda
+            Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+            Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+            Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
+            Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+            const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+                                           linearImpulseBody2, angularImpulseBody2);
+
+            // Apply the impulses to the bodies of the constraint
+            applyImpulse(impulseFriction1, contactManifold);
+
+            // ------ Second friction constraint at the center of the contact manifol ----- //
+
+            // Compute J*v
+            deltaV = v2 + w2.cross(contactManifold.r2Friction)
+                    - v1 - w1.cross(contactManifold.r1Friction);
+            Jv = deltaV.dot(contactManifold.frictionVector2);
+
+            // Compute the Lagrange multiplier lambda
+            deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+            frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+            lambdaTemp = contactManifold.friction2Impulse;
+            contactManifold.friction2Impulse = std::max(-frictionLimit,
+                                                        std::min(contactManifold.friction2Impulse +
+                                                                 deltaLambda, frictionLimit));
+            deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda
+            linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
+            angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+            linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
+            angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+            const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+                                           linearImpulseBody2, angularImpulseBody2);
+
+            // Apply the impulses to the bodies of the constraint
+            applyImpulse(impulseFriction2, contactManifold);
+
+            // ------ Twist friction constraint at the center of the contact manifol ------ //
+
+            // Compute J*v
+            deltaV = w2 - w1;
+            Jv = deltaV.dot(contactManifold.normal);
+
+            deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+            frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+            lambdaTemp = contactManifold.frictionTwistImpulse;
+            contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
+                                                            std::min(contactManifold.frictionTwistImpulse
+                                                                     + deltaLambda, frictionLimit));
+            deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+
+            // Compute the impulse P=J^T * lambda
+            linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+            angularImpulseBody1 = -contactManifold.normal * deltaLambda;
+            linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
+            angularImpulseBody2 = contactManifold.normal * deltaLambda;
+            const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+                                               linearImpulseBody2, angularImpulseBody2);
+
+            // Apply the impulses to the bodies of the constraint
+            applyImpulse(impulseTwistFriction, contactManifold);
+        }
     }
 }
 
-// Solve the constraints
-void ContactSolver::solve(decimal timeStep) {
-
-    PROFILE("ContactSolver::solve()");
-
-    // Set the current time step
-    mTimeStep = timeStep;
-
-    // Initialize the solver
-    initialize();
-
-    // Fill-in all the matrices needed to solve the LCP problem
-    initializeContactConstraints();
-
-    // Warm start the solver
-    if (mIsWarmStartingActive) {
-        warmStart();
-    }
-
-    // Solve the contact constraints
-    solveContactConstraints();
-
-    // Cache the lambda values in order to use them in the next step
-    storeImpulses();
-}
-
 // Store the computed impulses to use them to
 // warm start the solver at the next iteration
 void ContactSolver::storeImpulses() {

src/engine/ContactSolver.h

@@ -345,9 +345,6 @@ class ContactSolver {
         /// Reference to all the contact manifold of the world
         std::vector<ContactManifold*>& mContactManifolds;
 
-        /// Number of iterations of the contact solver
-        uint mNbIterations;
-
         /// Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
@@ -389,25 +386,12 @@ class ContactSolver {
 
         // -------------------- Methods -------------------- //
 
-        /// Initialize the constraint solver
-        void initialize();
-
         /// Initialize the split impulse velocities
         void initializeSplitImpulseVelocities();
 
         /// Initialize the contact constraints before solving the system
         void initializeContactConstraints();
 
-        /// Store the computed impulses to use them to
-        /// warm start the solver at the next iteration
-        void storeImpulses();
-
-        /// Warm start the solver.
-        void warmStart();
-
-        /// Solve the contact constraints by applying sequential impulses
-        void solveContactConstraints();
-
         /// Apply an impulse to the two bodies of a constraint
         void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
 
@@ -460,8 +444,18 @@ class ContactSolver {
         /// Destructor
         virtual ~ContactSolver();
 
-        /// Solve the constraints
+        /// Initialize the constraint solver
-        void solve(decimal timeStep);
+        void initialize(decimal dt);
+
+        /// Warm start the solver.
+        void warmStart();
+
+        /// Store the computed impulses to use them to
+        /// warm start the solver at the next iteration
+        void storeImpulses();
+
+        /// Solve the contacts
+        void solve();
 
         /// Return true if the body is in at least one constraint
         bool isConstrainedBody(RigidBody* body) const;
@@ -481,9 +475,6 @@ class ContactSolver {
         /// Clean up the constraint solver
         void cleanup();
 
-        /// Set the number of iterations of the constraint solver
-        void setNbIterationsSolver(uint nbIterations);
-
         /// Activate or Deactivate the split impulses for contacts
         void setIsSplitImpulseActive(bool isActive);
 
@@ -511,11 +502,6 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
     return mSplitAngularVelocities[indexBody];
 }
 
-// Set the number of iterations of the constraint solver
-inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
-    mNbIterations = nbIterations;
-}
-
 // Activate or Deactivate the split impulses for contacts
 inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
     mIsSplitImpulseActive = isActive;

src/engine/DynamicsWorld.cpp

@@ -38,6 +38,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                                mMapBodyToConstrainedVelocityIndex),
                 mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
                                   mMapBodyToConstrainedVelocityIndex),
+                mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -94,15 +95,11 @@ void DynamicsWorld::update() {
         // Compute the collision detection
         mCollisionDetection.computeCollisionDetection();
 
-        // Initialize the constrained velocities
+        // Integrate the velocities
-        initConstrainedVelocitiesArray();
+        integrateRigidBodiesVelocities();
 
-        // If there are contacts
+        // Solve the contacts and constraints
-        if (!mContactManifolds.empty()) {
+        solveContactsAndConstraints();
-
-            // Solve the contacts
-            mContactSolver.solve(static_cast<decimal>(mTimer.getTimeStep()));
-        }
 
         // Update the timer
         mTimer.nextStep();
@@ -110,8 +107,8 @@ void DynamicsWorld::update() {
         // Reset the movement boolean variable of each body to false
         resetBodiesMovementVariable();
 
-        // Update the position and orientation of each body
+        // Integrate the position and orientation of each body
-        updateRigidBodiesPositionAndOrientation();
+        integrateRigidBodiesPositions();
 
         // Cleanup of the contact solver
         mContactSolver.cleanup();
@@ -124,8 +121,8 @@ void DynamicsWorld::update() {
     setInterpolationFactorToAllBodies();
 }
 
-// Update the position and orientation of the rigid bodies
+// Integrate position and orientation of the rigid bodies
-void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
+void DynamicsWorld::integrateRigidBodiesPositions() {
 
     PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()");
 
@@ -200,8 +197,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
     }
 }
 
-// Initialize the constrained velocities array at each step
+// Integrate the velocities of rigid bodies
-void DynamicsWorld::initConstrainedVelocitiesArray() {
+void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     // TODO : Use better memory allocation here
     mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
@@ -228,6 +225,50 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
     assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
 }
 
+// Solve the contacts and constraints
+void DynamicsWorld::solveContactsAndConstraints() {
+
+    PROFILE("DynamicsWorld::solveContactsAndConstraints()");
+
+    // Get the current time step
+    decimal dt = static_cast<decimal>(mTimer.getTimeStep());
+
+    // Check if there are contacts and constraints to solve
+    bool isConstraintsToSolve = !mJoints.empty();
+    bool isContactsToSolve = !mContactManifolds.empty();
+    if (!isConstraintsToSolve && !isContactsToSolve) return;
+
+    // If there are contacts
+    if (isContactsToSolve) {
+
+        // Initialize the solver
+        mContactSolver.initialize(dt);
+
+        // Warm start the contact solver
+        mContactSolver.warmStart();
+    }
+
+    // If there are constraints
+    if (isConstraintsToSolve) {
+
+        // Initialize the constraint solver
+        mConstraintSolver.initialize(dt);
+    }
+
+    // For each iteration of the solver
+    for (uint i=0; i<mNbSolverIterations; i++) {
+
+        // Solve the constraints
+        if (isConstraintsToSolve) mConstraintSolver.solve();
+
+        // Solve the contacts
+        if (isContactsToSolve) mContactSolver.solve();
+    }
+
+    // Cache the lambda values in order to use them in the next step
+    if (isContactsToSolve) mContactSolver.storeImpulses();
+}
+
 // Cleanup the constrained velocities array at each step
 void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
 
@@ -235,9 +276,6 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
     mConstrainedLinearVelocities.clear();
     mConstrainedAngularVelocities.clear();
 
-    // Clear the constrained bodies
-    mConstrainedBodies.clear();
-
     // Clear the rigid body to velocities array index mapping
     mMapBodyToConstrainedVelocityIndex.clear();
 }

src/engine/DynamicsWorld.h

@@ -59,6 +59,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Constraint solver
         ConstraintSolver mConstraintSolver;
 
+        /// Number of solver iterations for the Sequential Impulses technique
+        uint mNbSolverIterations;
+
         /// True if the deactivation (sleeping) of inactive bodies is enabled
         bool mIsDeactivationActive;
 
@@ -71,9 +74,6 @@ class DynamicsWorld : public CollisionWorld {
         /// All the joints of the world
         std::set<Constraint*> mJoints;
 
-        /// All the bodies that are part of contacts or constraints
-        std::set<RigidBody*> mConstrainedBodies;
-
         /// Gravity vector of the world
         Vector3 mGravity;
 
@@ -99,8 +99,8 @@ class DynamicsWorld : public CollisionWorld {
         /// Private assignment operator
         DynamicsWorld& operator=(const DynamicsWorld& world);
 
-        /// Compute the motion of all bodies and update their positions and orientations
+        /// Integrate the positions and orientations of rigid bodies
-        void updateRigidBodiesPositionAndOrientation();
+        void integrateRigidBodiesPositions();
 
         /// Update the position and orientation of a body
         void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@@ -109,8 +109,11 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
-        /// Initialize the constrained velocities array at each step
+        /// Integrate the velocities of rigid bodies
-        void initConstrainedVelocitiesArray();
+        void integrateRigidBodiesVelocities();
+
+        /// Solve the contacts and constraints
+        void solveContactsAndConstraints();
 
         /// Cleanup the constrained velocities array at each step
         void cleanupConstrainedVelocitiesArray();
@@ -212,7 +215,7 @@ inline void DynamicsWorld::stop() {
 
 // Set the number of iterations of the constraint solver
 inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
-    mContactSolver.setNbIterationsSolver(nbIterations);
+    mNbSolverIterations = nbIterations;
 }
 
 // Activate or Deactivate the split impulses for contacts