diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 01e1694b..e4ce4458 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -39,10 +39,10 @@ using namespace reactphysics3d;
 * @param id The ID of the body
 */
 RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
-          : CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
+          : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
             mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
             mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
-            mJointsList(nullptr), mArrayIndex(0) {
+            mJointsList(nullptr) {
 
     // Compute the inverse mass
     mMassInverse = decimal(1.0) / mInitMass;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 5e40f45f..2605eaf9 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -374,8 +374,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
     uint firstEdgeIndex = face.edgeIndex;
     uint edgeIndex = firstEdgeIndex;
 
-    std::vector<Vector3> planesPoints;
-    std::vector<Vector3> planesNormals;
+    List<Vector3> planesPoints(mMemoryAllocator, 2);
+    List<Vector3> planesNormals(mMemoryAllocator, 2);
 
     // For each adjacent edge of the separating face of the polyhedron
     do {
@@ -393,15 +393,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
         Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
 
         // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
-        planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
-        planesNormals.push_back(clipPlaneNormal);
+        planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex));
+        planesNormals.add(clipPlaneNormal);
 
         edgeIndex = edge.nextEdgeIndex;
 
     } while(edgeIndex != firstEdgeIndex);
 
     // First we clip the inner segment of the capsule with the four planes of the adjacent faces
-    std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
+    List<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator);
 
 	// Project the two clipped points into the polyhedron face
 	const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index b82c9fbd..0d8994f4 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -222,27 +222,34 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co
 
 // Clip a segment against multiple planes and return the clipped segment vertices
 // This method implements the Sutherland–Hodgman clipping algorithm
-std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
-                                                           const std::vector<Vector3>& planesPoints,
-                                                           const std::vector<Vector3>& planesNormals) {
+List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+                                                           const List<Vector3>& planesPoints,
+                                                           const List<Vector3>& planesNormals,
+                                                           Allocator& allocator) {
 
     assert(planesPoints.size() == planesNormals.size());
 
-    std::vector<Vector3> inputVertices = {segA, segB};
-    std::vector<Vector3> outputVertices;
+    List<Vector3> list1(allocator, 2);
+    List<Vector3> list2(allocator, 2);
+
+    List<Vector3>* inputVertices = &list1;
+    List<Vector3>* outputVertices = &list2;
+
+    inputVertices->add(segA);
+    inputVertices->add(segB);
 
     // For each clipping plane
     for (uint p=0; p<planesPoints.size(); p++) {
 
         // If there is no more vertices, stop
-        if (inputVertices.empty()) return inputVertices;
+        if (inputVertices->size() == 0) return *inputVertices;
 
-        assert(inputVertices.size() == 2);
+        assert(inputVertices->size() == 2);
 
-        outputVertices.clear();
+        outputVertices->clear();
 
-        Vector3& v1 = inputVertices[0];
-        Vector3& v2 = inputVertices[1];
+        Vector3& v1 = (*inputVertices)[0];
+        Vector3& v2 = (*inputVertices)[1];
 
         decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
         decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -257,39 +264,40 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
                 decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
 
                 if (t >= decimal(0) && t <= decimal(1.0)) {
-                    outputVertices.push_back(v1 + t * (v2 - v1));
+                    outputVertices->add(v1 + t * (v2 - v1));
                 }
                 else {
-                    outputVertices.push_back(v2);
+                    outputVertices->add(v2);
                 }
             }
             else {
-                outputVertices.push_back(v1);
+                outputVertices->add(v1);
             }
 
             // Add the second vertex
-            outputVertices.push_back(v2);
+            outputVertices->add(v2);
         }
         else {  // If the second vertex is behind the clipping plane
 
             // If the first vertex is in front of the clippling plane
             if (v1DotN >= decimal(0.0)) {
 
-                outputVertices.push_back(v1);
+                outputVertices->add(v1);
 
                 // The first point we keep is the intersection between the segment v1, v2 and the clipping plane
                 decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
 
                 if (t >= decimal(0.0) && t <= decimal(1.0)) {
-                    outputVertices.push_back(v1 + t * (v2 - v1));
+                    outputVertices->add(v1 + t * (v2 - v1));
                 }
             }
         }
 
         inputVertices = outputVertices;
+        outputVertices = p % 2 == 0 ? &list1 : &list2;
     }
 
-    return outputVertices;
+    return *outputVertices;
 }
 
 // Clip a polygon against multiple planes and return the clipped polygon vertices
@@ -302,7 +310,6 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
     uint nbMaxElements = polygonVertices.size() + planesPoints.size();
     List<Vector3> list1(allocator, nbMaxElements);
     List<Vector3> list2(allocator, nbMaxElements);
-    List<decimal> dotProducts(allocator, nbMaxElements * 2);
 
     const List<Vector3>* inputVertices = &polygonVertices;
     List<Vector3>* outputVertices = &list2;
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index ff6ac6f4..5081d741 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -112,9 +112,10 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB
 decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
 
 /// Clip a segment against multiple planes and return the clipped segment vertices
-std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
-                                                           const std::vector<Vector3>& planesPoints,
-                                                           const std::vector<Vector3>& planesNormals);
+List<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+                                                           const List<Vector3>& planesPoints,
+                                                           const List<Vector3>& planesNormals,
+                                                           Allocator& allocator);
 
 /// Clip a polygon against multiple planes and return the clipped polygon vertices
 List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,