diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 6e0e6ed6..f650351a 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() {
         OverlappingPair* pair = it->second;
 
         // Make all the contact manifolds and contact points of the pair obsolete
-        pair->makeContactsObselete();
+        pair->makeContactsObsolete();
 
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
@@ -410,12 +410,12 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
 		potentialManifold = potentialManifold->mNext;
 	}
 
-    // Reset the isNew status of the manifolds
-    pair->resetIsNewManifoldStatus();
-
     // Clear the obsolete contact manifolds and contact points
     pair->clearObsoleteManifoldsAndContactPoints();
 
+    // Reduce the contact manifolds and contact points if there are too many of them
+    pair->reduceContactManifolds();
+
     // Reset the potential contacts of the pair
     pair->clearPotentialContactManifolds();
 }
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 37ad3ad8..55ce8ba3 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -34,20 +34,14 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
                 : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
                   mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
                   mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
-                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) {
+                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
     
     // For each contact point info in the manifold
     const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
     while(pointInfo != nullptr) {
 
-        // Create the new contact point
-        ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
-                ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
-
-        // Add the new contact point into the manifold
-        contact->setNext(mContactPoints);
-        mContactPoints = contact;
-        mNbContactPoints++;
+        // Add the new contact point
+        addContactPoint(pointInfo);
 
         pointInfo = pointInfo->next;
     }
@@ -73,52 +67,122 @@ ContactManifold::~ContactManifold() {
     }
 }
 
+// Remove a contact point
+void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
+
+    assert(mNbContactPoints > 0);
+    assert(mContactPoints != nullptr);
+    assert(contactPoint != nullptr);
+
+    ContactPoint* previous = contactPoint->getPrevious();
+    ContactPoint* next = contactPoint->getNext();
+
+    if (previous != nullptr) {
+        previous->setNext(next);
+    }
+    else {
+        mContactPoints = next;
+    }
+
+    if (next != nullptr) {
+        next->setPrevious(previous);
+    }
+
+    // Delete the contact point
+    contactPoint->~ContactPoint();
+    mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+    mNbContactPoints--;
+    assert(mNbContactPoints >= 0);
+}
+
 // Add a contact point
 void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
 
+    assert(contactPointInfo != nullptr);
+
     // Create the new contact point
-    ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
-            ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+    ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo);
 
     // Add the new contact point into the manifold
     contactPoint->setNext(mContactPoints);
+    contactPoint->setPrevious(nullptr);
+    if (mContactPoints != nullptr) {
+        mContactPoints->setPrevious(contactPoint);
+    }
     mContactPoints = contactPoint;
 
     mNbContactPoints++;
-    assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
 }
 
 // Clear the obsolete contact points
-void ContactManifold::clearObseleteContactPoints() {
+void ContactManifold::clearObsoleteContactPoints() {
 
+    assert(mContactPoints != nullptr);
+
+    // For each contact point of the manifold
     ContactPoint* contactPoint = mContactPoints;
-    ContactPoint* previousContactPoint = nullptr;
     while (contactPoint != nullptr) {
 
         ContactPoint* nextContactPoint =  contactPoint->getNext();
 
+        // If the contact point is obsolete
         if (contactPoint->getIsObsolete()) {
 
-            // Delete the contact point
-            contactPoint->~ContactPoint();
-            mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
-
-            if (previousContactPoint != nullptr) {
-                previousContactPoint->setNext(nextContactPoint);
-            }
-            else {
-                mContactPoints = nextContactPoint;
-            }
-
-            mNbContactPoints--;
-        }
-        else {
-            previousContactPoint = contactPoint;
+            // Remove the contact point
+            removeContactPoint(contactPoint);
         }
 
         contactPoint = nextContactPoint;
     }
 
-    assert(mNbContactPoints >= 0);
-    assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+    assert(mNbContactPoints > 0);
+    assert(mContactPoints != nullptr);
+}
+
+// Make sure we do not have too much contact points by keeping only the best
+// contact points of the manifold (with largest penetration depth)
+void ContactManifold::reduce() {
+
+    assert(mContactPoints != nullptr);
+
+    // Remove contact points while there is too much contact points
+    while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+        removeNonOptimalContactPoint();
+    }
+
+    assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0);
+    assert(mContactPoints != nullptr);
+}
+
+// Remove a contact point that is not optimal (with a small penetration depth)
+void ContactManifold::removeNonOptimalContactPoint() {
+
+    assert(mContactPoints != nullptr);
+    assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+    // Get the contact point with the minimum penetration depth among all points
+    ContactPoint* contactPoint = mContactPoints;
+    ContactPoint* minContactPoint = nullptr;
+    decimal minPenetrationDepth = DECIMAL_LARGEST;
+    while (contactPoint != nullptr) {
+
+        ContactPoint* nextContactPoint =  contactPoint->getNext();
+
+        if (contactPoint->getPenetrationDepth() < minPenetrationDepth) {
+
+            minContactPoint = contactPoint;
+            minPenetrationDepth = contactPoint->getPenetrationDepth();
+        }
+
+        contactPoint = nextContactPoint;
+    }
+
+    assert(minContactPoint != nullptr);
+
+    // Remove the non optimal contact point
+    removeContactPoint(minContactPoint);
+
+    assert(mNbContactPoints > 0);
+    assert(mContactPoints != nullptr);
 }
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index b24ed378..b5c25cb9 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -143,9 +143,6 @@ class ContactManifold {
         /// True if the contact manifold is obsolete
         bool mIsObsolete;
 
-        /// True if the contact manifold is new (has just been added from potential contacts)
-        bool mIsNew;
-
         // -------------------- Methods -------------------- //
 
         /// Return true if the contact manifold has already been added into an island
@@ -161,7 +158,7 @@ class ContactManifold {
         void setIsObsolete(bool isObselete, bool setContactPoints);
 
         /// Clear the obsolete contact points
-        void clearObseleteContactPoints();
+        void clearObsoleteContactPoints();
 
         /// Return the contact normal direction Id of the manifold
         short getContactNormalId() const;
@@ -184,6 +181,15 @@ class ContactManifold {
         /// Add a contact point
         void addContactPoint(const ContactPointInfo* contactPointInfo);
 
+        /// Make sure we do not have too much contact points by keeping only the best ones
+        void reduce();
+
+        /// Remove a contact point that is not optimal (with a small penetration depth)
+        void removeNonOptimalContactPoint();
+
+        /// Remove a contact point
+        void removeContactPoint(ContactPoint* contactPoint);
+
         /// Set the friction twist accumulated impulse
         void setFrictionTwistImpulse(decimal frictionTwistImpulse);
 
@@ -208,12 +214,6 @@ class ContactManifold {
         /// Return the friction twist accumulated impulse
         decimal getFrictionTwistImpulse() const;
 
-        /// Return true if the manifold has just been created
-        bool getIsNew() const;
-
-        /// Set the isNew attribute
-        void setIsNew(bool isNew);
-        
     public:
 
         // -------------------- Methods -------------------- //
@@ -417,18 +417,6 @@ inline short ContactManifold::getContactNormalId() const {
     return mContactNormalId;
 }
 
-
-// Return true if the manifold has just been created
-inline bool ContactManifold::getIsNew() const {
-    return mIsNew;
-}
-
-// Set the isNew attribute
-inline void ContactManifold::setIsNew(bool isNew) {
-    mIsNew = isNew;
-}
-
-
 }
 #endif
 
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 06ae4963..9b8bf58b 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -60,43 +60,16 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
     }
     else {
 
-        bool addNewManifold = true;
-
-        // If there are too much contact manifolds in the set
-        if (mNbManifolds >= mNbMaxManifolds) {
-
-            // We need to remove a manifold from the set.
-            // We search for an old manifold with the smallest maximum penetration depth among its contact points.
-            // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth.
-            ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth());
-
-            // If the manifold with the minimum penetration depth is an existing one
-            if (minDepthManifold != nullptr) {
-
-                // Remove the manifold
-                removeManifold(minDepthManifold);
-            }
-            else {
-
-                // The manifold we do not want to get is the new one. Therefore, we do not add it to the set
-                addNewManifold = false;
-            }
-        }
-
-        // If we need to add the new contact manifold
-        if (addNewManifold) {
-
-            // Create a new contact manifold
-            createManifold(contactManifoldInfo);
-        }
+        // Create a new contact manifold
+        createManifold(contactManifoldInfo);
     }
 }
 
 // Update a previous similar manifold with a new one
 void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
 
-    assert(oldManifold != nullptr);
-    assert(newManifold != nullptr);
+   assert(oldManifold != nullptr);
+   assert(newManifold != nullptr);
 
    // For each contact point of the new manifold
    ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
@@ -114,7 +87,7 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
             if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
 
                 // Replace (update) the old contact point with the new one
-                oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(),  mShape2->getLocalToWorldTransform());
+                oldContactPoint->update(contactPointInfo);
                 isSimilarPointFound = true;
                 break;
             }
@@ -136,53 +109,34 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
    oldManifold->setIsObsolete(false, false);
 }
 
-// Return the manifold to remove (because it is too old or has not the largest penetration depth)
-ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const {
+// Remove a contact manifold that is the least optimal (smaller penetration depth)
+void ContactManifoldSet::removeNonOptimalManifold() {
 
-    assert(mNbManifolds == mNbMaxManifolds);
+    assert(mNbManifolds > mNbMaxManifolds);
     assert(mManifolds != nullptr);
 
     // Look for a manifold that is not new and with the smallest contact penetration depth.
     // At the same time, we also look for a new manifold with the smallest contact penetration depth
     // in case no old manifold exists.
-    ContactManifold* minDepthOldManifold = nullptr;
-    ContactManifold* minDepthNewManifold = nullptr;
-    decimal minDepthOld = DECIMAL_LARGEST;
-    decimal minDepthNew = penDepthNewManifold;
+    ContactManifold* minDepthManifold = nullptr;
+    decimal minDepth = DECIMAL_LARGEST;
     ContactManifold* manifold = mManifolds;
     while (manifold != nullptr) {
 
         // Get the largest contact point penetration depth of the manifold
         const decimal depth = manifold->getLargestContactDepth();
 
-        // If it is a new manifold
-        if (manifold->getIsNew()) {
-
-            if (depth < minDepthNew) {
-                minDepthNew = depth;
-                minDepthNewManifold = manifold;
-            }
-        }
-        else {
-
-            if (depth < minDepthOld) {
-                minDepthOld = depth;
-                minDepthOldManifold = manifold;
-            }
+        if (depth < minDepth) {
+            minDepth = depth;
+            minDepthManifold = manifold;
         }
 
         manifold = manifold->getNext();
     }
 
-    // If there was a contact manifold that was not new
-    if (minDepthOldManifold != nullptr) {
-
-        // Return the old manifold with the smallest penetration depth
-        return minDepthOldManifold;
-    }
-
-    // Otherwise, we return the new manifold with the smallest penetration depth
-    return minDepthNewManifold;
+    // Remove the non optimal manifold
+    assert(minDepthManifold != nullptr);
+    removeManifold(minDepthManifold);
 }
 
 // Return the contact manifold with a similar average normal.
@@ -262,7 +216,6 @@ void ContactManifoldSet::clear() {
 
 // Create a new contact manifold and add it to the set
 void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
-    assert(mNbManifolds < mNbMaxManifolds);
 
     ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
                                     ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
@@ -333,9 +286,26 @@ void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
         else {
 
             // Clear the obsolete contact points of the manifold
-            manifold->clearObseleteContactPoints();
+            manifold->clearObsoleteContactPoints();
         }
 
         manifold = nextManifold;
     }
 }
+
+
+// Remove some contact manifolds and contact points if there are too many of them
+void ContactManifoldSet::reduce() {
+
+    // Remove non optimal contact manifold while there are too many manifolds in the set
+    while (mNbManifolds > mNbMaxManifolds) {
+        removeNonOptimalManifold();
+    }
+
+    // Reduce all the contact manifolds in case they have too many contact points
+    ContactManifold* manifold = mManifolds;
+    while (manifold != nullptr) {
+        manifold->reduce();
+        manifold = manifold->getNext();
+    }
+}
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index ea832cdb..4b33eb8c 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -74,8 +74,8 @@ class ContactManifoldSet {
         // Return the contact manifold with a similar average normal.
         ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
 
-        /// Return the manifold to remove (because it is too old or has not the largest penetration depth)
-        ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const;
+        /// Remove a contact manifold that is the least optimal (smaller penetration depth)
+        void removeNonOptimalManifold();
 
         /// Update a previous similar manifold with a new one
         void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
@@ -124,8 +124,8 @@ class ContactManifoldSet {
         /// Clear the obsolete contact manifolds and contact points
         void clearObsoleteManifoldsAndContactPoints();
 
-        /// Reset the isNew status of all the manifolds
-        void resetIsNewManifoldStatus();
+        // Remove some contact manifolds and contact points if there are too many of them
+        void reduce();
 
         // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
         // Each face of the cube is divided into 4x4 buckets. This method maps the
@@ -180,17 +180,6 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape
     }
 }
 
-
-// Reset the isNew status of all the manifolds
-inline void ContactManifoldSet::resetIsNewManifoldStatus() {
-
-    ContactManifold* manifold = mManifolds;
-    while (manifold != nullptr) {
-        manifold->setIsNew(false);
-        manifold = manifold->getNext();
-    }
-}
-
 }
 
 #endif
diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h
index 1ff8bfdd..6fbf9834 100644
--- a/src/collision/ContactPointInfo.h
+++ b/src/collision/ContactPointInfo.h
@@ -77,6 +77,8 @@ struct ContactPointInfo {
                          : normal(contactNormal), penetrationDepth(penDepth),
                            localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
 
+            assert(contactNormal.lengthSquare() > decimal(0.8));
+            assert(penDepth > decimal(0.0));
         }
 
         /// Destructor
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 2cd951b4..b99d52f1 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -31,26 +31,22 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
-                           const Transform& body2Transform)
+ContactPoint::ContactPoint(const ContactPointInfo* contactInfo)
              : mNormal(contactInfo->normal),
                mPenetrationDepth(contactInfo->penetrationDepth),
                mLocalPointOnBody1(contactInfo->localPoint1),
                mLocalPointOnBody2(contactInfo->localPoint2),
-               mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) {
+               mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) {
 
     assert(mPenetrationDepth > decimal(0.0));
-
-    // Compute the world position of the contact points
-    mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
-    mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+    assert(mNormal.lengthSquare() > decimal(0.8));
 
     mIsObsolete = false;
 }
 
 // Update the contact point with a new one that is similar (very close)
 /// The idea is to keep the cache impulse (for warm starting the contact solver)
-void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
+void ContactPoint::update(const ContactPointInfo* contactInfo) {
 
     assert(isSimilarWithContactPoint(contactInfo));
     assert(contactInfo->penetrationDepth > decimal(0.0));
@@ -60,9 +56,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
     mLocalPointOnBody1 = contactInfo->localPoint1;
     mLocalPointOnBody2 = contactInfo->localPoint2;
 
-    // Compute the world position of the contact points
-    mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
-    mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
-
     mIsObsolete = false;
 }
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 288abdcc..e2a9a5e5 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -59,12 +59,6 @@ class ContactPoint {
         /// Contact point on body 2 in local space of body 2
         Vector3 mLocalPointOnBody2;
 
-        /// Contact point on body 1 in world space
-        Vector3 mWorldPointOnBody1;
-
-        /// 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;
 
@@ -74,14 +68,16 @@ class ContactPoint {
         /// True if the contact point is obsolete
         bool mIsObsolete;
 
-        /// Pointer to the next contact point in the linked-list
+        /// Pointer to the next contact point in the double linked-list
         ContactPoint* mNext;
 
+        /// Pointer to the previous contact point in the double linked-list
+        ContactPoint* mPrevious;
+
         // -------------------- Methods -------------------- //
 
         /// Update the contact point with a new one that is similar (very close)
-        void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
-                    const Transform& body2Transform);
+        void update(const ContactPointInfo* contactInfo);
 
         /// Return true if the contact point is similar (close enougth) to another given contact point
         bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
@@ -99,6 +95,9 @@ class ContactPoint {
         /// Set the next contact point in the linked list
         void setNext(ContactPoint* next);
 
+        /// Set the previous contact point in the linked list
+        void setPrevious(ContactPoint* previous);
+
         /// Return true if the contact point is obsolete
         bool getIsObsolete() const;
 
@@ -107,8 +106,7 @@ class ContactPoint {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
-                     const Transform& body2Transform);
+        ContactPoint(const ContactPointInfo* contactInfo);
 
         /// Destructor
         ~ContactPoint() = default;
@@ -128,18 +126,15 @@ class ContactPoint {
         /// Return the contact local point on body 2
         Vector3 getLocalPointOnBody2() const;
 
-        /// Return the contact world point on body 1
-        Vector3 getWorldPointOnBody1() const;
-
-        /// Return the contact world point on body 2
-        Vector3 getWorldPointOnBody2() const;
-
         /// Return the cached penetration impulse
         decimal getPenetrationImpulse() const;
 
         /// Return true if the contact is a resting contact
         bool getIsRestingContact() const;
 
+        /// Return the previous contact point in the linked list
+        inline ContactPoint* getPrevious() const;
+
         /// Return the next contact point in the linked list
         ContactPoint* getNext() const;
 
@@ -170,16 +165,6 @@ inline Vector3 ContactPoint::getLocalPointOnBody2() const {
     return mLocalPointOnBody2;
 }
 
-// Return the contact world point on body 1
-inline Vector3 ContactPoint::getWorldPointOnBody1() const {
-    return mWorldPointOnBody1;
-}
-
-// Return the contact world point on body 2
-inline Vector3 ContactPoint::getWorldPointOnBody2() const {
-    return mWorldPointOnBody2;
-}
-
 // Return the cached penetration impulse
 inline decimal ContactPoint::getPenetrationImpulse() const {
     return mPenetrationImpulse;
@@ -226,6 +211,16 @@ inline void ContactPoint::setNext(ContactPoint* next) {
     mNext = next;
 }
 
+// Return the previous contact point in the linked list
+inline ContactPoint* ContactPoint::getPrevious() const {
+   return mPrevious;
+}
+
+// Set the previous contact point in the linked list
+inline void ContactPoint::setPrevious(ContactPoint* previous) {
+    mPrevious = previous;
+}
+
 // Return the penetration depth of the contact
 inline decimal ContactPoint::getPenetrationDepth() const {
     return mPenetrationDepth;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 94a82b14..819a1b76 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -120,6 +120,10 @@ void ContactSolver::initializeForIsland(Island* island) {
         assert(body1 != nullptr);
         assert(body2 != nullptr);
 
+        // Get the two contact shapes
+        const ProxyShape* shape1 = externalManifold->getShape1();
+        const ProxyShape* shape2 = externalManifold->getShape2();
+
         // Get the position of the two bodies
         const Vector3& x1 = body1->mCenterOfMassWorld;
         const Vector3& x2 = body2->mCenterOfMassWorld;
@@ -149,11 +153,12 @@ void ContactSolver::initializeForIsland(Island* island) {
 
         // For each  contact point of the contact manifold
         ContactPoint* externalContact = externalManifold->getContactPoints();
+        assert(externalContact != nullptr);
         while (externalContact != nullptr) {
 
             // Get the contact point on the two bodies
-            Vector3 p1 = externalContact->getWorldPointOnBody1();
-            Vector3 p2 = externalContact->getWorldPointOnBody2();
+            Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1();
+            Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2();
 
             new (mContactPoints + mNbContactPoints) ContactPointSolver();
             mContactPoints[mNbContactPoints].externalContact = externalContact;
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 2725a8d5..47f13bfb 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -136,9 +136,6 @@ class OverlappingPair {
         /// Return the last frame collision info
         LastFrameCollisionInfo& getLastFrameCollisionInfo();
 
-        /// Return the number of contacts in the cache
-        uint getNbContactPoints() const;
-
         /// Return the a reference to the contact manifold set
         const ContactManifoldSet& getContactManifoldSet();
 
@@ -167,13 +164,13 @@ class OverlappingPair {
         void reducePotentialContactManifolds();
 
         /// Make the contact manifolds and contact points obsolete
-        void makeContactsObselete();
+        void makeContactsObsolete();
 
         /// Clear the obsolete contact manifold and contact points
         void clearObsoleteManifoldsAndContactPoints();
 
-        /// Reset the isNew status of all the manifolds
-        void resetIsNewManifoldStatus();
+        /// Reduce the contact manifolds that have too many contact points
+        void reduceContactManifolds();
 
         /// Return the pair of bodies index
         static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -206,18 +203,13 @@ inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
     return mLastFrameCollisionInfo;
 }
 
-// Return the number of contact points in the contact manifold
-inline uint OverlappingPair::getNbContactPoints() const {
-    return mContactManifoldSet.getTotalNbContactPoints();
-}
-
 // Return the contact manifold
 inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
     return mContactManifoldSet;
 }
 
 // Make the contact manifolds and contact points obsolete
-inline void OverlappingPair::makeContactsObselete() {
+inline void OverlappingPair::makeContactsObsolete() {
 
     mContactManifoldSet.makeContactsObsolete();
 }
@@ -272,10 +264,9 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
     mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
 }
 
-
-// Reset the isNew status of all the manifolds
-inline void OverlappingPair::resetIsNewManifoldStatus() {
-   mContactManifoldSet.resetIsNewManifoldStatus();
+// Reduce the contact manifolds that have too many contact points
+inline void OverlappingPair::reduceContactManifolds() {
+   mContactManifoldSet.reduce();
 }
 
 }