diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h
index 728ee1ed..6f6a0e74 100644
--- a/include/reactphysics3d/collision/ContactManifoldInfo.h
+++ b/include/reactphysics3d/collision/ContactManifoldInfo.h
@@ -55,7 +55,7 @@ struct ContactManifoldInfo {
 
         /// Constructor
         ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator)
-               : potentialContactPointsIndices(allocator), pairId(pairId) {
+               : potentialContactPointsIndices(allocator, 4), pairId(pairId) {
 
         }
 
diff --git a/include/reactphysics3d/collision/ContactPair.h b/include/reactphysics3d/collision/ContactPair.h
index 82bf9d20..f9a49817 100644
--- a/include/reactphysics3d/collision/ContactPair.h
+++ b/include/reactphysics3d/collision/ContactPair.h
@@ -91,7 +91,7 @@ struct ContactPair {
         /// Constructor
         ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity,
                     Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator)
-            : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
+            : pairId(pairId), potentialContactManifoldsIndices(allocator, 8), body1Entity(body1Entity), body2Entity(body2Entity),
               collider1Entity(collider1Entity), collider2Entity(collider2Entity),
               isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
               contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) {
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 6f0ba719..97667ed8 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -36,7 +36,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, decimal persiste
                mPenetrationDepth(contactInfo->penetrationDepth),
                mLocalPointOnShape1(contactInfo->localPoint1),
                mLocalPointOnShape2(contactInfo->localPoint2),
-               mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr),
+               mIsRestingContact(false), mIsObsolete(false),
                mPersistentContactDistanceThreshold(persistentContactDistanceThreshold) {
 
     assert(mPenetrationDepth > decimal(0.0));
diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp
index 24f9660c..9c15e7c7 100644
--- a/src/systems/CollisionDetectionSystem.cpp
+++ b/src/systems/CollisionDetectionSystem.cpp
@@ -520,6 +520,9 @@ void CollisionDetectionSystem::computeNarrowPhase() {
     // Swap the previous and current contacts lists
     swapPreviousAndCurrentContacts();
 
+    mPotentialContactManifolds.reserve(mPreviousContactManifolds->size());
+    mPotentialContactPoints.reserve(mPreviousContactPoints->size());
+
     // Test the narrow-phase collision detection on the batches to be tested
     testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator);
 
diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp
index 1f6b6f74..76a79c74 100644
--- a/src/systems/ContactSolverSystem.cpp
+++ b/src/systems/ContactSolverSystem.cpp
@@ -125,17 +125,17 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
 
         assert(externalManifold.nbContactPoints > 0);
 
+        const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
+        const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);
+
         // Get the two bodies of the contact
-        RigidBody* body1 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity1));
-        RigidBody* body2 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity2));
+        RigidBody* body1 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex1];
+        RigidBody* body2 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex2];
         assert(body1 != nullptr);
         assert(body2 != nullptr);
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
 
-        const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
-        const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);
-
         Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1);
         Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2);