Merge branch 'sequentialimpulse2' into develop

src/body/RigidBody.cpp

@@ -35,9 +35,9 @@ using namespace reactphysics3d;
                       CollisionShape *collisionShape, bodyindex id)
            : CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
              mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
-             mMassInverse(1.0/mass) {
+             mMassInverse(decimal(1.0) / mass), mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT) {
 
-    mRestitution = 1.0;
+    mRestitution = decimal(1.0);
 
     // Set the body pointer of the AABB and the collision shape
     mAabb->setBodyPointer(this);

src/body/RigidBody.h

@@ -77,6 +77,8 @@ class RigidBody : public CollisionBody {
         // Coefficient of restitution (between 0 and 1) where 1 is for a very bouncy body
         decimal mRestitution;
 
+        // Friction coefficient
+        decimal mFrictionCoefficient;
 
         // -------------------- Methods -------------------- //
 
@@ -153,6 +155,12 @@ class RigidBody : public CollisionBody {
 
         // Set the restitution coefficient
         void setRestitution(decimal restitution) throw(std::invalid_argument);
+
+        // Get the friction coefficient
+        decimal getFrictionCoefficient() const;
+
+        // Set the friction coefficient
+        void setFrictionCoefficient(decimal frictionCoefficient);
 };
 
 // Method that return the mass of the body
@@ -277,6 +285,16 @@ inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_ar
     }
 }
 
+// Get the friction coefficient
+inline decimal RigidBody::getFrictionCoefficient() const {
+    return mFrictionCoefficient;
+}
+
+// Set the friction coefficient
+inline void RigidBody::setFrictionCoefficient(decimal frictionCoefficient) {
+    mFrictionCoefficient = frictionCoefficient;
+}
+
 } // End of the ReactPhyscis3D namespace
 
  #endif

src/collision/narrowphase/EPA/EdgeEPA.h

@@ -105,6 +105,7 @@ inline int EdgeEPA::getIndex() const {
 inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
     mOwnerTriangle = edge.mOwnerTriangle;
     mIndex = edge.mIndex;
+    return *this;
 }
 
 // Return the index of the next counter-clockwise edge of the ownver triangle

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -26,7 +26,7 @@
 // Libraries
 #include "GJKAlgorithm.h"
 #include "Simplex.h"
-#include "../../../constraint/Contact.h"
+#include "../../../constraint/ContactPoint.h"
 #include "../../../configuration.h"
 #include <algorithm>
 #include <cmath>

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -37,7 +37,7 @@
 namespace reactphysics3d {
 
 // Constants
-const decimal REL_ERROR = 1.0e-3;
+const decimal REL_ERROR = decimal(1.0e-3);
 const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
 
 /*  -------------------------------------------------------------------

src/collision/shapes/BoxShape.cpp

@@ -57,7 +57,7 @@ BoxShape::~BoxShape() {
 
 // Return the local inertia tensor of the collision shape
 void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    decimal factor = (1.0 / 3.0) * mass;
+    decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
     decimal xSquare = mExtent.getX() * mExtent.getX();
     decimal ySquare = mExtent.getY() * mExtent.getY();
     decimal zSquare = mExtent.getZ() * mExtent.getZ();

src/collision/shapes/ConeShape.cpp

@@ -45,7 +45,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ConeShape::ConeShape(decimal radius, decimal height)
-          : CollisionShape(CONE), mRadius(radius), mHalfHeight(height/2.0) {
+          : CollisionShape(CONE), mRadius(radius), mHalfHeight(height / decimal(2.0)) {
     assert(radius > 0.0);
     assert(mHalfHeight > 0.0);
     

src/collision/shapes/ConeShape.h

@@ -118,12 +118,12 @@ inline void ConeShape::setRadius(decimal radius) {
 
 // Return the height
 inline decimal ConeShape::getHeight() const {
-    return 2.0 * mHalfHeight;
+    return decimal(2.0) * mHalfHeight;
 }
 
 // Set the height
 inline void ConeShape::setHeight(decimal height) {
-    mHalfHeight = height / 2.0;
+    mHalfHeight = height * decimal(0.5);
 
     // Update the sine of the semi-angle at the apex point
     mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@@ -137,8 +137,10 @@ inline Vector3 ConeShape::getLocalExtents(decimal margin) const {
 // Return the local inertia tensor of the collision shape
 inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
     decimal rSquare = mRadius * mRadius;
-    decimal diagXZ = 0.15 * mass * (rSquare + mHalfHeight);
+    decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
-    tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ);
+    tensor.setAllValues(diagXZ, 0.0, 0.0,
+                        0.0, decimal(0.3) * mass * rSquare,
+                        0.0, 0.0, 0.0, diagXZ);
 }
 
 }; // End of the ReactPhysics3D namespace

src/collision/shapes/CylinderShape.h

@@ -116,7 +116,7 @@ inline decimal CylinderShape::getHeight() const {
 
 // Set the height
 inline void CylinderShape::setHeight(decimal height) {
-    this->mHalfHeight = height / 2.0;
+    mHalfHeight = height * decimal(0.5);
 }
 
 // Return the local extents in x,y and z direction
@@ -126,9 +126,11 @@ inline Vector3 CylinderShape::getLocalExtents(decimal margin) const {
 
 // Return the local inertia tensor of the cylinder
 inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    decimal height = 2.0 * mHalfHeight;
+    decimal height = decimal(2.0) * mHalfHeight;
-    decimal diag = (1.0 / 12.0) * mass * (3 * mRadius * mRadius + height * height);
+    decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
-    tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * mRadius * mRadius, 0.0, 0.0, 0.0, diag);
+    tensor.setAllValues(diag, 0.0, 0.0, 0.0,
+                        decimal(0.5) * mass * mRadius * mRadius, 0.0,
+                        0.0, 0.0, diag);
 }
 
 }; // End of the ReactPhysics3D namespace

src/collision/shapes/SphereShape.h

@@ -121,8 +121,10 @@ inline Vector3 SphereShape::getLocalExtents(decimal margin) const {
 
 // Return the local inertia tensor of the sphere
 inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    decimal diag = 0.4 * mass * mRadius * mRadius;
+    decimal diag = decimal(0.4) * mass * mRadius * mRadius;
-    tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
+    tensor.setAllValues(diag, 0.0, 0.0,
+                        0.0, diag, 0.0,
+                        0.0, 0.0, diag);
 }
 
 }; // End of the ReactPhysics3D namespace

src/configuration.h

@@ -53,52 +53,36 @@ typedef std::pair<bodyindex, bodyindex> bodyindexpair;
 
 // ------------------- Constants ------------------- //
 
-const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
+const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
 
 // Maximum decimal value
-const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
+const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
 
 // Machine epsilon
-const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
+const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
-
-// Infinity
-const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<decimal>::infinity();
 
 // Pi constant
-const reactphysics3d::decimal PI = 3.14159265;
+const decimal PI = decimal(3.14159265);
 
 // Default internal constant timestep in seconds
-const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0;
+const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
+
+// Default friction coefficient for a rigid body
+const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
 
 // True if the deactivation (sleeping) of inactive bodies is enabled
 const bool DEACTIVATION_ENABLED = true;
 
 // // Object margin for collision detection in cm (For GJK-EPA Algorithm)
-const reactphysics3d::decimal OBJECT_MARGIN = 0.04;
+const decimal OBJECT_MARGIN = decimal(0.04);
 
-// Friction coefficient
+// Distance threshold for two contact points for a valid persistent contact (in meters)
-const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
+const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
 
-// Distance threshold for two contact points for a valid persistent contact
+const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
-const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;
-
-// Maximum number of bodies
-const int NB_MAX_BODIES = 100000;
-
-// Maximum number of constraints
-const int NB_MAX_CONSTRAINTS = 100000;
 
 // Number of iterations when solving a LCP problem
-const uint DEFAULT_LCP_ITERATIONS = 15;
+const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
-
-// Number of iterations when solving a LCP problem for error correction
-const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5;
-
-// True if the error correction projection (first order world) is active in the constraint solver
-const bool ERROR_CORRECTION_PROJECTION_ENABLED = true;
-
-// Contacts with penetration depth (in meters) larger that this use error correction with projection
-const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20;
 
 }
 

src/constraint/Constraint.h

@@ -100,21 +100,6 @@ class Constraint {
         // Return the type of the constraint
         ConstraintType getType() const;
 
-        // Compute the jacobian matrix for all mathematical constraints
-        virtual void computeJacobian(int noConstraint,
-                                     decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0;
-
-        // Compute the lowerbounds values for all the mathematical constraints
-        virtual void computeLowerBound(int noConstraint,
-                                       decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0;
-
-        // Compute the upperbounds values for all the mathematical constraints
-        virtual void computeUpperBound(int noConstraint,
-                                       decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0;
-
-        // Compute the error values for all the mathematical constraints
-        virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0;
-
         // Return the number of mathematical constraints
         unsigned int getNbConstraints() const;
 

src/constraint/Contact.cpp

@@ -1,176 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 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 "Contact.h"
-
-using namespace reactphysics3d;
-using namespace std;
-
-// Constructor
-Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo)
-        : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
-          mPenetrationDepth(contactInfo->penetrationDepth),
-          mLocalPointOnBody1(contactInfo->localPoint1),
-          mLocalPointOnBody2(contactInfo->localPoint2),
-          mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
-          mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) {
-    assert(mPenetrationDepth > 0.0);
-    
-    // Compute the auxiliary lower and upper bounds
-    // TODO : Now mC is only the mass of the first body but it is probably wrong
-    // TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
-    mMu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81;
-
-    // Compute the friction vectors that span the tangential friction plane
-    computeFrictionVectors();
-}
-
-// Destructor
-Contact::~Contact() {
-
-}
-
-// This method computes the jacobian matrix for all mathematical constraints
-// The argument "J_sp" is the jacobian matrix of the constraint solver. This method
-// fill in this matrix with all the jacobian matrix of the mathematical constraint
-// of the contact. The argument "noConstraint", is the row were the method have
-// to start to fill in the J_sp matrix.
-void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
-    assert(mBody1);
-    assert(mBody2);
-
-    Vector3 body1Position = mBody1->getTransform().getPosition();
-    Vector3 body2Position = mBody2->getTransform().getPosition();
-    int currentIndex = noConstraint;                        // Current constraint index
-
-    Vector3 r1 = mWorldPointOnBody1 - body1Position;
-    Vector3 r2 = mWorldPointOnBody2 - body2Position;
-    Vector3 r1CrossN = r1.cross(mNormal);
-    Vector3 r2CrossN = r2.cross(mNormal);
-
-    // Compute the jacobian matrix for the body 1 for the contact constraint
-    J_sp[currentIndex][0] = -mNormal.getX();
-    J_sp[currentIndex][1] = -mNormal.getY();
-    J_sp[currentIndex][2] = -mNormal.getZ();
-    J_sp[currentIndex][3] = -r1CrossN.getX();
-    J_sp[currentIndex][4] = -r1CrossN.getY();
-    J_sp[currentIndex][5] = -r1CrossN.getZ();
-
-    // Compute the jacobian matrix for the body 2 for the contact constraint
-    J_sp[currentIndex][6] = mNormal.getX();
-    J_sp[currentIndex][7] = mNormal.getY();
-    J_sp[currentIndex][8] = mNormal.getZ();
-    J_sp[currentIndex][9] = r2CrossN.getX();
-    J_sp[currentIndex][10] = r2CrossN.getY();
-    J_sp[currentIndex][11] = r2CrossN.getZ();
-
-    currentIndex++;
-
-    // Compute the jacobian matrix for the body 1 for the first friction constraint
-    Vector3 r1CrossU1 = r1.cross(mFrictionVectors[0]);
-    Vector3 r2CrossU1 = r2.cross(mFrictionVectors[0]);
-    Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]);
-    Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]);
-    J_sp[currentIndex][0] = -mFrictionVectors[0].getX();
-    J_sp[currentIndex][1] = -mFrictionVectors[0].getY();
-    J_sp[currentIndex][2] = -mFrictionVectors[0].getZ();
-    J_sp[currentIndex][3] = -r1CrossU1.getX();
-    J_sp[currentIndex][4] = -r1CrossU1.getY();
-    J_sp[currentIndex][5] = -r1CrossU1.getZ();
-
-    // Compute the jacobian matrix for the body 2 for the first friction constraint
-    J_sp[currentIndex][6] = mFrictionVectors[0].getX();
-    J_sp[currentIndex][7] = mFrictionVectors[0].getY();
-    J_sp[currentIndex][8] = mFrictionVectors[0].getZ();
-    J_sp[currentIndex][9] = r2CrossU1.getX();
-    J_sp[currentIndex][10] = r2CrossU1.getY();
-    J_sp[currentIndex][11] = r2CrossU1.getZ();
-
-    currentIndex++;
-
-    // Compute the jacobian matrix for the body 1 for the second friction constraint
-    J_sp[currentIndex][0] = -mFrictionVectors[1].getX();
-    J_sp[currentIndex][1] = -mFrictionVectors[1].getY();
-    J_sp[currentIndex][2] = -mFrictionVectors[1].getZ();
-    J_sp[currentIndex][3] = -r1CrossU2.getX();
-    J_sp[currentIndex][4] = -r1CrossU2.getY();
-    J_sp[currentIndex][5] = -r1CrossU2.getZ();
-
-    // Compute the jacobian matrix for the body 2 for the second friction constraint
-    J_sp[currentIndex][6] = mFrictionVectors[1].getX();
-    J_sp[currentIndex][7] = mFrictionVectors[1].getY();
-    J_sp[currentIndex][8] = mFrictionVectors[1].getZ();
-    J_sp[currentIndex][9] = r2CrossU2.getX();
-    J_sp[currentIndex][10] = r2CrossU2.getY();
-    J_sp[currentIndex][11] = r2CrossU2.getZ();
-}
-
-// Compute the lowerbounds values for all the mathematical constraints. The
-// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
-// this methods has to fill in this vector starting from the row "noConstraint"
-void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const {
-    assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
-
-    lowerBounds[noConstraint] = 0.0;            // Lower bound for the contact constraint
-    lowerBounds[noConstraint + 1] = -mMu_mc_g;  // Lower bound for the first friction constraint
-    lowerBounds[noConstraint + 2] = -mMu_mc_g;  // Lower bound for the second friction constraint
-}
-
-// Compute the upperbounds values for all the mathematical constraints. The
-// argument "upperBounds" is the upperbounds values vector of the constraint solver and
-// this methods has to fill in this vector starting from the row "noConstraint"
-void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const {
-    assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
-
-    upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint
-    upperBounds[noConstraint + 1] = mMu_mc_g;     // Upper bound for the first friction constraint
-    upperBounds[noConstraint + 2] = mMu_mc_g;     // Upper bound for the second friction constraint
-}
-
-// Compute the error values for all the mathematical constraints. The argument
-// "errorValues" is the error values vector of the constraint solver and this
-// method has to fill in this vector starting from the row "noConstraint"
-void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const {
-    assert(mBody1);
-    assert(mBody2);
-
-	// TODO : Do we need this casting anymore ?
-    RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(mBody1);
-    RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(mBody2);
-
-    assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
-
-    // Compute the error value for the contact constraint
-    Vector3 velocity1 = rigidBody1->getLinearVelocity();
-    Vector3 velocity2 = rigidBody2->getLinearVelocity();
-    decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
-    decimal errorValue = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2));
-    
-    // Assign the error value to the vector of error values
-    errorValues[noConstraint] = errorValue;    // Error value for contact constraint
-    errorValues[noConstraint + 1] = 0.0;       // Error value for friction constraint
-    errorValues[noConstraint + 2] = 0.0;       // Error value for friction constraint
-}

src/constraint/ContactPoint.cpp

@@ -0,0 +1,50 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2012 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 "ContactPoint.h"
+
+using namespace reactphysics3d;
+using namespace std;
+
+// Constructor
+ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
+                           const ContactInfo* contactInfo)
+             : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
+               mPenetrationDepth(contactInfo->penetrationDepth),
+               mLocalPointOnBody1(contactInfo->localPoint1),
+               mLocalPointOnBody2(contactInfo->localPoint2),
+               mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
+              mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
+              mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) {
+
+    assert(mPenetrationDepth > 0.0);
+
+}
+
+// Destructor
+ContactPoint::~ContactPoint() {
+
+}

src/constraint/ContactPoint.h

@@ -23,8 +23,8 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef CONTACT_H
+#ifndef CONTACT_POINT_H
-#define CONTACT_H
+#define CONTACT_POINT_H
 
 // Libraries
 #include "Constraint.h"
@@ -52,16 +52,13 @@
 namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
-    Class Contact :
+    Class ContactPoint :
-        This class represents a collision contact between two bodies in
+        This class represents a collision contact point between two
-        the physics engine. The contact class inherits from the
+        bodies in the physics engine. The ContactPoint class inherits from
-        Constraint class. Each Contact represent a contact between two bodies
+        the Constraint class.
-        and contains the two contact points on each body. The contact has 3
-        mathematical constraints (1 for the contact constraint, and 2
-        for the friction constraints).
     -------------------------------------------------------------------
 */
-class Contact : public Constraint {
+class ContactPoint : public Constraint {
 
     protected :
 
@@ -85,31 +82,29 @@ class Contact : public Constraint {
         // Contact point on body 2 in world space
         Vector3 mWorldPointOnBody2;
 
+        // True if the contact is a resting contact (exists for more than one time step)
+        bool mIsRestingContact;
+
         // Two orthogonal vectors that span the tangential friction plane
         std::vector<Vector3> mFrictionVectors;
-
-        decimal mMu_mc_g;
         
         // -------------------- Methods -------------------- //
 
         // Private copy-constructor
-        Contact(const Contact& contact);
+        ContactPoint(const ContactPoint& contact);
 
         // Private assignment operator
-        Contact& operator=(const Contact& contact);
+        ContactPoint& operator=(const ContactPoint& contact);
-
-        // Compute the two friction vectors that span the tangential friction plane
-        void computeFrictionVectors();
 
     public :
 
         // -------------------- Methods -------------------- //
 
         // Constructor
-        Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
+        ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
 
         // Destructor
-        virtual ~Contact();
+        virtual ~ContactPoint();
 
         // Return the normal vector of the contact
         Vector3 getNormal() const;
@@ -135,20 +130,23 @@ class Contact : public Constraint {
         // Set the contact world point on body 2
         void setWorldPointOnBody2(const Vector3& worldPoint);
 
-        // Compute the jacobian matrix for all mathematical constraints
+        // Return true if the contact is a resting contact
-        virtual void computeJacobian(int noConstraint,
+        bool getIsRestingContact() const;
-                                     decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const;
 
-        // Compute the lowerbounds values for all the mathematical constraints
+        // Set the mIsRestingContact variable
-        virtual void computeLowerBound(int noConstraint,
+        void setIsRestingContact(bool isRestingContact);
-                                       decimal lowerBounds[NB_MAX_CONSTRAINTS]) const;
 
-        // Compute the upperbounds values for all the mathematical constraints
+        // Get the first friction vector
-        virtual void computeUpperBound(int noConstraint,
+        Vector3 getFrictionVector1() const;
-                                       decimal upperBounds[NB_MAX_CONSTRAINTS]) const;
 
-        // Compute the error values for all the mathematical constraints
+        // Set the first friction vector
-        virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const;
+        void setFrictionVector1(const Vector3& frictionVector1);
+
+        // Get the second friction vector
+        Vector3 getFrictionVector2() const;
+
+        // Set the second friction vector
+        void setFrictionVector2(const Vector3& frictionVector2);
 
         // Return the penetration depth
         decimal getPenetrationDepth() const;
@@ -159,73 +157,89 @@ class Contact : public Constraint {
         #endif
 };
 
-// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
-// The two vectors have to be such that : v1 x v2 = contactNormal
-inline void Contact::computeFrictionVectors() {
-    // Delete the current friction vectors
-    mFrictionVectors.clear();
-
-    // Compute the first orthogonal vector
-    Vector3 vector1 = mNormal.getOneOrthogonalVector();
-    mFrictionVectors.push_back(vector1);
-
-    // Compute the second orthogonal vector using the cross product
-    mFrictionVectors.push_back(mNormal.cross(vector1));
-}
-
 // Return the normal vector of the contact
-inline Vector3 Contact::getNormal() const {
+inline Vector3 ContactPoint::getNormal() const {
     return mNormal;
 }
 
 // Set the penetration depth of the contact
-inline void Contact::setPenetrationDepth(decimal penetrationDepth) {
+inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
     this->mPenetrationDepth = penetrationDepth;
 }
 
 // Return the contact point on body 1
-inline Vector3 Contact::getLocalPointOnBody1() const {
+inline Vector3 ContactPoint::getLocalPointOnBody1() const {
     return mLocalPointOnBody1;
 }
 
 // Return the contact point on body 2
-inline Vector3 Contact::getLocalPointOnBody2() const {
+inline Vector3 ContactPoint::getLocalPointOnBody2() const {
     return mLocalPointOnBody2;
 }
 
 // Return the contact world point on body 1
-inline Vector3 Contact::getWorldPointOnBody1() const {
+inline Vector3 ContactPoint::getWorldPointOnBody1() const {
     return mWorldPointOnBody1;
 }
 
 // Return the contact world point on body 2
-inline Vector3 Contact::getWorldPointOnBody2() const {
+inline Vector3 ContactPoint::getWorldPointOnBody2() const {
     return mWorldPointOnBody2;
 }
 
 // Set the contact world point on body 1
-inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) {
+inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
     mWorldPointOnBody1 = worldPoint;
 }
 
 // Set the contact world point on body 2
-inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) {
+inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
     mWorldPointOnBody2 = worldPoint;
 }
 
+// Return true if the contact is a resting contact
+inline bool ContactPoint::getIsRestingContact() const {
+    return mIsRestingContact;
+}
+
+// Set the mIsRestingContact variable
+inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
+    mIsRestingContact = isRestingContact;
+}
+
+// Get the first friction vector
+inline Vector3 ContactPoint::getFrictionVector1() const {
+    return mFrictionVectors[0];
+}
+
+// Set the first friction vector
+inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
+    mFrictionVectors[0] = frictionVector1;
+}
+
+// Get the second friction vector
+inline Vector3 ContactPoint::getFrictionVector2() const {
+    return mFrictionVectors[1];
+}
+
+// Set the second friction vector
+inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
+    mFrictionVectors[1] = frictionVector2;
+}
+
 // Return the penetration depth of the contact
-inline decimal Contact::getPenetrationDepth() const {
+inline decimal ContactPoint::getPenetrationDepth() const {
     return mPenetrationDepth;
 }
 
 
 #ifdef VISUAL_DEBUG
-inline void Contact::draw() const {
+inline void ContactPoint::draw() const {
     glColor3f(1.0, 0.0, 0.0);
     glutSolidSphere(0.3, 20, 20);
 }
 #endif 
 
-} // End of the ReactPhysics3D namespace
+}
 
 #endif

src/engine/CollisionWorld.cpp

@@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     collisionBody->CollisionBody::~CollisionBody();
 
     // Remove the collision body from the list of bodies
-    mBodies.erase(collisionBody);           // TOOD : Maybe use a set to make this faster
+    mBodies.erase(collisionBody);           // TODO : Maybe use a set to make this faster
 
     // Free the object from the memory pool
     mMemoryPoolCollisionBodies.freeObject(collisionBody);

src/engine/CollisionWorld.h

@@ -35,7 +35,7 @@
 #include "OverlappingPair.h"
 #include "../collision/CollisionDetection.h"
 #include "../constraint/Constraint.h"
-#include "../constraint/Contact.h"
+#include "../constraint/ContactPoint.h"
 #include "../memory/MemoryPool.h"
 
 // Namespace reactphysics3d

src/engine/ConstraintSolver.cpp

@@ -1,540 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 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 "ConstraintSolver.h"
-#include "DynamicsWorld.h"
-#include "../body/RigidBody.h"
-
-using namespace reactphysics3d;
-using namespace std;
-
-
-// Constructor
-ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
-                 :world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
-                  nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
-                  isErrorCorrectionActive(false) {
-
-}
-
-// Destructor
-ConstraintSolver::~ConstraintSolver() {
-
-}
-
- // Initialize the constraint solver before each solving
-void ConstraintSolver::initialize() {
-    Constraint* constraint;
-    
-    nbConstraints = 0;
-    nbConstraintsError = 0;
-
-    // For each constraint
-    vector<Constraint*>::iterator it;
-    for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
-        constraint = *it;
-
-        // If the constraint is active
-        if (constraint->isActive()) {
-            activeConstraints.push_back(constraint);
-
-            // Add the two bodies of the constraint in the constraintBodies list
-            constraintBodies.insert(constraint->getBody1());
-            constraintBodies.insert(constraint->getBody2());
-
-            // Fill in the body number maping
-            bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
-            bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
-
-            // Update the size of the jacobian matrix
-            nbConstraints += constraint->getNbConstraints();
-            
-            // Update the size of the jacobian matrix for error correction projection
-            if (constraint->getType() == CONTACT) {
-                nbConstraintsError++;
-            }
-        }
-    }
-
-    // Compute the number of bodies that are part of some active constraint
-    nbBodies = bodyNumberMapping.size();
-
-    assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
-    assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
-    assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
-}
-
-// Fill in all the matrices needed to solve the LCP problem
-// Notice that all the active constraints should have been evaluated first
-void ConstraintSolver::fillInMatrices(decimal dt) {
-    Constraint* constraint;
-    decimal oneOverDt = 1.0 / dt;
-
-    // For each active constraint
-    int noConstraint = 0;
-    int noConstraintError = 0;
-    
-    for (int c=0; c<activeConstraints.size(); c++) {
-        
-        constraint = activeConstraints.at(c);
-
-        // Fill in the J_sp matrix
-        constraint->computeJacobian(noConstraint, J_sp);
-
-        // Fill in the body mapping matrix
-        for(int i=0; i<constraint->getNbConstraints(); i++) {
-            bodyMapping[noConstraint+i][0] = constraint->getBody1();
-            bodyMapping[noConstraint+i][1] = constraint->getBody2();
-        }
-
-        // Fill in the limit vectors for the constraint
-        constraint->computeLowerBound(noConstraint, lowerBounds);
-        constraint->computeUpperBound(noConstraint, upperBounds);
-
-        // Fill in the error vector
-        constraint->computeErrorValue(noConstraint, errorValues);
-
-        // Get the cached lambda values of the constraint
-        for (int i=0; i<constraint->getNbConstraints(); i++) {
-            lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
-        }
-
-        // If the constraint is a contact
-        if (constraint->getType() == CONTACT) {
-            Contact* contact = dynamic_cast<Contact*>(constraint);
-            decimal penetrationDepth = contact->getPenetrationDepth();
-
-            // If error correction with projection is active
-            if (isErrorCorrectionActive) {
-                
-                // Fill in the error correction projection parameters
-                lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
-                upperBoundsError[noConstraintError] = upperBounds[noConstraint];
-                for (int i=0; i<12; i++) {
-                  J_spError[noConstraintError][i] = J_sp[noConstraint][i];
-                }
-                bodyMappingError[noConstraintError][0] = constraint->getBody1();
-                bodyMappingError[noConstraintError][1] = constraint->getBody2();
-                penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
-                
-                // If the penetration depth is small
-                if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
-                    // Use the Baumgarte error correction term for contacts instead of
-                    // first order world projection
-                    errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
-                }
-
-                noConstraintError++;
-            }
-            else {  // If error correction with projection is not active
-                // Add the Baumgarte error correction term for contacts
-                errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
-            }
-        }
-        
-        noConstraint += constraint->getNbConstraints();
-    }
-
-    // For each current body that is implied in some constraint
-    RigidBody* rigidBody;
-    Body* body;
-    uint b=0;
-    for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) {
-        body = *it;
-        uint bodyNumber = bodyNumberMapping[body];
-        
-        // TODO : Use polymorphism and remove this downcasting
-        rigidBody = dynamic_cast<RigidBody*>(body);
-        assert(rigidBody);
-
-        // Compute the vector V1 with initial velocities values
-        Vector3 linearVelocity = rigidBody->getLinearVelocity();
-        Vector3 angularVelocity = rigidBody->getAngularVelocity();
-        int bodyIndexArray = 6 * bodyNumber;
-        V1[bodyIndexArray] = linearVelocity[0];
-        V1[bodyIndexArray + 1] = linearVelocity[1];
-        V1[bodyIndexArray + 2] = linearVelocity[2];
-        V1[bodyIndexArray + 3] = angularVelocity[0];
-        V1[bodyIndexArray + 4] = angularVelocity[1];
-        V1[bodyIndexArray + 5] = angularVelocity[2];
-
-        // Compute the vector Vconstraint with final constraint velocities
-        Vconstraint[bodyIndexArray] = 0.0;
-        Vconstraint[bodyIndexArray + 1] = 0.0;
-        Vconstraint[bodyIndexArray + 2] = 0.0;
-        Vconstraint[bodyIndexArray + 3] = 0.0;
-        Vconstraint[bodyIndexArray + 4] = 0.0;
-        Vconstraint[bodyIndexArray + 5] = 0.0;
-        
-        // Compute the vector Vconstraint with final constraint velocities
-        if (isErrorCorrectionActive) {
-            VconstraintError[bodyIndexArray] = 0.0;
-            VconstraintError[bodyIndexArray + 1] = 0.0;
-            VconstraintError[bodyIndexArray + 2] = 0.0;
-            VconstraintError[bodyIndexArray + 3] = 0.0;
-            VconstraintError[bodyIndexArray + 4] = 0.0;
-            VconstraintError[bodyIndexArray + 5] = 0.0;
-        }
-        
-        // Compute the vector with forces and torques values
-        Vector3 externalForce = rigidBody->getExternalForce();
-        Vector3 externalTorque = rigidBody->getExternalTorque();
-        Fext[bodyIndexArray] = externalForce[0];
-        Fext[bodyIndexArray + 1] = externalForce[1];
-        Fext[bodyIndexArray + 2] = externalForce[2];
-        Fext[bodyIndexArray + 3] = externalTorque[0];
-        Fext[bodyIndexArray + 4] = externalTorque[1];
-        Fext[bodyIndexArray + 5] = externalTorque[2];
-		
-        // Initialize the mass and inertia tensor matrices
-        Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-        Minv_sp_mass_diag[bodyNumber] = 0.0;
-
-        // If the motion of the rigid body is enabled
-        if (rigidBody->getIsMotionEnabled()) {
-            Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
-            Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
-        }
-    }
-}
-
-// Compute the vector b
-void ConstraintSolver::computeVectorB(decimal dt) {
-    uint indexBody1, indexBody2;
-    decimal oneOverDT = 1.0 / dt;
-
-    for (uint c = 0; c<nbConstraints; c++) {
-        
-        // b = errorValues * oneOverDT;
-        b[c] = errorValues[c] * oneOverDT;
-		
-        // Substract 1.0/dt*J*V to the vector b
-        indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
-        indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-        decimal multiplication = 0.0;
-        int body1ArrayIndex = 6 * indexBody1;
-        int body2ArrayIndex = 6 * indexBody2;
-        for (uint i=0; i<6; i++) {
-                multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
-                multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
-        }
-        b[c] -= multiplication * oneOverDT ;
-
-        // Substract J*M^-1*F_ext to the vector b
-        decimal value1 = 0.0;
-        decimal value2 = 0.0;
-        decimal sum1, sum2;
-        value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
-                          J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
-                          J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
-        value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
-                           J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
-                           J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
-        for (uint i=0; i<3; i++) {
-                sum1 = 0.0;
-                sum2 = 0.0;
-                for (uint j=0; j<3; j++) {
-                        sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
-                        sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
-                }
-                value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
-                value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
-        }
-
-        b[c] -= value1 + value2;	
-    }
-}
-
-// Compute the vector b for error correction projection
-void ConstraintSolver::computeVectorBError(decimal dt) {
-    decimal oneOverDT = 1.0 / dt;
-
-    for (uint c = 0; c<nbConstraintsError; c++) {
-        
-        // b = errorValues * oneOverDT;
-        if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
-            bError[c] = penetrationDepths[c] * oneOverDT;
-        }
-        else {
-            bError[c] = 0.0;
-        }
-    }
-}
-
-// Compute the matrix B_sp
-void ConstraintSolver::computeMatrixB_sp() {
-	uint indexConstraintArray, indexBody1, indexBody2;
-	
-    // For each constraint
-    for (uint c = 0; c<nbConstraints; c++) {
-
-        indexConstraintArray = 6 * c;
-        indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
-        indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-        B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
-        B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
-        B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
-        B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
-        B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
-        B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
-
-        for (uint i=0; i<3; i++) {
-            B_sp[0][indexConstraintArray + 3 + i] = 0.0;
-            B_sp[1][indexConstraintArray + 3 + i] = 0.0;
-            for (uint j=0; j<3; j++) {
-                    B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
-                    B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
-            }
-        }
-    }
-}
-
-// Same as B_sp matrix but for error correction projection and
-// therefore, only for contact constraints
-void ConstraintSolver::computeMatrixB_spErrorCorrection() {
-    uint indexConstraintArray, indexBody1, indexBody2;
-	
-    // For each contact constraint
-    for (uint c = 0; c<nbConstraintsError; c++) {
-
-        indexConstraintArray = 6 * c;
-        indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
-        indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
-        B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
-        B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
-        B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
-        B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
-        B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
-        B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
-
-        for (uint i=0; i<3; i++) {
-            B_spError[0][indexConstraintArray + 3 + i] = 0.0;
-            B_spError[1][indexConstraintArray + 3 + i] = 0.0;
-
-            for (uint j=0; j<3; j++) {
-                B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
-                B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
-            }            
-        }
-    }
-}
-
-// Compute the vector V_constraint (which corresponds to the constraint part of
-// the final V2 vector) according to the formula
-// V_constraint = dt * (M^-1 * J^T * lambda)
-// Note that we use the vector V to store both V1 and V_constraint.
-// Note that M^-1 * J^T = B.
-// This method is called after that the LCP solver has computed lambda
-void ConstraintSolver::computeVectorVconstraint(decimal dt) {
-    uint indexBody1Array, indexBody2Array, indexConstraintArray;
-	uint j;
-	
-    // Compute dt * (M^-1 * J^T * lambda
-    for (uint i=0; i<nbConstraints; i++) {
-        indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
-        indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
-		indexConstraintArray = 6 * i;
-		for (j=0; j<6; j++) {
-			Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt;
-			Vconstraint[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i] * dt;
-		}
-    }
-}
-
-// Same as computeVectorVconstraint() but for error correction projection
-void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
-    uint indexBody1Array, indexBody2Array, indexConstraintArray;
-	uint j;
-	
-    // Compute M^-1 * J^T * lambda
-    for (uint i=0; i<nbConstraintsError; i++) {
-        indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
-        indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
-		indexConstraintArray = 6 * i;
-		for (j=0; j<6; j++) {
-			VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
-			VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
-		}
-    }
-}
-
-
-// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
-// This method outputs the result in the lambda vector
-void ConstraintSolver::solveLCP() {
-
-    for (uint i=0; i<nbConstraints; i++) {
-            lambda[i] = lambdaInit[i];
-    }
-
-    uint indexBody1Array, indexBody2Array;
-    decimal deltaLambda;
-    decimal lambdaTemp;
-    uint i, iter;
-
-    // Compute the vector a
-    computeVectorA();
-
-
-    // For each constraint
-    for (i=0; i<nbConstraints; i++) {
-        uint indexConstraintArray = 6 * i;
-        d[i] = 0.0;
-        for (uint j=0; j<6; j++) {
-                d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
-        }
-    }
-
-    for(iter=0; iter<nbIterationsLCP; iter++) {
-        for (i=0; i<nbConstraints; i++) {
-            indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
-            indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
-            uint indexConstraintArray = 6 * i;
-            deltaLambda = b[i];
-            for (uint j=0; j<6; j++) {
-                    deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
-            }
-            deltaLambda /= d[i];
-            lambdaTemp = lambda[i];
-            lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
-            deltaLambda = lambda[i] - lambdaTemp;
-            for (uint j=0; j<6; j++) {
-                    a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
-                    a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
-            }
-        }
-    }
-}
-
-// Solve a LCP problem for error correction projection
-// using the Projected-Gauss-Seidel algorithm
-// This method outputs the result in the lambda vector
-void ConstraintSolver::solveLCPErrorCorrection() {
-
-    for (uint i=0; i<nbConstraintsError; i++) {
-        lambdaError[i] = 0;
-    }
-
-    uint indexBody1Array, indexBody2Array;
-    decimal deltaLambda;
-    decimal lambdaTemp;
-    uint i, iter;
-
-    // Compute the vector a
-    computeVectorAError();
-
-
-    // For each constraint
-    for (i=0; i<nbConstraintsError; i++) {
-        uint indexConstraintArray = 6 * i;
-        d[i] = 0.0;
-        for (uint j=0; j<6; j++) {
-           d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
-        }
-    }
-
-    for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
-        for (i=0; i<nbConstraintsError; i++) {
-            indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
-            indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
-            uint indexConstraintArray = 6 * i;
-            deltaLambda = bError[i];
-            for (uint j=0; j<6; j++) {
-                deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
-            }
-            deltaLambda /= d[i];
-            lambdaTemp = lambdaError[i];
-            lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
-            deltaLambda = lambdaError[i] - lambdaTemp;
-            for (uint j=0; j<6; j++) {
-                    aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
-                    aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
-            }
-        }
-    }
-}
-
-// Compute the vector a used in the solve() method
-// Note that a = B * lambda
-void ConstraintSolver::computeVectorA() {
-    uint i;
-    uint indexBody1Array, indexBody2Array;
-    
-    // Init the vector a with zero values
-    for (i=0; i<6*nbBodies; i++) {
-       a[i] = 0.0;
-    }
-
-    for(i=0; i<nbConstraints; i++) {
-        indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
-        indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
-		uint indexConstraintArray = 6 * i;
-		for (uint j=0; j<6; j++) {
-			a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i];
-			a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i];
-		}
-    }
-}
-
-// Same as computeVectorA() but for error correction projection
-void ConstraintSolver::computeVectorAError() {
-    uint i;
-    uint indexBody1Array, indexBody2Array;
-    
-    // Init the vector a with zero values
-    for (i=0; i<6*nbBodies; i++) {
-       aError[i] = 0.0;
-    }
-
-    for(i=0; i<nbConstraintsError; i++) {
-        indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
-        indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
-		uint indexConstraintArray = 6 * i;
-		for (uint j=0; j<6; j++) {
-			aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
-			aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
-		}
-    }
-}
-
-
-// Cache the lambda values in order to reuse them in the next step
-// to initialize the lambda vector
-void ConstraintSolver::cacheLambda() {
-    
-    // For each active constraint
-    int noConstraint = 0;
-    for (int c=0; c<activeConstraints.size(); c++) {
-
-        // For each constraint of the contact
-        for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
-            // Get the lambda value that have just been computed
-            activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]);
-        }
-
-        noConstraint += activeConstraints[c]->getNbConstraints();
-    }
-}

src/engine/ConstraintSolver.h

@@ -1,237 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 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 CONSTRAINT_SOLVER_H
-#define CONSTRAINT_SOLVER_H
-
-// Libraries
-#include "../configuration.h"
-#include "../constraint/Constraint.h"
-#include <map>
-#include <set>
-
-// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Declarations
-class DynamicsWorld;
-    
-    
-/*  -------------------------------------------------------------------
-    Class ConstrainSolver :
-        This class represents the constraint solver. The constraint solver
-        is based on the theory from the paper "Iterative Dynamics with
-        Temporal Coherence" from Erin Catto. We keep the same notations as
-        in the paper. The idea is to construct a LCP problem and then solve
-        it using a Projected Gauss Seidel (PGS) solver.
- 
-        The idea is to solve the following system for lambda :
-                JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0
-        
-        By default, error correction using first world order projections (described
-        by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and
-        Versatile Multibody Dynamics Animation") is used for very large penetration
-        depths. Error correction with projection requires to solve another LCP problem
-        that is simpler than the above one and by considering only the contact
-        constraints. The LCP problem for error correction is the following one :
-                J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0
- 
-        where "d" is a vector with penetration depths. If the penetration depth of
-       a given contact is not very large, we use the Baumgarte error correction (see
-       the paper from Erin Catto).
-        
-    -------------------------------------------------------------------
-*/
-class ConstraintSolver {
-    private:
-        DynamicsWorld* world;                           // Reference to the world
-        std::vector<Constraint*> activeConstraints;     // Current active constraints in the physics world
-        bool isErrorCorrectionActive;                   // True if error correction (with world order) is active
-        uint nbIterationsLCP;                           // Number of iterations of the LCP solver
-        uint nbIterationsLCPErrorCorrection;            // Number of iterations of the LCP solver for error correction
-        uint nbConstraints;                             // Total number of constraints (with the auxiliary constraints)
-        uint nbConstraintsError;                        // Number of constraints for error correction projection (only contact constraints)
-        uint nbBodies;                                  // Current number of bodies in the physics world
-        std::set<Body*> constraintBodies;               // Bodies that are implied in some constraint
-        std::map<Body*, uint> bodyNumberMapping;        // Map a body pointer with its index number
-        Body* bodyMapping[NB_MAX_CONSTRAINTS][2];       // 2-dimensional array that contains the mapping of body reference
-                                                        // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
-                                                        // the pointer to the body that correspond to the 1x6 J_ij matrix in the
-                                                        // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
-        Body* bodyMappingError[NB_MAX_CONSTRAINTS][2];  // Same as bodyMapping but for error correction projection
-        decimal J_sp[NB_MAX_CONSTRAINTS][2*6];          // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
-                                                        // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
-                                                        // a 1x6 matrix
-        decimal B_sp[2][6*NB_MAX_CONSTRAINTS];          // 2-dimensional array that correspond to a useful matrix in sparse representation
-                                                        // This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
-                                                        // a 6x1 matrix
-        decimal J_spError[NB_MAX_CONSTRAINTS][2*6];     // Same as J_sp for error correction projection (only contact constraints)
-        decimal B_spError[2][6*NB_MAX_CONSTRAINTS];     // Same as B_sp for error correction projection (only contact constraints)
-        decimal b[NB_MAX_CONSTRAINTS];                  // Vector "b" of the LCP problem
-        decimal bError[NB_MAX_CONSTRAINTS];             // Vector "b" of the LCP problem for error correction projection
-        decimal d[NB_MAX_CONSTRAINTS];                  // Vector "d"
-        decimal a[6*NB_MAX_BODIES];                     // Vector "a"
-        decimal aError[6*NB_MAX_BODIES];                // Vector "a" for error correction projection
-        decimal penetrationDepths[NB_MAX_CONSTRAINTS];  // Array of penetration depths for error correction projection
-        decimal lambda[NB_MAX_CONSTRAINTS];             // Lambda vector of the LCP problem
-        decimal lambdaError[NB_MAX_CONSTRAINTS];        // Lambda vector of the LCP problem for error correction projections
-        decimal lambdaInit[NB_MAX_CONSTRAINTS];         // Lambda init vector for the LCP solver
-        decimal errorValues[NB_MAX_CONSTRAINTS];        // Error vector of all constraints
-        decimal lowerBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the low limits for the variables of the LCP problem
-        decimal upperBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the high limits for the variables of the LCP problem
-        decimal lowerBoundsError[NB_MAX_CONSTRAINTS];   // Same as "lowerBounds" but for error correction projections
-        decimal upperBoundsError[NB_MAX_CONSTRAINTS];   // Same as "upperBounds" but for error correction projections
-        Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES];       // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
-        decimal Minv_sp_mass_diag[NB_MAX_BODIES];       // Array that contains for each body the inverse of its mass
-                                                        // This is an array of size nbBodies that contains in each cell a 6x6 matrix
-        decimal V1[6*NB_MAX_BODIES];                    // Array that contains for each body the 6x1 vector that contains linear and angular velocities
-                                                        // Each cell contains a 6x1 vector with linear and angular velocities
-        decimal Vconstraint[6*NB_MAX_BODIES];           // Same kind of vector as V1 but contains the final constraint velocities
-        decimal VconstraintError[6*NB_MAX_BODIES];      // Same kind of vector as V1 but contains the final constraint velocities
-        decimal Fext[6*NB_MAX_BODIES];                  // Array that contains for each body the 6x1 vector that contains external forces and torques
-                                                        // Each cell contains a 6x1 vector with external force and torque.
-        void initialize();                              // Initialize the constraint solver before each solving
-        void fillInMatrices(decimal dt);                // Fill in all the matrices needed to solve the LCP problem
-        void computeVectorB(decimal dt);                // Compute the vector b
-        void computeVectorBError(decimal dt);           // Compute the vector b for error correction projection
-        void computeMatrixB_sp();                       // Compute the matrix B_sp
-        void computeMatrixB_spErrorCorrection();        // Compute the matrix B_spError for error correction projection
-        void computeVectorVconstraint(decimal dt);      // Compute the vector V2
-        void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
-        void cacheLambda();                             // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
-        void computeVectorA();                          // Compute the vector a used in the solve() method
-        void computeVectorAError();                     // Same as computeVectorA() but for error correction projection
-        void solveLCP();                                // Solve a LCP problem using Projected-Gauss-Seidel algorithm
-        void solveLCPErrorCorrection();                 // Solve the LCP problem for error correction projection
-    
-   public:
-        ConstraintSolver(DynamicsWorld* world);                         // Constructor
-        virtual ~ConstraintSolver();                                   // Destructor
-        void solve(decimal dt);                                         // Solve the current LCP problem
-        bool isConstrainedBody(Body* body) const;                      // Return true if the body is in at least one constraint
-        Vector3 getConstrainedLinearVelocityOfBody(Body* body);        // Return the constrained linear velocity of a body after solving the LCP problem
-        Vector3 getConstrainedAngularVelocityOfBody(Body* body);       // Return the constrained angular velocity of a body after solving the LCP problem
-        Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body);   // Return the constrained linear velocity of a body after solving the LCP problem for error correction
-        Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body);  // Return the constrained angular velocity of a body after solving the LCP problem for error correction
-        void cleanup();                                                 // Cleanup of the constraint solver
-        void setNbLCPIterations(uint nbIterations);                     // Set the number of iterations of the LCP solver
-        void setIsErrorCorrectionActive(bool isErrorCorrectionActive);  // Set the isErrorCorrectionActive value
-};
-
-// Return true if the body is in at least one constraint
-inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
-    if(constraintBodies.find(body) != constraintBodies.end()) {
-        return true;
-    }
-    return false;
-}
-
-// Return the constrained linear velocity of a body after solving the LCP problem
-inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
-    assert(isConstrainedBody(body));
-    uint indexBodyArray = 6 * bodyNumberMapping[body];
-    return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
-}
-
-// Return the constrained angular velocity of a body after solving the LCP problem
-inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
-    assert(isConstrainedBody(body));
-    uint indexBodyArray = 6 * bodyNumberMapping[body];
-    return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
-}
-
-// Return the constrained linear velocity of a body after solving the LCP problem for error correction
-inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
-    //assert(isConstrainedBody(body));
-    uint indexBodyArray = 6 * bodyNumberMapping[body];
-    return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
-}  
-
-// Return the constrained angular velocity of a body after solving the LCP problem for error correction
-inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
-    //assert(isConstrainedBody(body));
-    uint indexBodyArray = 6 * bodyNumberMapping[body];
-    return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
-} 
-
-// Cleanup of the constraint solver
-inline void ConstraintSolver::cleanup() {
-    bodyNumberMapping.clear();
-    constraintBodies.clear();
-    activeConstraints.clear();
-}
-
-// Set the number of iterations of the LCP solver
-inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
-    nbIterationsLCP = nbIterations;
-}         
-
-
-// Set the isErrorCorrectionActive value
-inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
-    this->isErrorCorrectionActive = isErrorCorrectionActive;
-}
-
-
-// Solve the current LCP problem
-inline void ConstraintSolver::solve(decimal dt) {
-    
-    // Initialize the solver
-    initialize();
-
-    // Fill-in all the matrices needed to solve the LCP problem
-    fillInMatrices(dt);
-
-    // Compute the vector b
-    computeVectorB(dt);
-    if (isErrorCorrectionActive) {
-        computeVectorBError(dt);
-    }
-
-    // Compute the matrix B
-    computeMatrixB_sp();
-    if (isErrorCorrectionActive) {
-        computeMatrixB_spErrorCorrection();
-    }
-
-    // Solve the LCP problem (computation of lambda)
-    solveLCP();
-    if (isErrorCorrectionActive) {
-        solveLCPErrorCorrection();
-    }
-
-    // Cache the lambda values in order to use them in the next step
-    cacheLambda();
-    
-    // Compute the vector Vconstraint
-    computeVectorVconstraint(dt);
-    if (isErrorCorrectionActive) {
-        computeVectorVconstraintError(dt);
-    }
-}
-
-} // End of ReactPhysics3D namespace
-
-#endif

src/engine/ContactManifold.cpp

@@ -0,0 +1,247 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2012 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 "ContactManifold.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactManifold::ContactManifold(Body* const body1, Body* const body2,
+                                 MemoryPool<ContactPoint>& memoryPoolContacts)
+                : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
+                  mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0),
+                  mMemoryPoolContacts(memoryPoolContacts) {
+    
+}
+
+// Destructor
+ContactManifold::~ContactManifold() {
+    clear();
+}
+
+// Add a contact point in the manifold
+void ContactManifold::addContactPoint(ContactPoint* contact) {
+	
+    // For contact already in the manifold
+    for (uint i=0; i<mNbContactPoints; i++) {
+
+		// Check if the new point point does not correspond to a same contact point
+        // already in the manifold.
+        decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare();
+        if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
+
+            // Delete the new contact
+            contact->ContactPoint::~ContactPoint();
+            mMemoryPoolContacts.freeObject(contact);
+            //removeContact(i);
+
+            return;
+            //break;
+		}
+	}
+    
+    // If the contact manifold is full
+    if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
+        int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
+        int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
+        removeContactPoint(indexToRemove);
+    }
+
+    // Add the new contact point in the manifold
+    mContactPoints[mNbContactPoints] = contact;
+    mNbContactPoints++;
+}
+
+// Remove a contact point from the manifold
+void ContactManifold::removeContactPoint(int index) {
+    assert(index >= 0 && index < mNbContactPoints);
+    assert(mNbContactPoints > 0);
+	
+	// Call the destructor explicitly and tell the memory pool that
+	// the corresponding memory block is now free
+    mContactPoints[index]->ContactPoint::~ContactPoint();
+    mMemoryPoolContacts.freeObject(mContactPoints[index]);
+	
+    // If we don't remove the last index
+    if (index < mNbContactPoints - 1) {
+        mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
+    }
+
+    mNbContactPoints--;
+}
+
+// Update the contact manifold
+// First the world space coordinates of the current contacts in the manifold are recomputed from
+// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
+// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
+// the contacts with a too large distance between the contact points in the plane orthogonal to the
+// contact normal
+void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
+    if (mNbContactPoints == 0) return;
+
+    // Update the world coordinates and penetration depth of the contact points in the manifold
+    for (int i=0; i<mNbContactPoints; i++) {
+        mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
+        mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
+        mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
+    }
+
+    const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
+                                                     PERSISTENT_CONTACT_DIST_THRESHOLD;
+
+    // Remove the contact points that don't represent very well the contact manifold
+    for (int i=mNbContactPoints-1; i>=0; i--) {
+        assert(i>= 0 && i < mNbContactPoints);
+
+        // Compute the distance between contact points in the normal direction
+        decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
+        
+        // If the contacts points are too far from each other in the normal direction
+        if (distanceNormal > squarePersistentContactThreshold) {
+            removeContactPoint(i);
+        }
+        else {
+            // Compute the distance of the two contact points in the plane
+            // orthogonal to the contact normal
+            Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
+                                   mContactPoints[i]->getNormal() * distanceNormal;
+            Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
+
+            // If the orthogonal distance is larger than the valid distance
+            // threshold, we remove the contact
+            if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
+                removeContactPoint(i);
+            }
+        }
+    }    
+}
+
+// Return the index of the contact point with the larger penetration depth. This
+// corresponding contact will be kept in the cache. The method returns -1 is
+// the new contact is the deepest.
+int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
+    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+    int indexMaxPenetrationDepth = -1;
+    decimal maxPenetrationDepth = newContact->getPenetrationDepth();
+
+    // For each contact in the cache
+    for (uint i=0; i<mNbContactPoints; i++) {
+        // If the current contact has a larger penetration depth
+        if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
+            maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
+            indexMaxPenetrationDepth = i;
+        }
+    }
+
+    // Return the index of largest penetration depth
+    return indexMaxPenetrationDepth;
+}
+
+// Return the index that will be removed. The index of the contact point with the larger penetration
+// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
+// the different area and we want to keep the contacts with the largest area. The new point is also
+// kept. In order to compute the area of a quadrilateral, we use the formula :
+// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
+// when we compute this area, we do not calculate it exactly but we
+// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
+// this is only a guess that is faster to compute.
+int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
+
+    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+    decimal area0 = 0.0;       // Area with contact 1,2,3 and newPoint
+    decimal area1 = 0.0;       // Area with contact 0,2,3 and newPoint
+    decimal area2 = 0.0;       // Area with contact 0,1,3 and newPoint
+    decimal area3 = 0.0;       // Area with contact 0,1,2 and newPoint
+
+    if (indexMaxPenetration != 0) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area0 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 1) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area1 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 2) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area2 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 3) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area3 = crossProduct.lengthSquare();
+    }
+    
+    // Return the index of the contact to remove
+    return getMaxArea(area0, area1, area2, area3);
+}
+
+// Return the index of maximum area
+int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
+    if (area0 < area1) {
+        if (area1 < area2) {
+            if (area2 < area3) return 3;
+            else return 2;
+        }
+        else {
+            if (area1 < area3) return 3;
+            else return 1;
+        }
+    }
+    else {
+        if (area0 < area2) {
+            if (area2 < area3) return 3;
+            else return 2;
+        }
+        else {
+            if (area0 < area3) return 3;
+            else return 0;
+        }
+    }
+}
+
+// Clear the contact manifold
+void ContactManifold::clear() {
+    for (uint i=0; i<mNbContactPoints; i++) {
+		
+		// Call the destructor explicitly and tell the memory pool that
+		// the corresponding memory block is now free
+        mContactPoints[i]->ContactPoint::~ContactPoint();
+        mMemoryPoolContacts.freeObject(mContactPoints[i]);
+    }
+    mNbContactPoints = 0;
+}

src/engine/ContactManifold.h

@@ -0,0 +1,244 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2012 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 CONTACT_MANIFOLD_H
+#define	CONTACT_MANIFOLD_H
+
+// Libraries
+#include <vector>
+#include "../body/Body.h"
+#include "../constraint/ContactPoint.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4;   // Maximum number of contacts in the manifold
+
+/*  -------------------------------------------------------------------
+    Class ContactManifold :
+        This class represents the set of contact points between two bodies.
+        The contact manifold is implemented in a way to cache the contact
+        points among the frames for better stability following the
+        "Contact Generation" presentation of Erwin Coumans at GDC 2010
+        conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
+        Some code of this class is based on the implementation of the
+        btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
+        The contacts between two bodies are added one after the other in the cache.
+        When the cache is full, we have to remove one point. The idea is to keep
+        the point with the deepest penetration depth and also to keep the
+        points producing the larger area (for a more stable contact manifold).
+        The new added point is always kept.
+    -------------------------------------------------------------------
+*/
+class ContactManifold {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        // Pointer to the first body
+        Body* const mBody1;
+
+        // Pointer to the second body
+        Body* const mBody2;
+
+        // Contact points in the manifold
+        ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
+
+        // Number of contacts in the cache
+        uint mNbContactPoints;
+
+        // First friction vector of the contact manifold
+        Vector3 mFrictionVector1;
+
+        // Second friction vector of the contact manifold
+        Vector3 mFrictionVector2;
+
+        // First friction constraint accumulated impulse
+        decimal mFrictionImpulse1;
+
+        // Second friction constraint accumulated impulse
+        decimal mFrictionImpulse2;
+
+        // Twist friction constraint accumulated impulse
+        decimal mFrictionTwistImpulse;
+
+        // Reference to the memory pool with the contacts
+        MemoryPool<ContactPoint>& mMemoryPoolContacts;
+
+        // -------------------- Methods -------------------- //
+
+        // Private copy-constructor
+        ContactManifold(const ContactManifold& contactManifold);
+
+        // Private assignment operator
+        ContactManifold& operator=(const ContactManifold& contactManifold);
+
+        // Return the index of maximum area
+        int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
+
+        // Return the index of the contact with the larger penetration depth
+        int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
+
+        // Return the index that will be removed
+        int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
+
+        // Remove a contact point from the manifold
+        void removeContactPoint(int index);
+
+        // Return true if two vectors are approximatively equal
+        bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
+        
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        // Constructor
+        ContactManifold(Body* const mBody1, Body* const mBody2,
+                        MemoryPool<ContactPoint>& mMemoryPoolContacts);
+
+        // Destructor
+        ~ContactManifold();
+
+        // Add a contact point to the manifold
+        void addContactPoint(ContactPoint* contact);
+
+        // Update the contact manifold
+        void update(const Transform& transform1, const Transform& transform2);
+
+        // Clear the contact manifold
+        void clear();
+
+        // Return the number of contact points in the manifold
+        uint getNbContactPoints() const;
+
+        // Return the first friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector1() const;
+
+        // set the first friction vector at the center of the contact manifold
+        void setFrictionVector1(const Vector3& mFrictionVector1);
+
+        // Return the second friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector2() const;
+
+        // set the second friction vector at the center of the contact manifold
+        void setFrictionVector2(const Vector3& mFrictionVector2);
+
+        // Return the first friction accumulated impulse
+        decimal getFrictionImpulse1() const;
+
+        // Set the first friction accumulated impulse
+        void setFrictionImpulse1(decimal frictionImpulse1);
+
+        // Return the second friction accumulated impulse
+        decimal getFrictionImpulse2() const;
+
+        // Set the second friction accumulated impulse
+        void setFrictionImpulse2(decimal frictionImpulse2);
+
+        // Return the friction twist accumulated impulse
+        decimal getFrictionTwistImpulse() const;
+
+        // Set the friction twist accumulated impulse
+        void setFrictionTwistImpulse(decimal frictionTwistImpulse);
+
+        // Return a contact point of the manifold
+        ContactPoint* getContactPoint(uint index) const;
+};
+
+// Return the number of contact points in the manifold
+inline uint ContactManifold::getNbContactPoints() const {
+    return mNbContactPoints;
+}
+
+// Return the first friction vector at the center of the contact manifold
+inline const Vector3& ContactManifold::getFrictionVector1() const {
+    return mFrictionVector1;
+}
+
+// set the first friction vector at the center of the contact manifold
+inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) {
+    mFrictionVector1 = frictionVector1;
+}
+
+// Return the second friction vector at the center of the contact manifold
+inline const Vector3& ContactManifold::getFrictionVector2() const {
+    return mFrictionVector2;
+}
+
+// set the second friction vector at the center of the contact manifold
+inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) {
+    mFrictionVector2 = frictionVector2;
+}
+
+// Return the first friction accumulated impulse
+inline decimal ContactManifold::getFrictionImpulse1() const {
+    return mFrictionImpulse1;
+}
+
+// Set the first friction accumulated impulse
+inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) {
+    mFrictionImpulse1 = frictionImpulse1;
+}
+
+// Return the second friction accumulated impulse
+inline decimal ContactManifold::getFrictionImpulse2() const {
+    return mFrictionImpulse2;
+}
+
+// Set the second friction accumulated impulse
+inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) {
+    mFrictionImpulse2 = frictionImpulse2;
+}
+
+// Return the friction twist accumulated impulse
+inline decimal ContactManifold::getFrictionTwistImpulse() const {
+    return mFrictionTwistImpulse;
+}
+
+// Set the friction twist accumulated impulse
+inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) {
+    mFrictionTwistImpulse = frictionTwistImpulse;
+}
+
+// Return a contact point of the manifold
+inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
+    assert(index >= 0 && index < mNbContactPoints);
+    return mContactPoints[index];
+}  
+
+// Return true if two vectors are approximatively equal
+inline bool ContactManifold::isApproxEqual(const Vector3& vector1,
+                                                  const Vector3& vector2) const {
+    const decimal epsilon = decimal(0.1);
+    return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
+            approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
+            approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
+}  
+
+}
+#endif
+

src/engine/ContactSolver.cpp

@@ -0,0 +1,915 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2012 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 "ContactSolver.h"
+#include "DynamicsWorld.h"
+#include "../body/RigidBody.h"
+
+using namespace reactphysics3d;
+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);
+
+// Constructor
+ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
+                             std::vector<Vector3>& constrainedAngularVelocities,
+                             const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
+              :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
+               mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
+               mContactConstraints(NULL),
+               mConstrainedLinearVelocities(constrainedLinearVelocities),
+               mConstrainedAngularVelocities(constrainedAngularVelocities),
+               mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
+               mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
+               mIsSolveFrictionAtContactManifoldCenterActive(true) {
+
+}
+
+// Destructor
+ContactSolver::~ContactSolver() {
+
+}
+
+// Initialize the constraint solver
+void ContactSolver::initialize() {
+
+    // TODO : Use better memory allocation here
+    mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
+
+    mNbContactManifolds = 0;
+
+    // For each contact manifold of the world
+    vector<ContactManifold*>::iterator it;
+    for (it = mWorld.getContactManifoldsBeginIterator();
+         it != mWorld.getContactManifoldsEndIterator(); ++it) {
+
+        ContactManifold* externalManifold = *it;
+
+        ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
+
+        assert(externalManifold->getNbContactPoints() > 0);
+
+        // Get the two bodies of the contact
+        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();
+
+        // Initialize the internal contact manifold structure using the external
+        // contact manifold
+        internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
+        internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+        internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
+        internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
+        internalManifold.isBody1Moving = body1->getIsMotionEnabled();
+        internalManifold.isBody2Moving = body2->getIsMotionEnabled();
+        internalManifold.massInverseBody1 = body1->getMassInverse();
+        internalManifold.massInverseBody2 = body2->getMassInverse();
+        internalManifold.nbContacts = externalManifold->getNbContactPoints();
+        internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
+        internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+        internalManifold.externalContactManifold = externalManifold;
+
+        // If we solve the friction constraints at the center of the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive) {
+            internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0);
+            internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0);
+        }
+
+        // For each  contact point of the contact manifold
+        for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
+
+            ContactPointSolver& contactPoint = internalManifold.contacts[c];
+
+            // Get a contact point
+            ContactPoint* externalContact = externalManifold->getContactPoint(c);
+
+            // Get the contact point on the two bodies
+            Vector3 p1 = externalContact->getWorldPointOnBody1();
+            Vector3 p2 = externalContact->getWorldPointOnBody2();
+
+            contactPoint.externalContact = externalContact;
+            contactPoint.normal = externalContact->getNormal();
+            contactPoint.r1 = p1 - x1;
+            contactPoint.r2 = p2 - x2;
+            contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
+            contactPoint.isRestingContact = externalContact->getIsRestingContact();
+            externalContact->setIsRestingContact(true);
+            contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
+            contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
+            contactPoint.penetrationImpulse = 0.0;
+            contactPoint.friction1Impulse = 0.0;
+            contactPoint.friction2Impulse = 0.0;
+
+            // If we solve the friction constraints at the center of the contact manifold
+            if (mIsSolveFrictionAtContactManifoldCenterActive) {
+                internalManifold.frictionPointBody1 += p1;
+                internalManifold.frictionPointBody2 += p2;
+            }
+        }
+
+        // If we solve the friction constraints at the center of the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+            internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
+            internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
+            internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
+            internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
+            internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
+            internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
+
+            // If warm starting is active
+            if (mIsWarmStartingActive) {
+
+                // Initialize the accumulated impulses with the previous step accumulated impulses
+                internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
+                internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
+                internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+            }
+            else {
+
+                // Initialize the accumulated impulses to zero
+                internalManifold.friction1Impulse = 0.0;
+                internalManifold.friction2Impulse = 0.0;
+                internalManifold.frictionTwistImpulse = 0.0;
+            }
+        }
+
+        mNbContactManifolds++;
+    }
+
+    // Allocated memory for split impulse velocities
+    // TODO : Use better memory allocation here
+    mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
+    mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
+    assert(mSplitLinearVelocities != NULL);
+    assert(mSplitAngularVelocities != NULL);
+
+    assert(mConstraintBodies.size() > 0);
+    assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
+    assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
+    assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
+
+    // Initialize the split impulse velocities
+    initializeSplitImpulseVelocities();
+}
+
+// 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() {
+    
+    // For each contact constraint
+    for (uint c=0; c<mNbContactManifolds; c++) {
+
+        ContactManifoldSolver& manifold = mContactConstraints[c];
+
+        // Get the inertia tensors of both bodies
+        Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
+        Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
+
+        // If we solve the friction constraints at the center of the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive) {
+            manifold.normal = Vector3(0.0, 0.0, 0.0);
+        }
+
+        // Get the velocities of the bodies
+        const Vector3& v1 = mConstrainedLinearVelocities[manifold.indexBody1];
+        const Vector3& w1 = mConstrainedAngularVelocities[manifold.indexBody1];
+        const Vector3& v2 = mConstrainedLinearVelocities[manifold.indexBody2];
+        const Vector3& w2 = mConstrainedAngularVelocities[manifold.indexBody2];
+
+        // For each contact point constraint
+        for (uint i=0; i<manifold.nbContacts; i++) {
+
+            ContactPointSolver& contactPoint = manifold.contacts[i];
+            ContactPoint* externalContact = contactPoint.externalContact;
+
+            // Compute the velocity difference
+            Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+
+            contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
+            contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+
+            // Compute the inverse mass matrix K for the penetration constraint
+            decimal massPenetration = 0.0;
+            if (manifold.isBody1Moving) {
+                massPenetration += manifold.massInverseBody1 +
+                    ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal);
+            }
+            if (manifold.isBody2Moving) {
+                massPenetration += manifold.massInverseBody2 +
+                    ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
+            }
+            massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
+                                                                          massPenetration :
+                                                                          decimal(0.0);
+
+            // If we do not solve the friction constraints at the center of the contact manifold
+            if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+                // Compute the friction vectors
+                computeFrictionVectors(deltaV, contactPoint);
+
+                contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
+                contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
+                contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
+                contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
+
+                // Compute the inverse mass matrix K for the friction
+                // constraints at each contact point
+                decimal friction1Mass = 0.0;
+                decimal friction2Mass = 0.0;
+                if (manifold.isBody1Moving) {
+                    friction1Mass += manifold.massInverseBody1 +
+                      ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
+                             contactPoint.frictionVector1);
+                    friction2Mass += manifold.massInverseBody1 +
+                            ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
+                            contactPoint.frictionVector2);
+                }
+                if (manifold.isBody2Moving) {
+                    friction1Mass += manifold.massInverseBody2 +
+                            ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
+                                   contactPoint.frictionVector1);
+                    friction2Mass += manifold.massInverseBody2 +
+                            ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
+                                   contactPoint.frictionVector2);
+                }
+                friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
+                                                                          friction1Mass :
+                                                                          decimal(0.0);
+                friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
+                                                                          friction2Mass :
+                                                                          decimal(0.0);
+            }
+
+            // Compute the restitution velocity bias "b". We compute this here instead
+            // of inside the solve() method because we need to use the velocity difference
+            // at the beginning of the contact. Note that if it is a resting contact (normal
+            // velocity under a given threshold), we don't add a restitution velocity bias
+            contactPoint.restitutionBias = 0.0;
+            decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+            if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) {
+                contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
+            }
+
+            // If the warm starting of the contact solver is active
+            if (mIsWarmStartingActive) {
+
+                // Get the cached accumulated impulses from the previous step
+                contactPoint.penetrationImpulse = externalContact->getCachedLambda(0);
+                contactPoint.friction1Impulse = externalContact->getCachedLambda(1);
+                contactPoint.friction2Impulse = externalContact->getCachedLambda(2);
+            }
+
+            // Initialize the split impulses to zero
+            contactPoint.penetrationSplitImpulse = 0.0;
+
+            // If we solve the friction constraints at the center of the contact manifold
+            if (mIsSolveFrictionAtContactManifoldCenterActive) {
+                manifold.normal += contactPoint.normal;
+            }
+        }
+
+        // If we solve the friction constraints at the center of the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+            manifold.normal.normalize();
+
+            Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
+                                          v1 - w1.cross(manifold.r1Friction);
+
+            // Compute the friction vectors
+            computeFrictionVectors(deltaVFrictionPoint, manifold);
+
+            // Compute the inverse mass matrix K for the friction constraints at the center of
+            // the contact manifold
+            manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
+            manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
+            manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
+            manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
+            decimal friction1Mass = 0.0;
+            decimal friction2Mass = 0.0;
+            if (manifold.isBody1Moving) {
+                friction1Mass += manifold.massInverseBody1 +
+                        ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
+                               manifold.frictionVector1);
+                friction2Mass += manifold.massInverseBody1 +
+                        ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
+                               manifold.frictionVector2);
+            }
+            if (manifold.isBody2Moving) {
+                friction1Mass += manifold.massInverseBody2 +
+                        ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
+                               manifold.frictionVector1);
+                friction2Mass += manifold.massInverseBody2 +
+                        ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
+                               manifold.frictionVector2);
+            }
+            decimal frictionTwistMass = manifold.normal.dot(
+                                           manifold.inverseInertiaTensorBody1 *
+                                           manifold.normal) +
+                                        manifold.normal.dot(
+                                           manifold.inverseInertiaTensorBody2 *
+                                           manifold.normal);
+            friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
+                                                                         : decimal(0.0);
+            friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
+                                                                         : decimal(0.0);
+            frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
+                                                                                 frictionTwistMass :
+                                                                                 decimal(0.0);
+        }
+    }
+}
+
+// Warm start the solver
+// For each constraint, we apply the previous impulse (from the previous step)
+// at the beginning. With this technique, we will converge faster towards
+// the solution of the linear system
+void ContactSolver::warmStart() {
+
+    // For each constraint
+    for (uint c=0; c<mNbContactManifolds; c++) {
+
+        ContactManifoldSolver& contactManifold = mContactConstraints[c];
+
+        bool atLeastOneRestingContactPoint = false;
+
+        for (uint i=0; i<contactManifold.nbContacts; i++) {
+
+            ContactPointSolver& contactPoint = contactManifold.contacts[i];
+
+            // If it is not a new contact (this contact was already existing at last time step)
+            if (contactPoint.isRestingContact) {
+
+                atLeastOneRestingContactPoint = true;
+
+                // --------- Penetration --------- //
+
+                // Compute the impulse P = J^T * lambda
+                const Impulse impulsePenetration = computePenetrationImpulse(
+                                                     contactPoint.penetrationImpulse, contactPoint);
+
+                // Apply the impulse to the bodies of the constraint
+                applyImpulse(impulsePenetration, contactManifold);
+
+                // If we do not solve the friction constraints at the center of the contact manifold
+                if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+                    // Project the old friction impulses (with old friction vectors) into
+                    // the new friction vectors to get the new friction impulses
+                    Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
+                                                 contactPoint.oldFrictionVector1 +
+                                                 contactPoint.friction2Impulse *
+                                                 contactPoint.oldFrictionVector2;
+                    contactPoint.friction1Impulse = oldFrictionImpulse.dot(
+                                                       contactPoint.frictionVector1);
+                    contactPoint.friction2Impulse = oldFrictionImpulse.dot(
+                                                       contactPoint.frictionVector2);
+
+                    // --------- Friction 1 --------- //
+
+                    // Compute the impulse P = J^T * lambda
+                    const Impulse impulseFriction1 = computeFriction1Impulse(
+                                                       contactPoint.friction1Impulse, contactPoint);
+
+                    // Apply the impulses to the bodies of the constraint
+                    applyImpulse(impulseFriction1, contactManifold);
+
+                    // --------- Friction 2 --------- //
+
+                    // Compute the impulse P=J^T * lambda
+                   const Impulse impulseFriction2 = computeFriction2Impulse(
+                                                       contactPoint.friction2Impulse, contactPoint);
+
+                    // Apply the impulses to the bodies of the constraint
+                    applyImpulse(impulseFriction2, contactManifold);
+                }
+            }
+            else {  // If it is a new contact point
+
+                // Initialize the accumulated impulses to zero
+                contactPoint.penetrationImpulse = 0.0;
+                contactPoint.friction1Impulse = 0.0;
+                contactPoint.friction2Impulse = 0.0;
+            }
+        }
+
+        // If we solve the friction constraints at the center of the contact manifold and there is
+        // at least one resting contact point in the contact manifold
+        if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
+
+            // Project the old friction impulses (with old friction vectors) into the new friction
+            // vectors to get the new friction impulses
+            Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
+                                         contactManifold.oldFrictionVector1 +
+                                         contactManifold.friction2Impulse *
+                                         contactManifold.oldFrictionVector2;
+            contactManifold.friction1Impulse = oldFrictionImpulse.dot(
+                                                  contactManifold.frictionVector1);
+            contactManifold.friction2Impulse = oldFrictionImpulse.dot(
+                                                  contactManifold.frictionVector2);
+
+            // ------ First friction constraint at the center of the contact manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
+                                          contactManifold.friction1Impulse;
+            Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
+                                           contactManifold.friction1Impulse;
+            Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
+                                         contactManifold.friction1Impulse;
+            Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
+                                          contactManifold.friction1Impulse;
+            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 manifold ----- //
+
+            // Compute the impulse P = J^T * lambda
+            linearImpulseBody1 = -contactManifold.frictionVector2 *
+                                  contactManifold.friction2Impulse;
+            angularImpulseBody1 = -contactManifold.r1CrossT2 *
+                                   contactManifold.friction2Impulse;
+            linearImpulseBody2 = contactManifold.frictionVector2 *
+                                 contactManifold.friction2Impulse;
+            angularImpulseBody2 = contactManifold.r2CrossT2 *
+                                  contactManifold.friction2Impulse;
+            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 manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+            angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
+            linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
+            angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
+            const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+                                               linearImpulseBody2, angularImpulseBody2);
+
+            // Apply the impulses to the bodies of the constraint
+            applyImpulse(impulseTwistFriction, contactManifold);
+        }
+        else {  // If it is a new contact manifold
+
+            // Initialize the accumulated impulses to zero
+            contactManifold.friction1Impulse = 0.0;
+            contactManifold.friction2Impulse = 0.0;
+            contactManifold.frictionTwistImpulse = 0.0;
+        }
+    }
+}
+
+// Solve the contact constraints by applying sequential impulses
+void ContactSolver::solveContactConstraints() {
+
+    decimal deltaLambda;
+    decimal lambdaTemp;
+    uint iter;
+
+    // For each iteration of the contact solver
+    for(iter=0; iter<mNbIterations; iter++) {
+
+        // For each contact manifold
+        for (uint c=0; c<mNbContactManifolds; c++) {
+
+            ContactManifoldSolver& contactManifold = mContactConstraints[c];
+
+            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];
+
+            for (uint i=0; i<contactManifold.nbContacts; i++) {
+
+                ContactPointSolver& contactPoint = contactManifold.contacts[i];
+
+                // --------- 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 the bias "b" of the constraint
+                decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+                decimal biasPenetrationDepth = 0.0;
+                if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+                        max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+                decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
+
+                // Compute the Lagrange multiplier lambda
+                if (mIsSplitImpulseActive) {
+                    deltaLambda = - (Jv + contactPoint.restitutionBias) *
+                                     contactPoint.inversePenetrationMass;
+                }
+                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 impulse P=J^T * lambda
+                const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
+                                                                             contactPoint);
+
+                // Apply the impulse to the bodies of the constraint
+                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 (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) {
+
+    // 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() {
+
+    // For each contact manifold
+    for (uint c=0; c<mNbContactManifolds; c++) {
+
+        ContactManifoldSolver& manifold = mContactConstraints[c];
+
+        for (uint i=0; i<manifold.nbContacts; i++) {
+
+            ContactPointSolver& contactPoint = manifold.contacts[i];
+
+            contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse);
+            contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse);
+            contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse);
+
+            contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
+            contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
+        }
+
+        manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
+        manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
+        manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
+        manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
+        manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
+    }
+}
+
+// Apply an impulse to the two bodies of a constraint
+void ContactSolver::applyImpulse(const Impulse& impulse,
+                                 const ContactManifoldSolver& manifold) {
+
+    // Update the velocities of the bodies by applying the impulse P
+    if (manifold.isBody1Moving) {
+        mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
+                                                    impulse.linearImpulseBody1;
+        mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
+                                                     impulse.angularImpulseBody1;
+    }
+    if (manifold.isBody2Moving) {
+        mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
+                                                    impulse.linearImpulseBody2;
+        mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
+                                                     impulse.angularImpulseBody2;
+    }
+}
+
+// Apply an impulse to the two bodies of a constraint
+void ContactSolver::applySplitImpulse(const Impulse& impulse,
+                                      const ContactManifoldSolver& manifold) {
+
+    // Update the velocities of the bodies by applying the impulse P
+    if (manifold.isBody1Moving) {
+        mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
+                                                    impulse.linearImpulseBody1;
+        mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
+                                                     impulse.angularImpulseBody1;
+    }
+    if (manifold.isBody2Moving) {
+        mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
+                                                              impulse.linearImpulseBody2;
+        mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
+                                                     impulse.angularImpulseBody2;
+    }
+}
+
+// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
+// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal.
+void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+                                           ContactPointSolver& contactPoint) const {
+
+    assert(contactPoint.normal.length() > 0.0);
+
+    // Compute the velocity difference vector in the tangential plane
+    Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
+    Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+
+    // If the velocty difference in the tangential plane is not zero
+    decimal lengthTangenVelocity = tangentVelocity.length();
+    if (lengthTangenVelocity > 0.0) {
+
+        // Compute the first friction vector in the direction of the tangent
+        // velocity difference
+        contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+    }
+    else {
+
+        // Get any orthogonal vector to the normal as the first friction vector
+        contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
+    }
+
+    // The second friction vector is computed by the cross product of the firs
+    // friction vector and the contact normal
+    contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
+}
+
+// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
+// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal.
+void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+                                           ContactManifoldSolver& contact) const {
+
+    assert(contact.normal.length() > 0.0);
+
+    // Compute the velocity difference vector in the tangential plane
+    Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
+    Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+
+    // If the velocty difference in the tangential plane is not zero
+    decimal lengthTangenVelocity = tangentVelocity.length();
+    if (lengthTangenVelocity > 0.0) {
+
+        // Compute the first friction vector in the direction of the tangent
+        // velocity difference
+        contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+    }
+    else {
+
+        // Get any orthogonal vector to the normal as the first friction vector
+        contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
+    }
+
+    // The second friction vector is computed by the cross product of the firs
+    // friction vector and the contact normal
+    contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
+}
+
+// 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

@@ -0,0 +1,455 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2012 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 CONTACT_SOLVER_H
+#define CONTACT_SOLVER_H
+
+// Libraries
+#include "../constraint/ContactPoint.h"
+#include "../configuration.h"
+#include "../constraint/Constraint.h"
+#include "ContactManifold.h"
+#include <map>
+#include <set>
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Declarations
+class DynamicsWorld;
+
+// Structure Impulse
+struct Impulse {
+
+    public:
+        const Vector3 linearImpulseBody1;
+        const Vector3 linearImpulseBody2;
+        const Vector3 angularImpulseBody1;
+        const Vector3 angularImpulseBody2;
+
+        // Constructor
+        Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
+                const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
+            : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
+              linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
+
+        }
+};
+
+
+    
+/*  -------------------------------------------------------------------
+    Class ContactSolver :
+        This class represents the contact solver that is used is solve rigid bodies contacts.
+        The constraint solver is based on the "Sequential Impulse" technique described by
+        Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
+
+        A constraint between two bodies is represented by a function C(x) which is equal to zero
+        when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
+        condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
+        the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
+        bodies and b is the constraint bias. We are looking for a force F_c that will act on the
+        bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
+        F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
+        Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
+        multiplier lambda.
+
+        An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
+        body to change its velocity. The idea of the Sequential Impulse technique is to apply
+        impulses to bodies of each constraints in order to keep the constraint satisfied.
+
+        --- Step 1 ---
+
+        First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
+        we obtain some new velocities v2' that tends to violate the constraints.
+
+        v2' = v1 + dt * M^-1 * F_a
+
+        where M is a matrix that contains mass and inertia tensor information.
+
+        --- Step 2 ---
+
+        During the second step, we iterate over all the constraints for a certain number of
+        iterations and for each constraint we compute the impulse to apply to the bodies needed
+        so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
+        M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
+        P_c is the constraint impulse to apply to the body. Therefore, we have
+        v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
+        using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
+        Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
+        the bodies to satisfy the constraint.
+
+        --- Step 3 ---
+
+        In the third step, we integrate the new position x2 of the bodies using the new velocities
+        v2 computed in the second step with : x2 = x1 + dt * v2.
+
+        Note that in the following code (as it is also explained in the slides from Erin Catto),
+        the value lambda is not only the lagrange multiplier but is the multiplication of the
+        Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
+        lambda, we mean (lambda * dt).
+
+        We are using the accumulated impulse technique that is also described in the slides from
+        Erin Catto.
+
+        We are also using warm starting. The idea is to warm start the solver at the beginning of
+        each step by applying the last impulstes for the constraints that we already existing at the
+        previous step. This allows the iterative solver to converge faster towards the solution.
+
+        For contact constraints, we are also using split impulses so that the position correction
+        that uses Baumgarte stabilization does not change the momentum of the bodies.
+
+        There are two ways to apply the friction constraints. Either the friction constraints are
+        applied at each contact point or they are applied only at the center of the contact manifold
+        between two bodies. If we solve the friction constraints at each contact point, we need
+        two constraints (two tangential friction directions) and if we solve the friction
+        constraints at the center of the contact manifold, we need two constraints for tangential
+        friction but also another twist friction constraint to prevent spin of the body around the
+        contact manifold center.
+        
+    -------------------------------------------------------------------
+*/
+class ContactSolver {
+
+    private:
+
+        // Structure ContactPointSolver
+        // Contact solver internal data structure that to store all the
+        // information relative to a contact point
+        struct ContactPointSolver {
+
+            decimal penetrationImpulse;      // Accumulated normal impulse
+            decimal friction1Impulse;        // Accumulated impulse in the 1st friction direction
+            decimal friction2Impulse;        // Accumulated impulse in the 2nd friction direction
+            decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction
+            Vector3 normal;                  // Normal vector of the contact
+            Vector3 frictionVector1;         // First friction vector in the tangent plane
+            Vector3 frictionVector2;         // Second friction vector in the tangent plane
+            Vector3 oldFrictionVector1;      // Old first friction vector in the tangent plane
+            Vector3 oldFrictionVector2;      // Old second friction vector in the tangent plane
+            Vector3 r1;                      // Vector from the body 1 center to the contact point
+            Vector3 r2;                      // Vector from the body 2 center to the contact point
+            Vector3 r1CrossT1;               // Cross product of r1 with 1st friction vector
+            Vector3 r1CrossT2;               // Cross product of r1 with 2nd friction vector
+            Vector3 r2CrossT1;               // Cross product of r2 with 1st friction vector
+            Vector3 r2CrossT2;               // Cross product of r2 with 2nd friction vector
+            Vector3 r1CrossN;                // Cross product of r1 with the contact normal
+            Vector3 r2CrossN;                // Cross product of r2 with the contact normal
+            decimal penetrationDepth;        // Penetration depth
+            decimal restitutionBias;         // Velocity restitution bias
+            decimal inversePenetrationMass;  // Inverse of the matrix K for the penenetration
+            decimal inverseFriction1Mass;    // Inverse of the matrix K for the 1st friction
+            decimal inverseFriction2Mass;    // Inverse of the matrix K for the 2nd friction
+            bool isRestingContact;           // True if the contact was existing last time step
+            ContactPoint* externalContact;   // Pointer to the external contact
+        };
+
+        // Structure ContactManifoldSolver
+        // Contact solver internal data structure to store all the
+        // information relative to a contact manifold
+        struct ContactManifoldSolver {
+
+            uint indexBody1;                         // Index of body 1 in the constraint solver
+            uint indexBody2;                         // Index of body 2 in the constraint solver
+            decimal massInverseBody1;                // Inverse of the mass of body 1
+            decimal massInverseBody2;                // Inverse of the mass of body 2
+            Matrix3x3 inverseInertiaTensorBody1;     // Inverse inertia tensor of body 1
+            Matrix3x3 inverseInertiaTensorBody2;     // Inverse inertia tensor of body 2
+            bool isBody1Moving;                      // True if the body 1 is allowed to move
+            bool isBody2Moving;                      // True if the body 2 is allowed to move
+            ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];// Contact point constraints
+            uint nbContacts;                         // Number of contact points
+            decimal restitutionFactor;               // Mix of the restitution factor for two bodies
+            decimal frictionCoefficient;             // Mix friction coefficient for the two bodies
+            ContactManifold* externalContactManifold; // Pointer to the external contact manifold
+
+            // Variables used when friction constraints are apply at the center of the manifold //
+
+            Vector3 normal;               // Average normal vector of the contact manifold
+            Vector3 frictionPointBody1;   // Point on body 1 where to apply the friction constraints
+            Vector3 frictionPointBody2;   // Point on body 2 where to apply the friction constraints
+            Vector3 r1Friction;           // R1 vector for the friction constraints
+            Vector3 r2Friction;           // R2 vector for the friction constraints
+            Vector3 r1CrossT1;            // Cross product of r1 with 1st friction vector
+            Vector3 r1CrossT2;            // Cross product of r1 with 2nd friction vector
+            Vector3 r2CrossT1;            // Cross product of r2 with 1st friction vector
+            Vector3 r2CrossT2;            // Cross product of r2 with 2nd friction vector
+            decimal inverseFriction1Mass; // Matrix K for the first friction constraint
+            decimal inverseFriction2Mass; // Matrix K for the second friction constraint
+            decimal inverseTwistFrictionMass;   // Matrix K for the twist friction constraint
+            Vector3 frictionVector1;      // First friction direction at contact manifold center
+            Vector3 frictionVector2;      // Second friction direction at contact manifold center
+            Vector3 oldFrictionVector1;   // Old 1st friction direction at contact manifold center
+            Vector3 oldFrictionVector2;   // Old 2nd friction direction at contact manifold center
+            decimal friction1Impulse;     // First friction direction impulse at manifold center
+            decimal friction2Impulse;     // Second friction direction impulse at manifold center
+            decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center
+        };
+
+        // -------------------- Constants --------------------- //
+
+        // Beta value for the penetration depth position correction without split impulses
+        static const decimal BETA;
+
+        // Beta value for the penetration depth position correction with split impulses
+        static const decimal BETA_SPLIT_IMPULSE;
+
+        // Slop distance (allowed penetration distance between bodies)
+        static const decimal SLOP;
+
+        // -------------------- Attributes -------------------- //
+
+        // Reference to the world
+        DynamicsWorld& mWorld;
+
+        // Number of iterations of the constraints solver
+        uint mNbIterations;
+
+        // 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
+        ContactManifoldSolver* mContactConstraints;
+
+        // Number of contact constraints
+        uint mNbContactManifolds;
+
+        // Constrained bodies
+        std::set<RigidBody*> mConstraintBodies;
+
+        // Pointer to the array of constrained linear velocities (state of the linear velocities
+        // after solving the constraints)
+        std::vector<Vector3>& mConstrainedLinearVelocities;
+
+        // Pointer to the array of constrained angular velocities (state of the angular velocities
+        // after solving the constraints)
+        std::vector<Vector3>& mConstrainedAngularVelocities;
+
+        // Reference to the map of rigid body to their index in the constrained velocities array
+        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
+
+        // True if the warm starting of the solver is active
+        bool mIsWarmStartingActive;
+
+        // True if the split impulse position correction is active
+        bool mIsSplitImpulseActive;
+
+        // True if we solve 3 friction constraints at the contact manifold center only
+        // instead of 2 friction constraints at each contact point
+        bool mIsSolveFrictionAtContactManifoldCenterActive;
+
+        // -------------------- 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);
+
+        // Apply an impulse to the two bodies of a constraint
+        void applySplitImpulse(const Impulse& impulse,
+                               const ContactManifoldSolver& manifold);
+
+        // Compute the collision restitution factor from the restitution factor of each body
+        decimal computeMixedRestitutionFactor(const RigidBody* body1,
+                                              const RigidBody* body2) const;
+
+        // Compute the mixed friction coefficient from the friction coefficient of each body
+        decimal computeMixedFrictionCoefficient(const RigidBody* body1,
+                                                const RigidBody* body2)const;
+
+        // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
+        // plane for a contact point constraint. The two vectors have to be
+        // such that : t1 x t2 = contactNormal.
+        void computeFrictionVectors(const Vector3& deltaVelocity,
+                                    ContactPointSolver &contactPoint) const;
+
+        // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
+        // plane for a contact constraint. The two vectors have to be
+        // such that : t1 x t2 = contactNormal.
+        void computeFrictionVectors(const Vector3& deltaVelocity,
+                                    ContactManifoldSolver& contactPoint) const;
+
+        // Compute a penetration constraint impulse
+        const Impulse computePenetrationImpulse(decimal deltaLambda,
+                                                const ContactPointSolver& contactPoint) const;
+
+        // Compute the first friction constraint impulse
+        const Impulse computeFriction1Impulse(decimal deltaLambda,
+                                              const ContactPointSolver& contactPoint) const;
+
+        // Compute the second friction constraint impulse
+        const Impulse computeFriction2Impulse(decimal deltaLambda,
+                                              const ContactPointSolver& contactPoint) const;
+
+   public:
+
+        // -------------------- Methods -------------------- //
+
+        // Constructor
+        ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
+                      std::vector<Vector3>& constrainedAngularVelocities,
+                      const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
+
+        // Destructor
+        virtual ~ContactSolver();
+
+        // Solve the constraints
+        void solve(decimal timeStep);
+
+        // 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();
+
+        // 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);
+
+        // 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);
+};
+
+// Return true if the body is in at least one constraint
+inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
+    return mConstraintBodies.count(body) == 1;
+}
+
+// Return the split linear velocity
+inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
+    assert(isConstrainedBody(body));
+    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
+    return mSplitLinearVelocities[indexBody];
+}
+
+// Return the split angular velocity
+inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
+    assert(isConstrainedBody(body));
+    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
+    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;
+}
+
+// Activate or deactivate the solving of friction constraints at the center of
+// the contact manifold instead of solving them at each contact point
+inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
+    mIsSolveFrictionAtContactManifoldCenterActive = isActive;
+}
+
+// Compute the collision restitution factor from the restitution factor of each body
+inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
+                                                               const RigidBody* body2) const {
+    decimal restitution1 = body1->getRestitution();
+    decimal restitution2 = body2->getRestitution();
+
+    // Return the largest restitution factor
+    return (restitution1 > restitution2) ? restitution1 : restitution2;
+}
+
+// Compute the mixed friction coefficient from the friction coefficient of each body
+inline decimal ContactSolver::computeMixedFrictionCoefficient(const RigidBody* body1,
+                                                              const RigidBody* body2) const {
+    // Use the geometric mean to compute the mixed friction coefficient
+    return sqrt(body1->getFrictionCoefficient() * body2->getFrictionCoefficient());
+}
+
+// Compute a penetration constraint impulse
+inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
+                                                          const ContactPointSolver& contactPoint)
+                                                          const {
+    return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
+                   contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
+}
+
+// Compute the first friction constraint impulse
+inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
+                                                        const ContactPointSolver& contactPoint)
+                                                        const {
+    return Impulse(-contactPoint.frictionVector1 * deltaLambda,
+                   -contactPoint.r1CrossT1 * deltaLambda,
+                   contactPoint.frictionVector1 * deltaLambda,
+                   contactPoint.r2CrossT1 * deltaLambda);
+}
+
+// Compute the second friction constraint impulse
+inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
+                                                        const ContactPointSolver& contactPoint)
+                                                        const {
+    return Impulse(-contactPoint.frictionVector2 * deltaLambda,
+                   -contactPoint.r1CrossT2 * deltaLambda,
+                   contactPoint.frictionVector2 * deltaLambda,
+                   contactPoint.r2CrossT2 * deltaLambda);
+}
+
+}
+
+#endif

src/engine/DynamicsWorld.cpp

@@ -26,13 +26,15 @@
 // Libraries
 #include "DynamicsWorld.h"
 
-// We want to use the ReactPhysics3D namespace
+// Namespaces
 using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
-              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mConstraintSolver(this),
+              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
+                mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                               mMapBodyToConstrainedVelocityIndex),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -46,11 +48,13 @@ DynamicsWorld::~DynamicsWorld() {
         (*it).second->OverlappingPair::~OverlappingPair();
         mMemoryPoolOverlappingPairs.freeObject((*it).second);
     }
+
+    // Free the allocated memory for the constrained velocities
+    cleanupConstrainedVelocitiesArray();
 }
 
 // Update the physics simulation
 void DynamicsWorld::update() {
-    bool existCollision = false;
 
     assert(mTimer.getIsRunning());
     
@@ -63,16 +67,20 @@ void DynamicsWorld::update() {
     // While the time accumulator is not empty
     while(mTimer.isPossibleToTakeStep()) {
 
-        existCollision = false;
+        // Remove all contact manifolds
-
+        mContactManifolds.clear();
-        removeAllContactConstraints();
 		
         // Compute the collision detection
-        if (mCollisionDetection.computeCollisionDetection()) {
+        mCollisionDetection.computeCollisionDetection();
-            existCollision = true;
 
-            // Solve constraints
+        // Initialize the constrained velocities
-            mConstraintSolver.solve(mTimer.getTimeStep());
+        initConstrainedVelocitiesArray();
+
+        // If there are contacts
+        if (!mContactManifolds.empty()) {
+
+            // Solve the contacts
+            mContactSolver.solve(mTimer.getTimeStep());
         }
 
         // Update the timer
@@ -82,69 +90,61 @@ void DynamicsWorld::update() {
         resetBodiesMovementVariable();
 
         // Update the position and orientation of each body
-        updateAllBodiesMotion();
+        updateRigidBodiesPositionAndOrientation();
 
-        // Cleanup of the constraint solver
+        // Cleanup of the contact solver
-        if (existCollision) {
+        mContactSolver.cleanup();
-           mConstraintSolver.cleanup();
+
-        }
+        // Cleanup the constrained velocities
+        cleanupConstrainedVelocitiesArray();
     }
 
     // Compute and set the interpolation factor to all the bodies
     setInterpolationFactorToAllBodies();
 }
 
-// Compute the motion of all bodies and update their positions and orientations
+// Update the position and orientation of the rigid bodies
-// First this method compute the vector V2 = V_constraint + V_forces + V1 where
+void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
-// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
-// V2 is the final velocity after the timestep and V1 is the velocity before the
-// timestep.
-// After having computed the velocity V2, this method will update the position
-// and orientation of each body.
-// This method uses the semi-implicit Euler method to update the position and
-// orientation of the body
-void DynamicsWorld::updateAllBodiesMotion() {
     decimal dt = mTimer.getTimeStep();
-    Vector3 newLinearVelocity;
-    Vector3 newAngularVelocity;
-    Vector3 linearVelocityErrorCorrection;
-    Vector3 angularVelocityErrorCorrection;
     
-    // For each body of thephysics world
+    // For each rigid body of the world
     for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
 
         RigidBody* rigidBody = *it;
         assert(rigidBody);
 
-        // If the body is able to move
+        // If the body is allowed to move
         if (rigidBody->getIsMotionEnabled()) {
-            newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
-            newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
-            linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
-            angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
 
-            // If it's a constrained body
+            // Update the old Transform of the body
-            if (mConstraintSolver.isConstrainedBody(*it)) {
+            rigidBody->updateOldTransform();
-                // Get the constrained linear and angular velocities from the constraint solver
+
-                newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
+            // Get the constrained velocity
-                newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
+            uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
-                
+            Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
-                linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
+            Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
-                angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
+
+            // Update the linear and angular velocity of the body
+            rigidBody->setLinearVelocity(newLinVelocity);
+            rigidBody->setAngularVelocity(newAngVelocity);
+
+            // Add the split impulse velocity from Contact Solver (only used to update the position)
+            if (mContactSolver.isConstrainedBody(rigidBody)) {
+                newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
+                newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
             }
 
-            // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
+            // Get current position and orientation of the body
-            // external forces and torques.
+            const Vector3& currentPosition = rigidBody->getTransform().getPosition();
-            newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
+            const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
-            newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
-            // Add the velocity V1 to the new velocity
+            // Compute the new position of the body
-            newLinearVelocity += rigidBody->getLinearVelocity();
+            Vector3 newPosition = currentPosition + newLinVelocity * dt;
-            newAngularVelocity += rigidBody->getAngularVelocity();
+            Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
-            
+
-            // Update the position and the orientation of the body according to the new velocity
+            // Update the Transform of the body
-            updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
+            Transform newTransform(newPosition, newOrientation.getUnit());
-                                               linearVelocityErrorCorrection, angularVelocityErrorCorrection);
+            rigidBody->setTransform(newTransform);
 
             // Update the AABB of the rigid body
             rigidBody->updateAABB();
@@ -152,37 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
     }
 }
 
-// Update the position and orientation of a body
-// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
-// orientation of the body
-void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
-                                                       const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
-    decimal dt = mTimer.getTimeStep();
-
-    assert(rigidBody);
-
-    // Update the old position and orientation of the body
-    rigidBody->updateOldTransform();
-
-    // Update the linear and angular velocity of the body
-    rigidBody->setLinearVelocity(newLinVelocity);
-    rigidBody->setAngularVelocity(newAngVelocity);
-    
-    // Get current position and orientation of the body
-    const Vector3& currentPosition = rigidBody->getTransform().getPosition();
-    const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
-        
-    // Error correction projection
-    Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
-    Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
-    
-    Vector3 newPosition = correctedPosition + newLinVelocity * dt;
-    Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
-
-    Transform newTransform(newPosition, newOrientation.getUnit());
-    rigidBody->setTransform(newTransform);
-}
-
 // Compute and set the interpolation factor to all bodies
 void DynamicsWorld::setInterpolationFactorToAllBodies() {
     
@@ -200,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
     }
 }
 
+// Initialize the constrained velocities array at each step
+void DynamicsWorld::initConstrainedVelocitiesArray() {
+
+    // TODO : Use better memory allocation here
+    mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+    mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+
+    double dt = mTimer.getTimeStep();
+
+    // Fill in the mapping of rigid body to their index in the constrained
+    // velocities arrays
+    uint i = 0;
+    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        RigidBody* rigidBody = *it;
+        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
+
+        // TODO : Move it somewhere else
+        mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
+        mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
+
+        i++;
+    }
+}
+
+// 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();
+}
+
 // Apply the gravity force to all bodies of the physics world
 void DynamicsWorld::applyGravity() {
 
@@ -262,26 +266,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     mMemoryPoolRigidBodies.freeObject(rigidBody);
 }
 
-// Remove all collision contacts constraints
-// TODO : This method should be in the collision detection class
-void DynamicsWorld::removeAllContactConstraints() {
-    // For all constraints
-    for (vector<Constraint*>::iterator it = mConstraints.begin(); it != mConstraints.end(); ) {
-
-        // Try a downcasting
-        Contact* contact = dynamic_cast<Contact*>(*it);
-
-        // If the constraint is a contact
-        if (contact) {
-            // Remove it from the constraints of the physics world
-            it = mConstraints.erase(it);
-        }
-        else {
-            ++it;
-        }
-    }
-}
-
 // Remove all constraints in the physics world
 void DynamicsWorld::removeAllConstraints() {
     mConstraints.clear();
@@ -321,7 +305,7 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
     assert(rigidBody2);
 
     // Create a new contact
-    Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo);
+    ContactPoint* contact = new (mMemoryPoolContacts.allocateObject()) ContactPoint(rigidBody1, rigidBody2, contactInfo);
     assert(contact);
 
     // Get the corresponding overlapping pair
@@ -332,9 +316,20 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
     // Add the contact to the contact cache of the corresponding overlapping pair
     overlappingPair->addContact(contact);
 
+    // TODO : Remove this
+    /*
+    // Create a contact manifold with the contact points of the two bodies
+    ContactManifold contactManifold;
+    contactManifold.nbContacts = 0;
+
     // Add all the contacts in the contact cache of the two bodies
     // to the set of constraints in the physics world
     for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
-        addConstraint(overlappingPair->getContact(i));
+        contactManifold.contacts[i] = overlappingPair->getContact(i);
+        contactManifold.nbContacts++;
     }
+    */
+
+    // Add the contact manifold to the world
+    mContactManifolds.push_back(overlappingPair->getContactManifold());
 }

src/engine/DynamicsWorld.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "CollisionWorld.h"
 #include "../collision/CollisionDetection.h"
-#include "ConstraintSolver.h"
+#include "ContactSolver.h"
 #include "../body/RigidBody.h"
 #include "Timer.h"
 #include "../configuration.h"
@@ -53,8 +53,8 @@ class DynamicsWorld : public CollisionWorld {
         // Timer of the physics engine
         Timer mTimer;
 
-        // Constraint solver
+        // Contact solver
-        ConstraintSolver mConstraintSolver;
+        ContactSolver mContactSolver;
 
         // True if the deactivation (sleeping) of inactive bodies is enabled
         bool mIsDeactivationActive;
@@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld {
         // All the rigid bodies of the physics world
         std::set<RigidBody*> mRigidBodies;
 
-        // List that contains all the current constraints
+        // All the contact constraints
+        std::vector<ContactManifold*> mContactManifolds;
+
+        // All the constraints (except contact constraints)
         std::vector<Constraint*> mConstraints;
 
         // Gravity vector of the world
@@ -78,7 +81,18 @@ class DynamicsWorld : public CollisionWorld {
         MemoryPool<RigidBody> mMemoryPoolRigidBodies;
 
         // Memory pool for the contacts
-        MemoryPool<Contact> mMemoryPoolContacts;
+        MemoryPool<ContactPoint> mMemoryPoolContacts;
+
+        // Array of constrained linear velocities (state of the linear velocities
+        // after solving the constraints)
+        std::vector<Vector3> mConstrainedLinearVelocities;
+
+        // Array of constrained angular velocities (state of the angular velocities
+        // after solving the constraints)
+        std::vector<Vector3> mConstrainedAngularVelocities;
+
+        // Map body to their index in the constrained velocities array
+        std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
 
         // -------------------- Methods -------------------- //
 
@@ -89,17 +103,21 @@ class DynamicsWorld : public CollisionWorld {
         DynamicsWorld& operator=(const DynamicsWorld& world);
 
         // Compute the motion of all bodies and update their positions and orientations
-        void updateAllBodiesMotion();
+        void updateRigidBodiesPositionAndOrientation();
 
         // Update the position and orientation of a body
-        void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
+        void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
-                                                const Vector3& newAngVelocity,
+                                                Vector3 newAngVelocity);
-                                                const Vector3& linearVelocityErrorCorrection,
-                                                const Vector3& angularVelocityErrorCorrection);
 
         // Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
+        // Initialize the constrained velocities array at each step
+        void initConstrainedVelocitiesArray();
+
+        // Cleanup the constrained velocities array at each step
+        void cleanupConstrainedVelocitiesArray();
+
         // Apply the gravity force to all bodies
         void applyGravity();
 
@@ -137,8 +155,15 @@ public :
         // Update the physics simulation
         void update();
 
-        // Set the number of iterations of the LCP solver
+        // Set the number of iterations of the constraint solver
-        void setNbLCPIterations(uint nbIterations);
+        void setNbIterationsSolver(uint nbIterations);
+
+        // Activate or Deactivate the split impulses for contacts
+        void setIsSplitImpulseActive(bool isActive);
+
+        // 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);
 
         // Set the isErrorCorrectionActive value
         void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
@@ -160,24 +185,33 @@ public :
         // Set the isGravityOn attribute
         void setIsGratityOn(bool isGravityOn);
 
+        // Return the number of rigid bodies in the world
+        uint getNbRigidBodies() const;
+
         // Add a constraint
         void addConstraint(Constraint* constraint);
 
         // Remove a constraint
         void removeConstraint(Constraint* constraint);
 
-        // Remove all collision contacts constraints
-        void removeAllContactConstraints();
-
         // Remove all constraints and delete them (free their memory)
         void removeAllConstraints();
 
+        // Return the number of contact constraints in the world
+        uint getNbContactManifolds() const;
+
         // Return a start iterator on the constraint list
         std::vector<Constraint*>::iterator getConstraintsBeginIterator();
 
         // Return a end iterator on the constraint list
         std::vector<Constraint*>::iterator getConstraintsEndIterator();
 
+        // Return a start iterator on the contact manifolds list
+        std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
+
+        // Return a end iterator on the contact manifolds list
+        std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
+
         // Return an iterator to the beginning of the rigid bodies of the physics world
         std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
 
@@ -197,14 +231,20 @@ inline void DynamicsWorld::stop() {
     mTimer.stop();
 }                
 
-// Set the number of iterations of the LCP solver
+// Set the number of iterations of the constraint solver
-inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) {
+inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
-    mConstraintSolver.setNbLCPIterations(nbIterations);
+    mContactSolver.setNbIterationsSolver(nbIterations);
-}   
+}
 
-// Set the isErrorCorrectionActive value
+// Activate or Deactivate the split impulses for contacts
-inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
+inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
-    mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
+    mContactSolver.setIsSplitImpulseActive(isActive);
+}
+
+// Activate or deactivate the solving of friction constraints at the center of
+// the contact manifold instead of solving them at each contact point
+inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
+    mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
 }
 
 // Reset the boolean movement variable of each body
@@ -265,6 +305,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
     mIsGravityOn = isGravityOn;
 }
 
+// Return the number of rigid bodies in the world
+inline uint DynamicsWorld::getNbRigidBodies() const {
+    return mRigidBodies.size();
+}
+
 // Return an iterator to the beginning of the bodies of the physics world
 inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
     return mRigidBodies.begin();
@@ -275,6 +320,11 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
     return mRigidBodies.end();
 }
 
+// Return the number of contact manifolds in the world
+inline uint DynamicsWorld::getNbContactManifolds() const {
+    return mContactManifolds.size();
+}
+
 // Return a start iterator on the constraint list
 inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
     return mConstraints.begin();
@@ -285,6 +335,16 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
     return mConstraints.end();
 }
 
+// Return a start iterator on the contact manifolds list
+inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
+    return mContactManifolds.begin();
+}
+
+// Return a end iterator on the contact manifolds list
+inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
+    return mContactManifolds.end();
+}
+
 }
 
 #endif

src/engine/OverlappingPair.cpp

@@ -31,11 +31,11 @@ using namespace reactphysics3d;
 
 // Constructor
 OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
-                                 MemoryPool<Contact>& memoryPoolContacts)
+                                 MemoryPool<ContactPoint>& memoryPoolContacts)
-                : mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts),
+                : mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts),
                   mCachedSeparatingAxis(1.0, 1.0, 1.0) {
     
-}   
+}
 
 // Destructor
 OverlappingPair::~OverlappingPair() {

src/engine/OverlappingPair.h

@@ -27,7 +27,7 @@
 #define	OVERLAPPING_PAIR_H
 
 // Libraries
-#include "PersistentContactCache.h"
+#include "ContactManifold.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -53,8 +53,8 @@ class OverlappingPair {
         // Pointer to the second body of the contact
         CollisionBody* const mBody2;
 
-        // Persistent contact cache
+        // Persistent contact manifold
-        PersistentContactCache mContactsCache;
+        ContactManifold mContactManifold;
 
         // Cached previous separating axis
         Vector3 mCachedSeparatingAxis;
@@ -73,7 +73,7 @@ class OverlappingPair {
 
         // Constructor
         OverlappingPair(CollisionBody* body1, CollisionBody* body2,
-                        MemoryPool<Contact>& memoryPoolContacts);
+                        MemoryPool<ContactPoint>& memoryPoolContacts);
 
         // Destructor
         ~OverlappingPair();
@@ -85,7 +85,7 @@ class OverlappingPair {
         CollisionBody* const getBody2() const;
 
         // Add a contact to the contact cache
-        void addContact(Contact* contact);
+        void addContact(ContactPoint* contact);
 
         // Update the contact cache
         void update();
@@ -99,8 +99,12 @@ class OverlappingPair {
         // Return the number of contacts in the cache
         uint getNbContacts() const;
 
+        // Return the contact manifold
+        ContactManifold* getContactManifold();
+
         // Return a contact of the cache
-        Contact* getContact(uint index) const;
+        // TODO : Maybe remove this method
+        ContactPoint* getContact(uint index) const;
 };
 
 // Return the pointer to first body
@@ -113,14 +117,14 @@ inline CollisionBody* const OverlappingPair::getBody2() const {
     return mBody2;
 }                
 
-// Add a contact to the contact cache
+// Add a contact to the contact manifold
-inline void OverlappingPair::addContact(Contact* contact) {
+inline void OverlappingPair::addContact(ContactPoint* contact) {
-    mContactsCache.addContact(contact);
+    mContactManifold.addContactPoint(contact);
 }  
 
-// Update the contact cache
+// Update the contact manifold
 inline void OverlappingPair::update() {
-    mContactsCache.update(mBody1->getTransform(), mBody2->getTransform());
+    mContactManifold.update(mBody1->getTransform(), mBody2->getTransform());
 }                                
 
 // Return the cached separating axis
@@ -136,12 +140,17 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
 
 // Return the number of contacts in the cache
 inline uint OverlappingPair::getNbContacts() const {
-    return mContactsCache.getNbContacts();
+    return mContactManifold.getNbContactPoints();
+}
+
+// Return the contact manifold
+inline ContactManifold* OverlappingPair::getContactManifold() {
+    return &mContactManifold;
 }
 
 // Return a contact of the cache    
-inline Contact* OverlappingPair::getContact(uint index) const {
+inline ContactPoint* OverlappingPair::getContact(uint index) const {
-    return mContactsCache.getContact(index);
+    return mContactManifold.getContactPoint(index);
 }         
 
 } // End of the ReactPhysics3D namespace

src/engine/PersistentContactCache.cpp

@@ -1,227 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 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 "PersistentContactCache.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
-                       : mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) {
-    
-}
-
-// Destructor
-PersistentContactCache::~PersistentContactCache() {
-    clear();
-}
-
-// Add a contact in the cache
-void PersistentContactCache::addContact(Contact* contact) {
-
-    int indexNewContact = mNbContacts;
-	
-	// For contact already in the cache
-	for (uint i=0; i<mNbContacts; i++) {
-		// Check if the new point point does not correspond to a same contact point
-		// already in the cache. If it's the case, we do not add the new contact
-		if (isApproxEqual(contact->getLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) {
-			// Delete the new contact
-			contact->Contact::~Contact();
-			mMemoryPoolContacts.freeObject(contact);
-			
-			return;
-		}
-	}
-    
-    // If the contact cache is full
-    if (mNbContacts == MAX_CONTACTS_IN_CACHE) {
-        int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
-        int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
-        removeContact(indexToRemove);
-        indexNewContact = indexToRemove;
-    }
-
-    // Add the new contact in the cache
-    mContacts[indexNewContact] = contact;
-    mNbContacts++;
-}
-
-// Remove a contact from the cache
-void PersistentContactCache::removeContact(int index) {
-    assert(index >= 0 && index < mNbContacts);
-    assert(mNbContacts > 0);
-	
-	// Call the destructor explicitly and tell the memory pool that
-	// the corresponding memory block is now free
-	mContacts[index]->Contact::~Contact();
-	mMemoryPoolContacts.freeObject(mContacts[index]);
-	
-    // If we don't remove the last index
-    if (index < mNbContacts - 1) {
-        mContacts[index] = mContacts[mNbContacts - 1];
-    }
-
-    mNbContacts--;
-}
-
-// Update the contact cache
-// First the world space coordinates of the current contacts in the cache are recomputed from
-// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
-// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
-// the contacts with a too large distance between the contact points in the plane orthogonal to the
-// contact normal
-void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) {
-    if (mNbContacts == 0) return;
-
-    // Update the world coordinates and penetration depth of the contacts in the cache
-    for (int i=0; i<mNbContacts; i++) {
-        mContacts[i]->setWorldPointOnBody1(transform1 * mContacts[i]->getLocalPointOnBody1());
-        mContacts[i]->setWorldPointOnBody2(transform2 * mContacts[i]->getLocalPointOnBody2());
-        mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal()));
-    }
-
-    // Remove the contacts that don't represent very well the persistent contact
-    for (int i=mNbContacts-1; i>=0; i--) {
-        assert(i>= 0 && i < mNbContacts);
-        
-        // Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
-        if (mContacts[i]->getPenetrationDepth() <= 0.0) {
-            removeContact(i);
-        }
-        else {
-            // Compute the distance of the two contact points in the place orthogonal to the contact normal
-            Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth();
-            Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1;
-
-            // If the orthogonal distance is larger than the valid distance threshold, we remove the contact
-            if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) {
-                removeContact(i);
-            }
-        }
-    }    
-}
-
-// Return the index of the contact with the larger penetration depth. This
-// corresponding contact will be kept in the cache. The method returns -1 is
-// the new contact is the deepest.
-int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
-    assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
-    int indexMaxPenetrationDepth = -1;
-    decimal maxPenetrationDepth = newContact->getPenetrationDepth();
-
-    // For each contact in the cache
-    for (uint i=0; i<mNbContacts; i++) {
-        // If the current contact has a larger penetration depth
-        if (mContacts[i]->getPenetrationDepth() > maxPenetrationDepth) {
-            maxPenetrationDepth = mContacts[i]->getPenetrationDepth();
-            indexMaxPenetrationDepth = i;
-        }
-    }
-
-    // Return the index of largest penetration depth
-    return indexMaxPenetrationDepth;
-}
-
-// Return the index that will be removed. The index of the contact with the larger penetration
-// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
-// the different area and we want to keep the contacts with the largest area. The new point is also
-// kept.
-int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
-    assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
-    decimal area0 = 0.0;       // Area with contact 1,2,3 and newPoint
-    decimal area1 = 0.0;       // Area with contact 0,2,3 and newPoint
-    decimal area2 = 0.0;       // Area with contact 0,1,3 and newPoint
-    decimal area3 = 0.0;       // Area with contact 0,1,2 and newPoint
-
-    if (indexMaxPenetration != 0) {
-        // Compute the area
-        Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1();
-        Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1();
-        Vector3 crossProduct = vector1.cross(vector2);
-        area0 = crossProduct.lengthSquare();
-    }
-    if (indexMaxPenetration != 1) {
-        // Compute the area
-        Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1();
-        Vector3 crossProduct = vector1.cross(vector2);
-        area1 = crossProduct.lengthSquare();
-    }
-    if (indexMaxPenetration != 2) {
-        // Compute the area
-        Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1();
-        Vector3 crossProduct = vector1.cross(vector2);
-        area2 = crossProduct.lengthSquare();
-    }
-    if (indexMaxPenetration != 3) {
-        // Compute the area
-        Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContacts[2]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1();
-        Vector3 crossProduct = vector1.cross(vector2);
-        area3 = crossProduct.lengthSquare();
-    }
-    
-    // Return the index of the contact to remove
-    return getMaxArea(area0, area1, area2, area3);
-}
-
-// Return the index of maximum area
-int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
-    if (area0 < area1) {
-        if (area1 < area2) {
-            if (area2 < area3) return 3;
-            else return 2;
-        }
-        else {
-            if (area1 < area3) return 3;
-            else return 1;
-        }
-    }
-    else {
-        if (area0 < area2) {
-            if (area2 < area3) return 3;
-            else return 2;
-        }
-        else {
-            if (area0 < area3) return 3;
-            else return 0;
-        }
-    }
-}
-
-// Clear the cache
-void PersistentContactCache::clear() {
-    for (uint i=0; i<mNbContacts; i++) {
-		
-		// Call the destructor explicitly and tell the memory pool that
-		// the corresponding memory block is now free
-		mContacts[i]->Contact::~Contact();
-		mMemoryPoolContacts.freeObject(mContacts[i]);
-    }
-    mNbContacts = 0;
-}

src/engine/PersistentContactCache.h

@@ -1,147 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 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 PERSISTENT_CONTACT_CACHE_H
-#define	PERSISTENT_CONTACT_CACHE_H
-
-// Libraries
-#include <vector>
-#include "../body/Body.h"
-#include "../constraint/Contact.h"
-
-// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Constants
-const uint MAX_CONTACTS_IN_CACHE = 4;   // Maximum number of contacts in the persistent cache
-
-
-/*  -------------------------------------------------------------------
-    Class PersistentContactCache :
-        This class represents a cache of at most 4 contact points between
-        two given bodies. The contacts between two bodies are added one
-        after the other in the cache. When the cache is full, we have
-        to remove one point. The idea is to keep the point with the deepest
-        penetration depth and also to keep the points producing the larger
-        area (for a more stable contact manifold). The new added point is
-        always kept. This kind of persistent cache has been explained for
-        instance in the presentation from Erin Catto about contact manifolds
-        at the GDC 2007 conference.
-    -------------------------------------------------------------------
-*/
-class PersistentContactCache {
-
-    private:
-
-        // -------------------- Attributes -------------------- //
-
-        // Pointer to the first body
-        Body* const mBody1;
-
-        // Pointer to the second body
-        Body* const mBody2;
-
-        // Contacts in the cache
-        Contact* mContacts[MAX_CONTACTS_IN_CACHE];
-
-        // Number of contacts in the cache
-        uint mNbContacts;
-
-        // Reference to the memory pool with the contacts
-        MemoryPool<Contact>& mMemoryPoolContacts;
-
-        // -------------------- Methods -------------------- //
-
-        // Private copy-constructor
-        PersistentContactCache(const PersistentContactCache& persistentContactCache);
-
-        // Private assignment operator
-        PersistentContactCache& operator=(const PersistentContactCache& persistentContactCache);
-
-        // Return the index of maximum area
-        int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
-
-        // Return the index of the contact with the larger penetration depth
-        int getIndexOfDeepestPenetration(Contact* newContact) const;
-
-        // Return the index that will be removed
-        int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
-
-        // Remove a contact from the cache
-        void removeContact(int index);
-
-        // Return true if two vectors are approximatively equal
-        bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
-        
-    public:
-
-        // -------------------- Methods -------------------- //
-
-        // Constructor
-        PersistentContactCache(Body* const mBody1, Body* const mBody2,
-                               MemoryPool<Contact>& mMemoryPoolContacts);
-
-        // Destructor
-        ~PersistentContactCache();
-
-        // Add a contact
-        void addContact(Contact* contact);
-
-        // Update the contact cache
-        void update(const Transform& transform1, const Transform& transform2);
-
-        // Clear the cache
-        void clear();
-
-        // Return the number of contacts in the cache
-        uint getNbContacts() const;
-
-        // Return a contact of the cache
-        Contact* getContact(uint index) const;
-};
-
-// Return the number of contacts in the cache
-inline uint PersistentContactCache::getNbContacts() const {
-    return mNbContacts;
-} 
-
-// Return a contact of the cache
-inline Contact* PersistentContactCache::getContact(uint index) const {
-    assert(index >= 0 && index < mNbContacts);
-    return mContacts[index];
-}  
-
-// Return true if two vectors are approximatively equal
-inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1,
-                                                  const Vector3& vector2) const {
-    const decimal epsilon = 0.1;
-    return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
-            approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
-            approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
-}  
-
-}
-#endif
-

src/engine/Timer.h

@@ -153,7 +153,7 @@ inline void Timer::start() {
         LARGE_INTEGER ticks;
         QueryPerformanceFrequency(&ticksPerSecond);
         QueryPerformanceCounter(&ticks);
-        lastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
+        mLastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
 #else
         // Initialize the lastUpdateTime with the current time in seconds
         timeval timeValue;

src/mathematics/Transform.h

@@ -176,7 +176,7 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform,
                                                   const Transform& newTransform,
                                                   decimal interpolationFactor) {
 
-    Vector3 interPosition = oldTransform.mPosition * (1.0 - interpolationFactor) +
+    Vector3 interPosition = oldTransform.mPosition * (decimal(1.0) - interpolationFactor) +
                             newTransform.mPosition * interpolationFactor;
 
     Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation,

src/mathematics/Vector3.cpp

@@ -26,7 +26,6 @@
 // Libraries
 #include "Vector3.h"
 #include <iostream>
-#include <cassert>
 #include <vector>
 
 // Namespaces
@@ -97,3 +96,27 @@ Vector3 Vector3::getOneOrthogonalVector() const {
     //assert(vector1.isUnit());
     return vector1;
 }
+
+// Return one unit orthogonal vector of the current vector
+Vector3 Vector3::getOneUnitOrthogonalVector() const {
+    assert(!this->isZero());
+
+    decimal x = mValues[0];
+    decimal y = mValues[1];
+    decimal z = mValues[2];
+
+    // Get the minimum element of the vector
+    Vector3 vectorAbs(fabs(x), fabs(y), fabs(z));
+    int minElement = vectorAbs.getMinAxis();
+
+    if (minElement == 0) {
+        return Vector3(0.0, -z, y) / sqrt(y*y + z*z);
+    }
+    else if (minElement == 1) {
+        return Vector3(-z, 0.0, x) / sqrt(x*x + z*z);
+    }
+    else {
+        return Vector3(-y, x, 0.0) / sqrt(x*x + y*y);
+    }
+
+}

src/mathematics/Vector3.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include <cmath>
+#include <cassert>
 #include "mathematics_functions.h"
 #include "../decimal.h"
 
@@ -95,6 +96,9 @@ class Vector3 {
         // Return the corresponding unit vector
         Vector3 getUnit() const;
 
+        // Return one unit orthogonal vector of the current vector
+        Vector3 getOneUnitOrthogonalVector() const;
+
         // Return true if the vector is unit and false otherwise
         bool isUnit() const;
 
@@ -110,6 +114,9 @@ class Vector3 {
         // Cross product of two vectors
         Vector3 cross(const Vector3& vector) const;
 
+        // Normalize the vector
+        void normalize();
+
         // Return the corresponding absolute value vector
         Vector3 getAbsoluteVector() const;
 
@@ -137,6 +144,9 @@ class Vector3 {
         // Overloaded operator for multiplication with a number with assignment
         Vector3& operator*=(decimal number);
 
+        // Overloaded operator for division by a number with assignment
+        Vector3& operator/=(decimal number);
+
         // Overloaded operator for value access
         decimal& operator[] (int index);
 
@@ -153,6 +163,7 @@ class Vector3 {
         friend Vector3 operator-(const Vector3& vector);
         friend Vector3 operator*(const Vector3& vector, decimal number);
         friend Vector3 operator*(decimal number, const Vector3& vector);
+        friend Vector3 operator/(const Vector3& vector, decimal number);
 };
 
 // Get the x component of the vector
@@ -217,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const {
                    mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]);
 }
 
+// Normalize the vector
+inline void Vector3::normalize() {
+    decimal l = length();
+    assert(l != 0.0);
+    mValues[0] /= l;
+    mValues[1] /= l;
+    mValues[2] /= l;
+}
+
 // Return the corresponding absolute value vector
 inline Vector3 Vector3::getAbsoluteVector() const {
     return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2]));
@@ -283,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) {
     return *this;
 }
 
+// Overloaded operator for division by a number with assignment
+inline Vector3& Vector3::operator/=(decimal number) {
+    mValues[0] /= number;
+    mValues[1] /= number;
+    mValues[2] /= number;
+    return *this;
+}
+
 // Overloaded operator for value access
 inline decimal& Vector3::operator[] (int index) {
     return mValues[index];
@@ -313,6 +341,11 @@ inline Vector3 operator*(const Vector3& vector, decimal number) {
     return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]);
 }
 
+// Overloaded operator for division by a number
+inline Vector3 operator/(const Vector3& vector, decimal number) {
+    return Vector3(vector.mValues[0] / number, vector.mValues[1] / number, vector.mValues[2] / number);
+}
+
 // Overloaded operator for multiplication with a number
 inline Vector3 operator*(decimal number, const Vector3& vector) {
     return vector * number;