diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 6aeac709..53792245 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -34,7 +34,6 @@
 #include "narrowphase/DefaultCollisionDispatch.h"
 #include "memory/MemoryManager.h"
 #include "constraint/ContactPoint.h"
-#include <vector>
 #include <set>
 #include <utility>
 #include <map>
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index f513ad82..eea392d5 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -27,7 +27,6 @@
 #define	REACTPHYSICS3D_CONTACT_MANIFOLD_H
 
 // Libraries
-#include <vector>
 #include "body/CollisionBody.h"
 #include "collision/ProxyShape.h"
 #include "constraint/ContactPoint.h"
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
index 70a731d5..4378aeca 100644
--- a/src/collision/HalfEdgeStructure.cpp
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -41,13 +41,13 @@ void HalfEdgeStructure::init() {
     std::map<uint, edgeKey> mapEdgeIndexToKey;
     std::map<uint, edgeKey> mapFaceIndexToEdgeKey;
 
+    List<edgeKey> currentFaceEdges(mAllocator, mFaces[0].faceVertices.size());
+
     // For each face
     for (uint f=0; f<mFaces.size(); f++) {
 
         Face face = mFaces[f];
 
-        std::vector<edgeKey> currentFaceEdges;
-
         edgeKey firstEdgeKey;
 
         // For each vertex of the face
@@ -100,8 +100,10 @@ void HalfEdgeStructure::init() {
                 mEdges.add(edge);
             }
 
-            currentFaceEdges.push_back(pairV1V2);
+            currentFaceEdges.add(pairV1V2);
         }
+
+        currentFaceEdges.clear();
     }
 
     // Set next edges
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 15658a93..924d32bf 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -28,7 +28,6 @@
 
 // Libraries
 #include "mathematics/mathematics.h"
-#include <vector>
 
 namespace reactphysics3d {
 
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index b459723a..b8cd1f52 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -31,7 +31,6 @@
 #include "HalfEdgeStructure.h"
 #include "collision/PolygonVertexArray.h"
 #include "memory/DefaultAllocator.h"
-#include <vector>
 
 namespace reactphysics3d {
 
diff --git a/src/collision/TriangleMesh.h b/src/collision/TriangleMesh.h
index e33fad67..69196d22 100644
--- a/src/collision/TriangleMesh.h
+++ b/src/collision/TriangleMesh.h
@@ -27,9 +27,10 @@
 #define REACTPHYSICS3D_TRIANGLE_MESH_H
 
 // Libraries
-#include <vector>
 #include <cassert>
 #include "TriangleVertexArray.h"
+#include "memory/MemoryManager.h"
+#include "containers/List.h"
 
 namespace reactphysics3d {
 
@@ -46,12 +47,14 @@ class TriangleMesh {
     protected:
 
         /// All the triangle arrays of the mesh (one triangle array per part)
-        std::vector<TriangleVertexArray*> mTriangleArrays;
+        List<TriangleVertexArray*> mTriangleArrays;
 
     public:
 
         /// Constructor
-        TriangleMesh() = default;
+        TriangleMesh() : mTriangleArrays(MemoryManager::getBaseAllocator()) {
+
+        }
 
         /// Destructor
         ~TriangleMesh() = default;
@@ -68,7 +71,7 @@ class TriangleMesh {
 
 // Add a subpart of the mesh
 inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
-    mTriangleArrays.push_back(triangleVertexArray );
+    mTriangleArrays.add(triangleVertexArray );
 }
 
 // Return a pointer to a given subpart (triangle vertex array) of the mesh
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 30a67920..3ae18677 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -27,7 +27,6 @@
 #define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
 
 // Libraries
-#include <vector>
 #include "body/CollisionBody.h"
 #include "collision/ProxyShape.h"
 #include "DynamicAABBTree.h"
diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h
index 9cbee47e..47f1b2c2 100644
--- a/src/collision/narrowphase/GJK/VoronoiSimplex.h
+++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h
@@ -28,7 +28,6 @@
 
 // Libraries
 #include "mathematics/mathematics.h"
-#include <vector>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index c13e32f4..109b736a 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -28,7 +28,6 @@
 #include "collision/ProxyShape.h"
 #include "configuration.h"
 #include "memory/MemoryManager.h"
-#include <vector>
 #include <cassert>
 
 using namespace reactphysics3d;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 8f215fc2..ec5fa913 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -155,7 +155,7 @@ uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex)
 decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
 
     // Add the id of the hit AABB node into
-    mHitAABBNodes.push_back(nodeId);
+    mHitAABBNodes.add(nodeId);
 
     return ray.maxFraction;
 }
@@ -163,7 +163,7 @@ decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const R
 // Raycast all collision shapes that have been collected
 void ConcaveMeshRaycastCallback::raycastTriangles() {
 
-    std::vector<int>::const_iterator it;
+    List<int>::Iterator it;
     decimal smallestHitFraction = mRay.maxFraction;
 
     for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) {
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index c8c91972..2ecefb94 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -31,6 +31,7 @@
 #include "collision/broadphase/DynamicAABBTree.h"
 #include "collision/TriangleMesh.h"
 #include "collision/shapes/TriangleShape.h"
+#include "containers/List.h"
 #include "engine/Profiler.h"
 
 namespace reactphysics3d {
@@ -70,7 +71,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
 
     private :
 
-        std::vector<int32> mHitAABBNodes;
+        List<int32> mHitAABBNodes;
         const DynamicAABBTree& mDynamicAABBTree;
         const ConcaveMeshShape& mConcaveMeshShape;
         ProxyShape* mProxyShape;
@@ -91,7 +92,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
         // Constructor
         ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
                                    ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator)
-            : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
+            : mHitAABBNodes(allocator), mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
               mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
 
         }
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 44d01df4..44e64ad3 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -33,7 +33,6 @@
 #include "collision/TriangleMesh.h"
 #include "collision/PolyhedronMesh.h"
 #include "collision/narrowphase/GJK/GJKAlgorithm.h"
-#include <vector>
 #include <set>
 #include <map>
 
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index da5bf609..1a462c60 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -34,7 +34,7 @@ using namespace std;
 // Constructor
 CollisionWorld::CollisionWorld()
                : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0),
-                 mEventListener(nullptr) {
+                 mFreeBodiesIDs(mMemoryManager.getPoolAllocator()), mEventListener(nullptr) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -102,7 +102,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     collisionBody->removeAllCollisionShapes();
 
     // Add the body ID to the list of free IDs
-    mFreeBodiesIDs.push_back(collisionBody->getID());
+    mFreeBodiesIDs.add(collisionBody->getID());
 
     // Call the destructor of the collision body
     collisionBody->~CollisionBody();
@@ -119,9 +119,9 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
 
     // Compute the body ID
     bodyindex bodyID;
-    if (!mFreeBodiesIDs.empty()) {
-        bodyID = mFreeBodiesIDs.back();
-        mFreeBodiesIDs.pop_back();
+    if (mFreeBodiesIDs.size() != 0) {
+        bodyID = mFreeBodiesIDs[mFreeBodiesIDs.size() - 1];
+        mFreeBodiesIDs.remove(mFreeBodiesIDs.size() - 1);
     }
     else {
         bodyID = mCurrentBodyID;
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 9f1fea58..bdffe4d1 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -27,11 +27,11 @@
 #define REACTPHYSICS3D_COLLISION_WORLD_H
 
 // Libraries
-#include <vector>
 #include <set>
 #include <list>
 #include <algorithm>
 #include "mathematics/mathematics.h"
+#include "containers/List.h"
 #include "Profiler.h"
 #include "body/CollisionBody.h"
 #include "collision/RaycastInfo.h"
@@ -74,7 +74,7 @@ class CollisionWorld {
         bodyindex mCurrentBodyID;
 
         /// List of free ID for rigid bodies
-        std::vector<luint> mFreeBodiesIDs;
+        List<luint> mFreeBodiesIDs;
 
         /// Pointer to an event listener object
         EventListener* mEventListener;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 35b1ba04..b523a505 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -446,7 +446,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     rigidBody->removeAllCollisionShapes();
 
     // Add the body ID to the list of free IDs
-    mFreeBodiesIDs.push_back(rigidBody->getID());
+    mFreeBodiesIDs.add(rigidBody->getID());
 
     // Destroy all the joints in which the rigid body to be destroyed is involved
     JointListElement* element;
@@ -828,9 +828,9 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
 }
 
 /// Return the list of all contacts of the world
-std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
+List<const ContactManifold*> DynamicsWorld::getContactsList() {
 
-    std::vector<const ContactManifold*> contactManifolds;
+    List<const ContactManifold*> contactManifolds(mMemoryManager.getPoolAllocator());
 
     // For each currently overlapping pair of bodies
     std::map<overlappingpairid, OverlappingPair*>::const_iterator it;
@@ -845,7 +845,7 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
         while (manifold != nullptr) {
 
             // Get the contact manifold
-            contactManifolds.push_back(manifold);
+            contactManifolds.add(manifold);
 
             manifold = manifold->getNext();
         }
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 0c614b65..705f63d5 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -249,7 +249,7 @@ class DynamicsWorld : public CollisionWorld {
         void setEventListener(EventListener* eventListener);
 
         /// Return the list of all contacts of the world
-        std::vector<const ContactManifold*> getContactsList() const;
+        List<const ContactManifold*> getContactsList();
 
         // -------------------- Friendship -------------------- //
 
diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp
index 271fd82f..eed109dc 100644
--- a/src/mathematics/Vector2.cpp
+++ b/src/mathematics/Vector2.cpp
@@ -25,7 +25,6 @@
 
 // Libraries
 #include "Vector2.h"
-#include <vector>
 
 // Namespaces
 using namespace reactphysics3d;
diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp
index 33b760ba..9a0cc082 100644
--- a/src/mathematics/Vector3.cpp
+++ b/src/mathematics/Vector3.cpp
@@ -26,7 +26,6 @@
 // Libraries
 #include "Vector3.h"
 #include <iostream>
-#include <vector>
 
 // Namespaces
 using namespace reactphysics3d;
diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h
index b2f2bd1f..9fac1571 100644
--- a/src/mathematics/mathematics.h
+++ b/src/mathematics/mathematics.h
@@ -36,7 +36,6 @@
 #include "Ray.h"
 #include "configuration.h"
 #include "mathematics_functions.h"
-#include <vector>
 #include <cstdio>
 #include <cassert>
 #include <cmath>
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 7e4a1427..52ac337b 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -28,7 +28,6 @@
 #include "Vector3.h"
 #include "Vector2.h"
 #include <cassert>
-#include <vector>
 
 using namespace reactphysics3d;
 
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index a72b0077..d0968904 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -32,7 +32,6 @@
 #include <algorithm>
 #include <cassert>
 #include <cmath>
-#include <vector>
 #include "containers/List.h"
 
 /// ReactPhysics3D namespace
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index dd2a6265..f2ea5abe 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -64,6 +64,7 @@
 #include "constraint/SliderJoint.h"
 #include "constraint/HingeJoint.h"
 #include "constraint/FixedJoint.h"
+#include "containers/List.h"
 
 /// Alias to the ReactPhysics3D namespace
 namespace rp3d = reactphysics3d;
diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h
index 53f5e618..e43c63e7 100755
--- a/test/tests/collision/TestDynamicAABBTree.h
+++ b/test/tests/collision/TestDynamicAABBTree.h
@@ -29,7 +29,6 @@
 // Libraries
 #include "Test.h"
 #include "collision/broadphase/DynamicAABBTree.h"
-#include <vector>
 
 /// Reactphysics3D namespace
 namespace reactphysics3d {
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 9a90f25c..e60893df 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -186,7 +186,7 @@ class CollisionDetectionScene : public SceneDemo {
         virtual void setIsContactPointsDisplayed(bool display) override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Display or not the surface normals at hit points
@@ -205,7 +205,7 @@ inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
 }
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() const {
+inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() {
     return mContactManager.getContactPoints();
 }
 
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
index b22d8429..dbb49539 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.h
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h
@@ -101,11 +101,11 @@ class CollisionShapesScene : public SceneDemo {
         virtual void reset() override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionShapesScene::getContactPoints() const {
+inline std::vector<ContactPoint> CollisionShapesScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h
index beca8f71..91ede1bb 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.h
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h
@@ -98,11 +98,11 @@ class ConcaveMeshScene : public SceneDemo {
         virtual void reset() override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> ConcaveMeshScene::getContactPoints() const {
+inline std::vector<ContactPoint> ConcaveMeshScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h
index 6d553835..e284a91d 100755
--- a/testbed/scenes/cubes/CubesScene.h
+++ b/testbed/scenes/cubes/CubesScene.h
@@ -69,11 +69,11 @@ class CubesScene : public SceneDemo {
         virtual void reset() override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> CubesScene::getContactPoints() const {
+inline std::vector<ContactPoint> CubesScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h
index 9bbc1c01..d4a4dace 100644
--- a/testbed/scenes/cubestack/CubeStackScene.h
+++ b/testbed/scenes/cubestack/CubeStackScene.h
@@ -69,11 +69,11 @@ class CubeStackScene : public SceneDemo {
         virtual void reset() override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> CubeStackScene::getContactPoints() const {
+inline std::vector<ContactPoint> CubeStackScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h
index 2a3e68f5..90edda93 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.h
+++ b/testbed/scenes/heightfield/HeightFieldScene.h
@@ -100,11 +100,11 @@ class HeightFieldScene : public SceneDemo {
         virtual void reset() override ;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override ;
+        virtual std::vector<ContactPoint> getContactPoints() override ;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> HeightFieldScene::getContactPoints() const {
+inline std::vector<ContactPoint> HeightFieldScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h
index dad9af38..f101bf32 100644
--- a/testbed/scenes/joints/JointsScene.h
+++ b/testbed/scenes/joints/JointsScene.h
@@ -127,11 +127,11 @@ class JointsScene : public SceneDemo {
         virtual void reset() override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> JointsScene::getContactPoints() const {
+inline std::vector<ContactPoint> JointsScene::getContactPoints() {
     return computeContactPointsOfWorld(getDynamicsWorld());
 }
 
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index 8024ff1a..e8e4a854 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -198,7 +198,7 @@ class RaycastScene : public SceneDemo {
         virtual void setIsContactPointsDisplayed(bool display) override;
 
         /// Return all the contact points of the scene
-        virtual std::vector<ContactPoint> getContactPoints() const override;
+        virtual std::vector<ContactPoint> getContactPoints() override;
 };
 
 // Display or not the surface normals at hit points
@@ -217,7 +217,7 @@ inline void RaycastScene::setIsContactPointsDisplayed(bool display) {
 }
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> RaycastScene::getContactPoints() const {
+inline std::vector<ContactPoint> RaycastScene::getContactPoints() {
     return mRaycastManager.getHitPoints();
 }
 
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index 89a9afdd..6ccc3254 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -230,7 +230,7 @@ class Scene {
         void setIsWireframeEnabled(bool isEnabled);
 
         /// Return all the contact points of the scene
-        std::vector<ContactPoint> virtual getContactPoints() const;
+        std::vector<ContactPoint> virtual getContactPoints();
 };
 
 // Called when a keyboard event occurs
@@ -303,7 +303,7 @@ inline void Scene::setIsWireframeEnabled(bool isEnabled) {
 }
 
 // Return all the contact points of the scene
-inline std::vector<ContactPoint> Scene::getContactPoints() const {
+inline std::vector<ContactPoint> Scene::getContactPoints() {
 
     // Return an empty list of contact points
     return std::vector<ContactPoint>();
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index 5ef6fb95..8e2a92da 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -418,15 +418,15 @@ void SceneDemo::removeAllContactPoints() {
 }
 
 // Return all the contact points of the scene
-std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const {
+std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(rp3d::DynamicsWorld* world) {
 
     std::vector<ContactPoint> contactPoints;
 
     // Get the list of contact manifolds from the world
-    std::vector<const rp3d::ContactManifold*> manifolds = world->getContactsList();
+    rp3d::List<const rp3d::ContactManifold*> manifolds = world->getContactsList();
 
     // For each contact manifold
-    std::vector<const rp3d::ContactManifold*>::const_iterator it;
+    rp3d::List<const rp3d::ContactManifold*>::Iterator it;
     for (it = manifolds.begin(); it != manifolds.end(); ++it) {
 
         const rp3d::ContactManifold* manifold = *it;
diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h
index 359c2374..1eaa30b9 100644
--- a/testbed/src/SceneDemo.h
+++ b/testbed/src/SceneDemo.h
@@ -158,7 +158,7 @@ class SceneDemo : public Scene {
         virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
 
         /// Return all the contact points of the scene
-        std::vector<ContactPoint> computeContactPointsOfWorld(const rp3d::DynamicsWorld* world) const;
+        std::vector<ContactPoint> computeContactPointsOfWorld(reactphysics3d::DynamicsWorld *world);
 };
 
 // Enabled/Disable the shadow mapping