Merge branch 'optimization' into develop

CMakeLists.txt

@@ -199,6 +199,8 @@ SET (REACTPHYSICS3D_SOURCES
     "src/containers/LinkedList.h"
     "src/containers/List.h"
     "src/containers/Map.h"
+    "src/containers/Set.h"
+    "src/containers/Pair.h"
 )
 
 # Create the library

src/collision/CollisionDetection.cpp

@@ -37,19 +37,19 @@
 #include "collision/OverlapCallback.h"
 #include <cassert>
 #include <complex>
-#include <set>
 #include <utility>
 #include <utility>
-#include <unordered_set>
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 using namespace std;
 
+
 // Constructor
 CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager)
-                   : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
+                   : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr),
-                     mIsCollisionShapesAdded(false) {
+                     mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this),
+                     mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) {
 
     // Set the default collision dispatch configuration
     setCollisionDispatch(&mDefaultCollisionDispatch);
@@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() {
     PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler);
 
     // For each possible collision pair of bodies
-    map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
 
         OverlappingPair* pair = it->second;
@@ -126,14 +126,11 @@ void CollisionDetection::computeMiddlePhase() {
         // overlapping pair
         if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 
-            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
-            ++it;
-
             // Destroy the overlapping pair
-            itToRemove->second->~OverlappingPair();
+            it->second->~OverlappingPair();
 
-            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
+            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair));
-            mOverlappingPairs.erase(itToRemove);
+            it = mOverlappingPairs.remove(it);
             continue;
         }
         else {
@@ -154,7 +151,7 @@ void CollisionDetection::computeMiddlePhase() {
 
             // Check if the bodies are in the set of bodies that cannot collide between each other
             bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
-            if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
+            if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue;
 
 			bool isShape1Convex = shape1->getCollisionShape()->isConvex();
 			bool isShape2Convex = shape2->getCollisionShape()->isConvex();
@@ -320,10 +317,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
         (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
 
     // Compute the overlapping pair ID
-    overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
+    Pair<uint, uint> pairID = OverlappingPair::computeID(shape1, shape2);
 
     // Check if the overlapping pair already exists
-    if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
+    if (mOverlappingPairs.containsKey(pairID)) return;
 
     // Create the overlapping pair and add it into the set of overlapping pairs
     OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
@@ -331,11 +328,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
                                               mMemoryManager.getSingleFrameAllocator());
     assert(newPair != nullptr);
 
-#ifndef NDEBUG
+    mOverlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, newPair));
-    std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
-#endif
-    mOverlappingPairs.insert(make_pair(pairID, newPair));
-    assert(check.second);
 
     // Wake up the two bodies
     shape1->getBody()->setIsSleeping(false);
@@ -348,19 +341,17 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
     assert(proxyShape->mBroadPhaseID != -1);
 
     // Remove all the overlapping pairs involving this proxy shape
-    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
         if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
             it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
-            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
-            ++it;
 
             // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
 
             // Destroy the overlapping pair
-            itToRemove->second->~OverlappingPair();
+            it->second->~OverlappingPair();
-            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
+            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair));
-            mOverlappingPairs.erase(itToRemove);
+            it = mOverlappingPairs.remove(it);
         }
         else {
             ++it;
@@ -376,7 +367,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() {
     PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler);
 
     // For each overlapping pairs in contact during the narrow-phase
-    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         // Add all the contact manifolds of the pair into the list of contact manifolds
@@ -427,7 +418,7 @@ void CollisionDetection::processAllPotentialContacts() {
     PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler);
 
     // For each overlapping pairs in contact during the narrow-phase
-    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         // Process the potential contacts of the overlapping pair
@@ -466,7 +457,7 @@ void CollisionDetection::reportAllContacts() {
     PROFILE("CollisionDetection::reportAllContacts()", mProfiler);
 
     // For each overlapping pairs in contact during the narrow-phase
-    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         // If there is a user callback
@@ -524,7 +515,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
                                          unsigned short categoryMaskBits) {
     assert(overlapCallback != nullptr);
 
-    std::unordered_set<bodyindex> reportedBodies;
+    Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
 
     // Ask the broad-phase to get all the overlapping shapes
     LinkedList<int> overlappingNodes(mMemoryManager.getPoolAllocator());
@@ -547,7 +538,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
             if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
 
                 // Add the body into the set of reported bodies
-                reportedBodies.insert(overlapBody->getID());
+                reportedBodies.add(overlapBody->getID());
 
                 // Notify the overlap to the user
                 overlapCallback->notifyOverlap(overlapBody);
@@ -640,7 +631,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
 
     assert(overlapCallback != nullptr);
 
-    std::unordered_set<bodyindex> reportedBodies;
+    Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
 
     // For each proxy shape proxy shape of the body
     ProxyShape* bodyProxyShape = body->getProxyShapesList();
@@ -720,7 +711,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
                             CollisionBody* overlapBody = proxyShape->getBody();
 
                             // Add the body into the set of reported bodies
-                            reportedBodies.insert(overlapBody->getID());
+                            reportedBodies.add(overlapBody->getID());
 
                             // Notify the overlap to the user
                             overlapCallback->notifyOverlap(overlapBody);
@@ -921,7 +912,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
     computeBroadPhase();
 
     // For each possible collision pair of bodies
-    map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         OverlappingPair* originalPair = it->second;

src/collision/CollisionDetection.h

@@ -34,10 +34,9 @@
 #include "narrowphase/DefaultCollisionDispatch.h"
 #include "memory/MemoryManager.h"
 #include "constraint/ContactPoint.h"
-#include <vector>
+#include "containers/Map.h"
-#include <set>
+#include "containers/Set.h"
 #include <utility>
-#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -80,7 +79,7 @@ class CollisionDetection {
         NarrowPhaseInfo* mNarrowPhaseInfoList;
 
         /// Broad-phase overlapping pairs
-        std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
+        Map<Pair<uint, uint>, OverlappingPair*> mOverlappingPairs;
 
         /// Broad-phase algorithm
         BroadPhaseAlgorithm mBroadPhaseAlgorithm;
@@ -89,9 +88,8 @@ class CollisionDetection {
         // TODO : Delete this
         GJKAlgorithm mNarrowPhaseGJKAlgorithm;
 
-        // TODO : Maybe delete this set (what is the purpose ?)
         /// Set of pair of bodies that cannot collide between each other
-        std::set<bodyindexpair> mNoCollisionPairs;
+        Set<bodyindexpair> mNoCollisionPairs;
 
         /// True if some collision shapes have been added previously
         bool mIsCollisionShapesAdded;
@@ -213,6 +211,9 @@ class CollisionDetection {
         /// Allow the broadphase to notify the collision detection about an overlapping pair.
         void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
 
+        /// Return a reference to the memory manager
+        MemoryManager& getMemoryManager() const;
+
         /// Return a pointer to the world
         CollisionWorld* getWorld();
 
@@ -264,13 +265,13 @@ inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
 // Add a pair of bodies that cannot collide with each other
 inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
                                                    CollisionBody* body2) {
-    mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
+    mNoCollisionPairs.add(OverlappingPair::computeBodiesIndexPair(body1, body2));
 }
 
 // Remove a pair of bodies that cannot collide with each other
 inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
                                                       CollisionBody* body2) {
-    mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
+    mNoCollisionPairs.remove(OverlappingPair::computeBodiesIndexPair(body1, body2));
 }
 
 // Ask for a collision shape to be tested again during broad-phase.
@@ -327,6 +328,11 @@ inline CollisionWorld* CollisionDetection::getWorld() {
     return mWorld;
 }
 
+// Return a reference to the memory manager
+inline MemoryManager& CollisionDetection::getMemoryManager() const {
+    return mMemoryManager;
+}
+
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler

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"

src/collision/HalfEdgeStructure.cpp

@@ -25,37 +25,37 @@
 
 // Libraries
 #include "HalfEdgeStructure.h"
-#include <map>
+#include "containers/Map.h"
+#include "containers/Pair.h"
+#include "containers/containers_common.h"
 
 using namespace reactphysics3d;
 
 // Initialize the structure (when all vertices and faces have been added)
 void HalfEdgeStructure::init() {
 
-    using edgeKey = std::pair<uint, uint>;
+    Map<VerticesPair, Edge> edges(mAllocator);
+    Map<VerticesPair, VerticesPair> nextEdges(mAllocator);
+    Map<VerticesPair, uint> mapEdgeToStartVertex(mAllocator);
+    Map<VerticesPair, uint> mapEdgeToIndex(mAllocator);
+    Map<uint, VerticesPair> mapEdgeIndexToKey(mAllocator);
+    Map<uint, VerticesPair> mapFaceIndexToEdgeKey(mAllocator);
 
-    std::map<edgeKey, Edge> edges;
+    List<VerticesPair> currentFaceEdges(mAllocator, mFaces[0].faceVertices.size());
-    std::map<edgeKey, edgeKey> nextEdges;
-    std::map<edgeKey, uint> mapEdgeToStartVertex;
-    std::map<edgeKey, uint> mapEdgeToIndex;
-    std::map<uint, edgeKey> mapEdgeIndexToKey;
-    std::map<uint, edgeKey> mapFaceIndexToEdgeKey;
 
     // For each face
     for (uint f=0; f<mFaces.size(); f++) {
 
         Face face = mFaces[f];
 
-        std::vector<edgeKey> currentFaceEdges;
+        VerticesPair firstEdgeKey(0, 0);
-
-        edgeKey firstEdgeKey;
 
         // For each vertex of the face
         for (uint v=0; v < face.faceVertices.size(); v++) {
             uint v1Index = face.faceVertices[v];
             uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1];
 
-            const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index);
+            const VerticesPair pairV1V2 = VerticesPair(v1Index, v2Index);
 
             // Create a new half-edge
             Edge edge;
@@ -65,19 +65,19 @@ void HalfEdgeStructure::init() {
                 firstEdgeKey = pairV1V2;
             }
             else if (v >= 1) {
-                nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
+                nextEdges.add(Pair<VerticesPair, VerticesPair>(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
             }
             if (v == (face.faceVertices.size() - 1)) {
-                nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey));
+                nextEdges.add(Pair<VerticesPair, VerticesPair>(pairV1V2, firstEdgeKey));
             }
-            edges.insert(std::make_pair(pairV1V2, edge));
+            edges.add(Pair<VerticesPair, Edge>(pairV1V2, edge));
 
-            const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index);
+            const VerticesPair pairV2V1(v2Index, v1Index);
 
-            mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index));
+            mapEdgeToStartVertex.add(Pair<VerticesPair, uint>(pairV1V2, v1Index), true);
-            mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index));
+            mapEdgeToStartVertex.add(Pair<VerticesPair, uint>(pairV2V1, v2Index), true);
 
-            mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2));
+            mapFaceIndexToEdgeKey.add(Pair<uint, VerticesPair>(f, pairV1V2), true);
 
             auto itEdge = edges.find(pairV2V1);
             if (itEdge != edges.end()) {
@@ -87,21 +87,23 @@ void HalfEdgeStructure::init() {
                 itEdge->second.twinEdgeIndex = edgeIndex + 1;
                 edge.twinEdgeIndex = edgeIndex;
 
-                mapEdgeIndexToKey[edgeIndex] = pairV2V1;
+                mapEdgeIndexToKey.add(Pair<uint, VerticesPair>(edgeIndex, pairV2V1));
-                mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2;
+                mapEdgeIndexToKey.add(Pair<uint, VerticesPair>(edgeIndex + 1, pairV1V2));
 
                 mVertices[v1Index].edgeIndex = edgeIndex + 1;
                 mVertices[v2Index].edgeIndex = edgeIndex;
 
-                mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
+                mapEdgeToIndex.add(Pair<VerticesPair, uint>(pairV1V2, edgeIndex + 1));
-                mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
+                mapEdgeToIndex.add(Pair<VerticesPair, uint>(pairV2V1, edgeIndex));
 
                 mEdges.add(itEdge->second);
                 mEdges.add(edge);
             }
 
-            currentFaceEdges.push_back(pairV1V2);
+            currentFaceEdges.add(pairV1V2);
         }
+
+        currentFaceEdges.clear();
     }
 
     // Set next edges

src/collision/HalfEdgeStructure.h

@@ -28,7 +28,6 @@
 
 // Libraries
 #include "mathematics/mathematics.h"
-#include <vector>
 
 namespace reactphysics3d {
 
@@ -42,6 +41,9 @@ class HalfEdgeStructure {
 
     public:
 
+        using VerticesPair = Pair<uint, uint>;
+
+        /// Edge
         struct Edge {
             uint vertexIndex;       // Index of the vertex at the beginning of the edge
             uint twinEdgeIndex;     // Index of the twin edge
@@ -49,6 +51,7 @@ class HalfEdgeStructure {
             uint nextEdgeIndex;     // Index of the next edge
         };
 
+        /// Face
         struct Face {
             uint edgeIndex;             // Index of an half-edge of the face
             List<uint> faceVertices;	// Index of the vertices of the face
@@ -60,6 +63,7 @@ class HalfEdgeStructure {
             Face(List<uint> vertices) : faceVertices(vertices) {}
         };
 
+        /// Vertex
         struct Vertex {
             uint vertexPointIndex;  // Index of the vertex point in the origin vertex array
             uint edgeIndex;         // Index of one edge emanting from this vertex

src/collision/PolyhedronMesh.h

@@ -31,7 +31,6 @@
 #include "HalfEdgeStructure.h"
 #include "collision/PolygonVertexArray.h"
 #include "memory/DefaultAllocator.h"
-#include <vector>
 
 namespace reactphysics3d {
 
@@ -86,6 +85,9 @@ class PolyhedronMesh {
         /// Return a vertex
         Vector3 getVertex(uint index) const;
 
+        /// Return the number of faces
+        uint getNbFaces() const;
+
         /// Return a face normal
         Vector3 getFaceNormal(uint faceIndex) const;
 
@@ -101,6 +103,11 @@ inline uint PolyhedronMesh::getNbVertices() const {
     return mHalfEdgeStructure.getNbVertices();
 }
 
+// Return the number of faces
+inline uint PolyhedronMesh::getNbFaces() const {
+   return mHalfEdgeStructure.getNbFaces();
+}
+
 // Return a face normal
 inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const {
     assert(faceIndex < mHalfEdgeStructure.getNbFaces());

src/collision/ProxyShape.cpp

@@ -62,7 +62,7 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
  * @param ray Ray to use for the raycasting
  * @param[out] raycastInfo Result of the raycasting that is valid only if the
  *             methods returned true
- * @return True if the ray hit the collision shape
+ * @return True if the ray hits the collision shape
  */
 bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
 

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

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -33,16 +33,19 @@ using namespace reactphysics3d;
 
 // Constructor
 BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
-                    :mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
+                    :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP),
+                     mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
                      mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
                      mCollisionDetection(collisionDetection) {
 
+    PoolAllocator& poolAllocator = collisionDetection.getMemoryManager().getPoolAllocator();
+
     // Allocate memory for the array of non-static proxy shapes IDs
-    mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+    mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
     assert(mMovedShapes != nullptr);
 
     // Allocate memory for the array of potential overlapping pairs
-    mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+    mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
     assert(mPotentialPairs != nullptr);
 
 #ifdef IS_PROFILING_ACTIVE
@@ -56,25 +59,34 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
 // Destructor
 BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
 
+    // Get the memory pool allocatory
+    PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
+
     // Release the memory for the array of non-static proxy shapes IDs
-    free(mMovedShapes);
+    poolAllocator.release(mMovedShapes, mNbAllocatedMovedShapes * sizeof (int));
 
     // Release the memory for the array of potential overlapping pairs
-    free(mPotentialPairs);
+    poolAllocator.release(mPotentialPairs, mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
 }
 
 // Add a collision shape in the array of shapes that have moved in the last simulation step
 // and that need to be tested again for broad-phase overlapping.
 void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
 
+
     // Allocate more elements in the array of shapes that have moved if necessary
     if (mNbAllocatedMovedShapes == mNbMovedShapes) {
+
+        // Get the memory pool allocatory
+        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
+
+        uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
         mNbAllocatedMovedShapes *= 2;
         int* oldArray = mMovedShapes;
-        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+        mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
         assert(mMovedShapes != nullptr);
-        memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
+        std::memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
-        free(oldArray);
+        poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
     }
 
     // Store the broad-phase ID into the array of shapes that have moved
@@ -95,9 +107,13 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
     if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
             mNbAllocatedMovedShapes > 8) {
 
+        // Get the memory pool allocatory
+        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
+
+        uint oldNbAllocatedMovedShapes = mNbAllocatedMovedShapes;
         mNbAllocatedMovedShapes /= 2;
         int* oldArray = mMovedShapes;
-        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+        mMovedShapes = static_cast<int*>(poolAllocator.allocate(mNbAllocatedMovedShapes * sizeof(int)));
         assert(mMovedShapes != nullptr);
         uint nbElements = 0;
         for (uint i=0; i<mNbMovedShapes; i++) {
@@ -108,7 +124,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
         }
         mNbMovedShapes = nbElements;
         mNbNonUsedMovedShapes = 0;
-        free(oldArray);
+        poolAllocator.release(oldArray, oldNbAllocatedMovedShapes * sizeof(int));
     }
 
     // Remove the broad-phase ID from the array
@@ -266,13 +282,16 @@ void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager)
     // number of overlapping pairs
     if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
 
+        PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
+
         // Reduce the number of allocated potential overlapping pairs
         BroadPhasePair* oldPairs = mPotentialPairs;
+        uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
         mNbAllocatedPotentialPairs /= 2;
-        mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+        mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
         assert(mPotentialPairs);
         memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
-        free(oldPairs);
+        poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
     }
 }
 
@@ -289,13 +308,16 @@ void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedL
             // If we need to allocate more memory for the array of potential overlapping pairs
             if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
 
+                PoolAllocator& poolAllocator = mCollisionDetection.getMemoryManager().getPoolAllocator();
+
                 // Allocate more memory for the array of potential pairs
                 BroadPhasePair* oldPairs = mPotentialPairs;
+                uint oldNbAllocatedPotentialPairs = mNbAllocatedPotentialPairs;
                 mNbAllocatedPotentialPairs *= 2;
-                mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+                mPotentialPairs = static_cast<BroadPhasePair*>(poolAllocator.allocate(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)));
                 assert(mPotentialPairs);
                 memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
-                free(oldPairs);
+                poolAllocator.release(oldPairs, oldNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
             }
 
             // Add the new potential pair into the array of potential overlapping pairs

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"

src/collision/broadphase/DynamicAABBTree.cpp

@@ -35,7 +35,8 @@ using namespace reactphysics3d;
 const int TreeNode::NULL_TREE_NODE = -1;
 
 // Constructor
-DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
+DynamicAABBTree::DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap)
+                : mAllocator(allocator), mExtraAABBGap(extraAABBGap) {
 
     init();
 }
@@ -44,7 +45,7 @@ DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABB
 DynamicAABBTree::~DynamicAABBTree() {
 
     // Free the allocated memory for the nodes
-    free(mNodes);
+    mAllocator.release(mNodes, mNbAllocatedNodes * sizeof(TreeNode));
 }
 
 // Initialize the tree
@@ -55,9 +56,9 @@ void DynamicAABBTree::init() {
     mNbAllocatedNodes = 8;
 
     // Allocate memory for the nodes of the tree
-    mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+    mNodes = static_cast<TreeNode*>(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode)));
     assert(mNodes);
-    memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
+    std::memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
 
     // Initialize the allocated nodes
     for (int i=0; i<mNbAllocatedNodes - 1; i++) {
@@ -73,7 +74,7 @@ void DynamicAABBTree::init() {
 void DynamicAABBTree::reset() {
 
     // Free the allocated memory for the nodes
-    free(mNodes);
+    mAllocator.release(mNodes, mNbAllocatedNodes * sizeof(TreeNode));
 
     // Initialize the tree
     init();
@@ -88,12 +89,13 @@ int DynamicAABBTree::allocateNode() {
         assert(mNbNodes == mNbAllocatedNodes);
 
         // Allocate more nodes in the tree
+        uint oldNbAllocatedNodes = mNbAllocatedNodes;
         mNbAllocatedNodes *= 2;
         TreeNode* oldNodes = mNodes;
-        mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+        mNodes = static_cast<TreeNode*>(mAllocator.allocate(mNbAllocatedNodes * sizeof(TreeNode)));
         assert(mNodes);
         memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
-        free(oldNodes);
+        mAllocator.release(oldNodes, oldNbAllocatedNodes * sizeof(TreeNode));
 
         // Initialize the allocated nodes
         for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
@@ -596,7 +598,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
                                                          DynamicAABBTreeOverlapCallback& callback) const {
 
     // Create a stack with the nodes to visit
-    Stack<int, 64> stack;
+    Stack<int, 64> stack(mAllocator);
     stack.push(mRootNodeID);
 
     // While there are still nodes to visit
@@ -637,7 +639,7 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
 
     decimal maxFraction = ray.maxFraction;
 
-    Stack<int, 128> stack;
+    Stack<int, 128> stack(mAllocator);
     stack.push(mRootNodeID);
 
     // Walk through the tree from the root looking for proxy shapes

src/collision/broadphase/DynamicAABBTree.h

@@ -30,6 +30,7 @@
 #include "configuration.h"
 #include "collision/shapes/AABB.h"
 #include "body/CollisionBody.h"
+#include "memory/MemoryAllocator.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -136,6 +137,9 @@ class DynamicAABBTree {
 
         // -------------------- Attributes -------------------- //
 
+        /// Memory allocator
+        MemoryAllocator& mAllocator;
+
         /// Pointer to the memory location of the nodes of the tree
         TreeNode* mNodes;
 
@@ -203,7 +207,7 @@ class DynamicAABBTree {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
+        DynamicAABBTree(MemoryAllocator& allocator, decimal extraAABBGap = decimal(0.0));
 
         /// Destructor
         ~DynamicAABBTree();

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -212,171 +212,3 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
 
     return GJKResult::INTERPENETRATE;
 }
-
-
-// Use the GJK Algorithm to find if a point is inside a convex collision shape
-bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
-
-    Vector3 suppA;             // Support point of object A
-    Vector3 w;                 // Support point of Minkowski difference A-B
-    decimal prevDistSquare;
-
-    assert(proxyShape->getCollisionShape()->isConvex());
-
-    const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
-
-    // Support point of object B (object B is a single point)
-    const Vector3 suppB(localPoint);
-
-    // Create a simplex set
-    VoronoiSimplex simplex;
-
-    // Initial supporting direction
-    Vector3 v(1, 1, 1);
-
-    // Initialize the upper bound for the square distance
-    decimal distSquare = DECIMAL_LARGEST;
-
-    do {
-
-        // Compute the support points for original objects (without margins) A and B
-        suppA = shape->getLocalSupportPointWithoutMargin(-v);
-
-        // Compute the support point for the Minkowski difference A-B
-        w = suppA - suppB;
-
-        // Add the new support point to the simplex
-        simplex.addPoint(w, suppA, suppB);
-
-        // If the simplex is affinely dependent
-        if (simplex.isAffinelyDependent()) {
-
-            return false;
-        }
-
-        // Compute the point of the simplex closest to the origin
-        // If the computation of the closest point fail
-        if (!simplex.computeClosestPoint(v)) {
-
-            return false;
-        }
-
-        // Store and update the squared distance of the closest point
-        prevDistSquare = distSquare;
-        distSquare = v.lengthSquare();
-
-        // If the distance to the closest point doesn't improve a lot
-        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
-
-            return false;
-        }
-    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
-                                 simplex.getMaxLengthSquareOfAPoint());
-
-    // The point is inside the collision shape
-    return true;
-}
-
-// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
-/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
-/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
-bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
-
-    assert(proxyShape->getCollisionShape()->isConvex());
-
-    const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
-
-    Vector3 suppA;      // Current lower bound point on the ray (starting at ray's origin)
-    Vector3 suppB;      // Support point on the collision shape
-    const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
-    const decimal epsilon = decimal(0.0001);
-
-    // Convert the ray origin and direction into the local-space of the collision shape
-    Vector3 rayDirection = ray.point2 - ray.point1;
-
-    // If the points of the segment are two close, return no hit
-    if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
-
-    Vector3 w;
-
-    // Create a simplex set
-    VoronoiSimplex simplex;
-
-    Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
-    decimal lambda = decimal(0.0);
-    suppA = ray.point1;    // Current lower bound point on the ray (starting at ray's origin)
-    suppB = shape->getLocalSupportPointWithoutMargin(rayDirection);
-    Vector3 v = suppA - suppB;
-    decimal vDotW, vDotR;
-    decimal distSquare = v.lengthSquare();
-    int nbIterations = 0;
-
-    // GJK Algorithm loop
-    while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
-
-        // Compute the support points
-        suppB = shape->getLocalSupportPointWithoutMargin(v);
-        w = suppA - suppB;
-
-        vDotW = v.dot(w);
-
-        if (vDotW > decimal(0)) {
-
-            vDotR = v.dot(rayDirection);
-
-            if (vDotR >= -machineEpsilonSquare) {
-                return false;
-            }
-            else {
-
-                // We have found a better lower bound for the hit point along the ray
-                lambda = lambda - vDotW / vDotR;
-                suppA = ray.point1 + lambda * rayDirection;
-                w = suppA - suppB;
-                n = v;
-            }
-        }
-
-        // Add the new support point to the simplex
-        if (!simplex.isPointInSimplex(w)) {
-            simplex.addPoint(w, suppA, suppB);
-        }
-
-        // Compute the closest point
-        if (simplex.computeClosestPoint(v)) {
-
-            distSquare = v.lengthSquare();
-        }
-        else {
-            distSquare = decimal(0.0);
-        }
-
-        // If the current lower bound distance is larger than the maximum raycasting distance
-        if (lambda > ray.maxFraction) return false;
-
-        nbIterations++;
-    }
-
-    // If the origin was inside the shape, we return no hit
-    if (lambda < MACHINE_EPSILON) return false;
-
-    // Compute the closet points of both objects (without the margins)
-    Vector3 pointA;
-    Vector3 pointB;
-    simplex.computeClosestPointsOfAandB(pointA, pointB);
-
-    // A raycast hit has been found, we fill in the raycast info
-    raycastInfo.hitFraction = lambda;
-    raycastInfo.worldPoint = pointB;
-    raycastInfo.body = proxyShape->getBody();
-    raycastInfo.proxyShape = proxyShape;
-
-    if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
-        raycastInfo.worldNormal = n;
-    }
-    else {  // Degenerated normal vector, we return a zero normal vector
-        raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));
-    }
-
-    return true;
-}

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -96,12 +96,6 @@ class GJKAlgorithm {
         /// Compute a contact info if the two bounding volumes collide.
         GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
 
-        /// Use the GJK Algorithm to find if a point is inside a convex collision shape
-        bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
-
-        /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
-        bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
-
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Set the profiler

src/collision/narrowphase/GJK/VoronoiSimplex.h

@@ -28,7 +28,6 @@
 
 // Libraries
 #include "mathematics/mathematics.h"
-#include <vector>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {

src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp

@@ -121,6 +121,10 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
 				contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
 			}
 
+            if (penetrationDepth <= decimal(0.0)) {
+                return false;
+            }
+
             // Create the contact info object
             narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
                                              isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,

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;

src/collision/shapes/ConcaveMeshShape.cpp

@@ -25,12 +25,13 @@
 
 // Libraries
 #include "ConcaveMeshShape.h"
+#include "memory/MemoryManager.h"
 
 using namespace reactphysics3d;
 
 // Constructor
 ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
-                 : ConcaveShape(CollisionShapeName::TRIANGLE_MESH) {
+                 : ConcaveShape(CollisionShapeName::TRIANGLE_MESH), mDynamicAABBTree(MemoryManager::getBaseAllocator()) {
     mTriangleMesh = triangleMesh;
     mRaycastTestType = TriangleRaycastSide::FRONT;
 
@@ -155,7 +156,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 +164,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) {

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) {
 
         }

src/collision/shapes/ConvexMeshShape.cpp

@@ -30,9 +30,6 @@
 
 using namespace reactphysics3d;
 
-// TODO : Check in every collision shape that localScalling is used correctly and even with SAT
-//        algorithm (not only in getLocalSupportPoint***() methods)
-
 // Constructor to initialize with an array of 3D vertices.
 /// This method creates an internal copy of the input vertices.
 /**
@@ -58,14 +55,14 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh)
 /// runs in almost constant time.
 Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
 
-    double maxDotProduct = DECIMAL_SMALLEST;
+    decimal maxDotProduct = DECIMAL_SMALLEST;
     uint indexMaxDotProduct = 0;
 
     // For each vertex of the mesh
     for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
 
         // Compute the dot product of the current vertex
-        double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i));
+        decimal dotProduct = direction.dot(mPolyhedronMesh->getVertex(i));
 
         // If the current dot product is larger than the maximum one
         if (dotProduct > maxDotProduct) {
@@ -83,14 +80,11 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
 // Recompute the bounds of the mesh
 void ConvexMeshShape::recalculateBounds() {
 
-    // TODO : Only works if the local origin is inside the mesh
+    mMinBounds = mPolyhedronMesh->getVertex(0);
-    //        => Make it more robust (init with first vertex of mesh instead)
+    mMaxBounds = mPolyhedronMesh->getVertex(0);
-
-    mMinBounds.setToZero();
-    mMaxBounds.setToZero();
 
     // For each vertex of the mesh
-    for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
+    for (uint i=1; i<mPolyhedronMesh->getNbVertices(); i++) {
 
         if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x;
         if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x;
@@ -105,14 +99,105 @@ void ConvexMeshShape::recalculateBounds() {
     // Apply the local scaling factor
     mMaxBounds = mMaxBounds * mScaling;
     mMinBounds = mMinBounds * mScaling;
-
-    // Add the object margin to the bounds
-    mMaxBounds += Vector3(mMargin, mMargin, mMargin);
-    mMinBounds -= Vector3(mMargin, mMargin, mMargin);
 }
 
 // Raycast method with feedback information
+/// This method implements the technique in the book "Real-time Collision Detection" by
+/// Christer Ericson.
 bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
-    return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
+
-                                     ray, proxyShape, raycastInfo);
+    // Ray direction
+    Vector3 direction = ray.point2 - ray.point1;
+
+    decimal tMin = decimal(0.0);
+    decimal tMax = ray.maxFraction;
+    Vector3 currentFaceNormal;
+    bool isIntersectionFound = false;
+
+    const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure();
+
+    // For each face of the convex mesh
+    for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) {
+
+        const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f);
+        const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f);
+        const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]);
+        const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex);
+        decimal denom = faceNormal.dot(direction);
+        decimal planeD = faceNormal.dot(facePoint);
+        decimal dist = planeD -  faceNormal.dot(ray.point1);
+
+        // If ray is parallel to the face
+        if (denom == decimal(0.0)) {
+
+            // If ray is outside the clipping face, we return no intersection
+            if (dist < decimal(0.0)) return false;
+        }
+        else {
+
+            // Compute the intersection between the ray and the current face plane
+            decimal t = dist / denom;
+
+            // Update the current ray intersection by clipping it with the current face plane
+            // If the place faces the ray
+            if (denom < decimal(0.0)) {
+                // Clip the current ray intersection as it enters the convex mesh
+                if (t > tMin) {
+                    tMin = t;
+                    currentFaceNormal = faceNormal;
+                    isIntersectionFound = true;
+                }
+            }
+            else {
+                // Clip the current ray intersection as it exits the convex mesh
+                if (t < tMax) tMax = t;
+            }
+
+            // If the ray intersection with the convex mesh becomes empty, report no intersection
+            if (tMin > tMax) return false;
+        }
+    }
+
+    if (isIntersectionFound) {
+
+        // The ray intersects with the convex mesh
+        assert(tMin >= decimal(0.0));
+        assert(tMax <= ray.maxFraction);
+        assert(tMin <= tMax);
+        assert(currentFaceNormal.lengthSquare() > decimal(0.0));
+
+        // The ray intersects the three slabs, we compute the hit point
+        Vector3 localHitPoint = ray.point1 + tMin * direction;
+
+        raycastInfo.hitFraction = tMin;
+        raycastInfo.body = proxyShape->getBody();
+        raycastInfo.proxyShape = proxyShape;
+        raycastInfo.worldPoint = localHitPoint;
+        raycastInfo.worldNormal = currentFaceNormal;
+
+        return true;
+    }
+
+    return false;
 }
+
+// Return true if a point is inside the collision shape
+bool ConvexMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+
+    const HalfEdgeStructure& halfEdgeStructure = mPolyhedronMesh->getHalfEdgeStructure();
+
+    // For each face plane of the convex mesh
+    for (uint f=0; f < mPolyhedronMesh->getNbFaces(); f++) {
+
+        const HalfEdgeStructure::Face& face = halfEdgeStructure.getFace(f);
+        const Vector3 faceNormal = mPolyhedronMesh->getFaceNormal(f);
+        const HalfEdgeStructure::Vertex& faceVertex = halfEdgeStructure.getVertex(face.faceVertices[0]);
+        const Vector3 facePoint = mPolyhedronMesh->getVertex(faceVertex.vertexPointIndex);
+
+        // If the point is out of the face plane, it is outside of the convex mesh
+        if (computePointToPlaneDistance(localPoint, faceNormal, facePoint) > decimal(0.0)) return false;
+    }
+
+    return true;
+}
+

src/collision/shapes/ConvexMeshShape.h

@@ -33,9 +33,6 @@
 #include "collision/TriangleMesh.h"
 #include "collision/PolyhedronMesh.h"
 #include "collision/narrowphase/GJK/GJKAlgorithm.h"
-#include <vector>
-#include <set>
-#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -176,15 +173,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima
                         0.0, 0.0, factor * (xSquare + ySquare));
 }
 
-// Return true if a point is inside the collision shape
-inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
-                                             ProxyShape* proxyShape) const {
-
-    // Use the GJK algorithm to test if the point is inside the convex mesh
-    return proxyShape->mBody->mWorld.mCollisionDetection.
-           mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
-}
-
 // Return the number of faces of the polyhedron
 inline uint ConvexMeshShape::getNbFaces() const {
     return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces();

src/configuration.h

@@ -32,6 +32,7 @@
 #include <utility>
 #include <cstdint>
 #include "decimal.h"
+#include "containers/Pair.h"
 
 // Windows platform
 #if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__)
@@ -52,7 +53,7 @@ using uchar = unsigned char;
 using ushort = unsigned short;
 using luint = long unsigned int;
 using bodyindex = luint;
-using bodyindexpair = std::pair<bodyindex, bodyindex>;
+using bodyindexpair = Pair<bodyindex, bodyindex>;
 
 using int8 = std::int8_t;
 using uint8 = std::uint8_t;

src/containers/List.h

@@ -57,9 +57,6 @@ class List {
         /// Memory allocator
         MemoryAllocator& mAllocator;
 
-        // -------------------- Methods -------------------- //
-
-
     public:
 
         /// Class Iterator
@@ -87,14 +84,14 @@ class List {
                 Iterator() = default;
 
                 /// Constructor
-                Iterator(T* buffer, size_t index, size_t size)
+                Iterator(void* buffer, size_t index, size_t size)
-                     :mCurrentIndex(index), mBuffer(buffer), mSize(size) {
+                     :mCurrentIndex(index), mBuffer(static_cast<T*>(buffer)), mSize(size) {
 
                 }
 
                 /// Copy constructor
                 Iterator(const Iterator& it)
-                     :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) {
+                     :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.mSize) {
 
                 }
 
@@ -112,14 +109,14 @@ class List {
 
                 /// Post increment (it++)
                 Iterator& operator++() {
-                    assert(mCurrentIndex < mSize - 1);
+                    assert(mCurrentIndex < mSize);
                     mCurrentIndex++;
                     return *this;
                 }
 
                 /// Pre increment (++it)
                 Iterator operator++(int number) {
-                    assert(mCurrentIndex < mSize - 1);
+                    assert(mCurrentIndex < mSize);
                     Iterator tmp = *this;
                     mCurrentIndex++;
                     return tmp;
@@ -149,13 +146,17 @@ class List {
                         return true;
                     }
 
-                    return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]);
+                    return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[iterator.mCurrentIndex]);
                 }
 
                 /// Inequality operator (it != end())
                 bool operator!=(const Iterator& iterator) const {
                     return !(*this == iterator);
                 }
+
+                /// Frienship
+                friend class List;
+
         };
 
         // -------------------- Methods -------------------- //
@@ -229,8 +230,35 @@ class List {
             mSize++;
         }
 
-        /// Remove an element from the list at a given index
+        /// Try to find a given item of the list and return an iterator
-        void remove(uint index) {
+        /// pointing to that element if it exists in the list. Otherwise,
+        /// this method returns the end() iterator
+        Iterator find(const T& element) {
+
+            for (uint i=0; i<mSize; i++) {
+                if (element == static_cast<T*>(mBuffer)[i]) {
+                    return Iterator(mBuffer, i, mSize);
+                }
+            }
+
+            return end();
+        }
+
+        /// Look for an element in the list and remove it
+        Iterator remove(const T& element) {
+           return remove(find(element));
+        }
+
+        /// Remove an element from the list and return a iterator
+        /// pointing to the element after the removed one (or end() if none)
+        Iterator remove(const Iterator& it) {
+           assert(it.mBuffer == mBuffer);
+           return removeAt(it.mCurrentIndex);
+        }
+
+        /// Remove an element from the list at a given index and return an
+        /// iterator pointing to the element after the removed one (or end() if none)
+        Iterator removeAt(uint index) {
 
           assert(index >= 0 && index < mSize);
 
@@ -246,6 +274,9 @@ class List {
               char* src = dest + sizeof(T);
               std::memcpy(static_cast<void*>(dest), static_cast<void*>(src), (mSize - index) * sizeof(T));
           }
+
+          // Return an iterator pointing to the element after the removed one
+          return Iterator(mBuffer, index, mSize);
         }
 
         /// Append another list at the end of the current one
@@ -334,6 +365,16 @@ class List {
 
             return *this;
         }
+
+        /// Return a begin iterator
+        Iterator begin() const {
+            return Iterator(mBuffer, 0, mSize);
+        }
+
+        /// Return a end iterator
+        Iterator end() const {
+            return Iterator(mBuffer, mSize, mSize);
+        }
 };
 
 }

src/containers/Map.h

@@ -29,14 +29,19 @@
 // Libraries
 #include "memory/MemoryAllocator.h"
 #include "mathematics/mathematics_functions.h"
+#include "containers/Pair.h"
 #include <cstring>
+#include <stdexcept>
+#include <functional>
 #include <limits>
 
+
 namespace reactphysics3d {
 
 // Class Map
 /**
- * This class represents a simple generic associative map
+ * This class represents a simple generic associative map. This map is
+ * implemented with a hash table.
   */
 template<typename K, typename V>
 class Map {
@@ -48,7 +53,7 @@ class Map {
 
             size_t hashCode;			// Hash code of the entry
             int next;					// Index of the next entry
-            std::pair<K, V>* keyValue;	// Pointer to the pair with key and value
+            Pair<K, V>* keyValue;	    // Pointer to the pair with key and value
 
             /// Constructor
             Entry() {
@@ -56,6 +61,13 @@ class Map {
                 keyValue = nullptr;
             }
 
+            /// Constructor
+            Entry(size_t hashcode, int nextEntry) {
+                hashCode = hashcode;
+                next = nextEntry;
+                keyValue = nullptr;
+            }
+
             /// Destructor
             ~Entry() {
 
@@ -220,7 +232,7 @@ class Map {
             // If elements have been allocated
             if (mCapacity > 0) {
 
-                // Clear the list
+                // Clear the map
                 clear();
 
                 // Destroy the entries
@@ -242,6 +254,109 @@ class Map {
 
     public:
 
+        /// Class Iterator
+        /**
+         * This class represents an iterator for the Map
+         */
+        class Iterator {
+
+            private:
+
+                /// Array of entries
+                const Entry* mEntries;
+
+                /// Capacity of the map
+                int mCapacity;
+
+                /// Number of used entries in the map
+                int mNbUsedEntries;
+
+                /// Index of the current entry
+                int mCurrentEntry;
+
+                /// Advance the iterator
+                void advance() {
+
+                    // If we are trying to move past the end
+                    assert(mCurrentEntry < mNbUsedEntries);
+
+                    for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) {
+
+                        // If the entry is not empty
+                        if (mEntries[mCurrentEntry].keyValue != nullptr) {
+
+                           // We have found the next non empty entry
+                           return;
+                        }
+                    }
+
+                    // We have not find a non empty entry, we return an iterator to the end
+                    mCurrentEntry = mCapacity;
+                }
+
+            public:
+
+                // Iterator traits
+                using value_type = Pair<K,V>;
+                using difference_type = std::ptrdiff_t;
+                using pointer = Pair<K, V>*;
+                using reference = Pair<K,V>&;
+                using iterator_category = std::forward_iterator_tag;
+
+                /// Constructor
+                Iterator() = default;
+
+                /// Constructor
+                Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry)
+                     :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) {
+
+                }
+
+                /// Copy constructor
+                Iterator(const Iterator& it)
+                     :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) {
+
+                }
+
+                /// Deferencable
+                reference operator*() const {
+                    assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries);
+                    assert(mEntries[mCurrentEntry].keyValue != nullptr);
+                    return *(mEntries[mCurrentEntry].keyValue);
+                }
+
+                /// Deferencable
+                pointer operator->() const {
+                    assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries);
+                    assert(mEntries[mCurrentEntry].keyValue != nullptr);
+                    return mEntries[mCurrentEntry].keyValue;
+                }
+
+                /// Post increment (it++)
+                Iterator& operator++() {
+                    advance();
+                    return *this;
+                }
+
+                /// Pre increment (++it)
+                Iterator operator++(int number) {
+                    Iterator tmp = *this;
+                    advance();
+                    return tmp;
+                }
+
+                /// Equality operator (it == end())
+                bool operator==(const Iterator& iterator) const {
+                    return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries;
+                }
+
+                /// Inequality operator (it != end())
+                bool operator!=(const Iterator& iterator) const {
+                    return !(*this == iterator);
+                }
+        };
+
+
         // -------------------- Methods -------------------- //
 
         /// Constructor
@@ -277,7 +392,15 @@ class Map {
             std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
 
             // Copy the entries
-            std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
+            for (int i=0; i < mCapacity; i++) {
+
+                new (&mEntries[i]) Entry(map.mEntries[i].hashCode, map.mEntries[i].next);
+
+                if (map.mEntries[i].keyValue != nullptr) {
+                   mEntries[i].keyValue = static_cast<Pair<K,V>*>(mAllocator.allocate(sizeof(Pair<K, V>)));
+                   new (mEntries[i].keyValue) Pair<K,V>(*(map.mEntries[i].keyValue));
+                }
+            }
         }
 
         /// Destructor
@@ -307,7 +430,7 @@ class Map {
         }
 
         /// Add an element into the map
-        void add(const std::pair<K,V>& keyValue) {
+        void add(const Pair<K,V>& keyValue, bool insertIfAlreadyPresent = false) {
 
             if (mCapacity == 0) {
                 initialize(0);
@@ -325,7 +448,19 @@ class Map {
                 // If there is already an item with the same key in the map
                 if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) {
 
-                    throw std::runtime_error("The key and value pair already exists in the map");
+                    if (insertIfAlreadyPresent) {
+
+                        // Destruct the previous key/value
+                        mEntries[i].keyValue->~Pair<K, V>();
+
+                        // Copy construct the new key/value
+                        new (mEntries[i].keyValue) Pair<K,V>(keyValue);
+
+                        return;
+                    }
+                    else {
+                        throw std::runtime_error("The key and value pair already exists in the map");
+                    }
                 }
             }
 
@@ -357,14 +492,25 @@ class Map {
             assert(mEntries[entryIndex].keyValue == nullptr);
             mEntries[entryIndex].hashCode = hashCode;
             mEntries[entryIndex].next = mBuckets[bucket];
-            mEntries[entryIndex].keyValue = static_cast<std::pair<K,V>*>(mAllocator.allocate(sizeof(std::pair<K,V>)));
+            mEntries[entryIndex].keyValue = static_cast<Pair<K,V>*>(mAllocator.allocate(sizeof(Pair<K,V>)));
             assert(mEntries[entryIndex].keyValue != nullptr);
-            new (mEntries[entryIndex].keyValue) std::pair<K,V>(keyValue);
+            new (mEntries[entryIndex].keyValue) Pair<K,V>(keyValue);
             mBuckets[bucket] = entryIndex;
         }
 
+        /// Remove the element pointed by some iterator
+        /// This method returns an iterator pointing to the element after
+        /// the one that has been removed
+        Iterator remove(const Iterator& it) {
+
+            const K& key = it->first;
+            return remove(key);
+        }
+
         /// Remove the element from the map with a given key
-        bool remove(const K& key) {
+        /// This method returns an iterator pointing to the element after
+        /// the one that has been removed
+        Iterator remove(const K& key) {
 
             if (mCapacity > 0) {
 
@@ -384,8 +530,8 @@ class Map {
 
                         // Release memory for the key/value pair if any
                         if (mEntries[i].keyValue != nullptr) {
-                            mEntries[i].keyValue->~pair<K,V>();
+                            mEntries[i].keyValue->~Pair<K,V>();
-                            mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
+                            mAllocator.release(mEntries[i].keyValue, sizeof(Pair<K,V>));
                             mEntries[i].keyValue = nullptr;
                         }
                         mEntries[i].hashCode = -1;
@@ -393,15 +539,27 @@ class Map {
                         mFreeIndex = i;
                         mNbFreeEntries++;
 
-                        return true;
+                        // Find the next entry to return the iterator
+                        for (i += 1; i < mNbUsedEntries; i++) {
+
+                            // If the entry is not empty
+                            if (mEntries[i].keyValue != nullptr) {
+
+                               // We have found the next non empty entry
+                               return Iterator(mEntries, mCapacity, mNbUsedEntries, i);
+                            }
+                        }
+
+                        return end();
                     }
                 }
             }
 
-            return false;
+            // Return the end iterator
+            return end();
         }
 
-        /// Clear the list
+        /// Clear the map
         void clear() {
 
             if (mNbUsedEntries > 0) {
@@ -410,8 +568,8 @@ class Map {
                     mBuckets[i] = -1;
                     mEntries[i].next = -1;
                     if (mEntries[i].keyValue != nullptr) {
-                        mEntries[i].keyValue->~pair<K,V>();
+                        mEntries[i].keyValue->~Pair<K,V>();
-                        mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
+                        mAllocator.release(mEntries[i].keyValue, sizeof(Pair<K,V>));
                         mEntries[i].keyValue = nullptr;
                     }
                 }
@@ -434,6 +592,36 @@ class Map {
             return mCapacity;
         }
 
+        /// Try to find an item of the map given a key.
+        /// The method returns an iterator to the found item or
+        /// an iterator pointing to the end if not found
+        Iterator find(const K& key) const {
+
+            int bucket;
+            int entry = -1;
+
+            if (mCapacity > 0) {
+
+               size_t hashCode = std::hash<K>()(key);
+               bucket = hashCode % mCapacity;
+
+               for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+                   if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) {
+                       entry = i;
+                       break;
+                   }
+               }
+            }
+
+            if (entry == -1) {
+                return end();
+            }
+
+            assert(mEntries[entry].keyValue != nullptr);
+
+            return Iterator(mEntries, mCapacity, mNbUsedEntries, entry);
+        }
+
         /// Overloaded index operator
         V& operator[](const K& key) {
 
@@ -465,14 +653,31 @@ class Map {
                 throw std::runtime_error("No item with given key has been found in the map");
             }
 
-            return mEntries[entry];
+            assert(mEntries[entry].keyValue != nullptr);
+
+            return mEntries[entry].keyValue->second;
         }
 
         /// Overloaded equality operator
         bool operator==(const Map<K,V>& map) const {
 
-            // TODO : Implement this
+            if (size() != map.size()) return false;
-            return false;
+
+            for (auto it = begin(); it != end(); ++it) {
+                auto it2 = map.find(it->first);
+                if (it2 == map.end() || it2->second != it->second) {
+                    return false;
+                }
+            }
+
+            for (auto it = map.begin(); it != map.end(); ++it) {
+                auto it2 = find(it->first);
+                if (it2 == end() || it2->second != it->second) {
+                    return false;
+                }
+            }
+
+            return true;
         }
 
         /// Overloaded not equal operator
@@ -505,7 +710,15 @@ class Map {
                     std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
 
                     // Copy the entries
-                    std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
+                    for (int i=0; i < mCapacity; i++) {
+
+                        new (&mEntries[i]) Entry(map.mEntries[i].hashCode, map.mEntries[i].next);
+
+                        if (map.mEntries[i].keyValue != nullptr) {
+                           mEntries[i].keyValue = static_cast<Pair<K,V>*>(mAllocator.allocate(sizeof(Pair<K, V>)));
+                           new (mEntries[i].keyValue) Pair<K,V>(*(map.mEntries[i].keyValue));
+                        }
+                    }
 
                     mNbUsedEntries = map.mNbUsedEntries;
                     mNbFreeEntries = map.mNbFreeEntries;
@@ -515,6 +728,32 @@ class Map {
 
             return *this;
         }
+
+        /// Return a begin iterator
+        Iterator begin() const {
+
+            // If the map is empty
+            if (size() == 0) {
+
+                // Return an iterator to the end
+                return end();
+            }
+
+            // Find the first used entry
+            int entry;
+            for (entry=0; entry < mNbUsedEntries; entry++) {
+                if (mEntries[entry].keyValue != nullptr) {
+                    return Iterator(mEntries, mCapacity, mNbUsedEntries, entry);
+                }
+            }
+
+            assert(false);
+        }
+
+        /// Return a end iterator
+        Iterator end() const {
+            return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity);
+        }
 };
 
 template<typename K, typename V>

src/containers/Pair.h

@@ -0,0 +1,106 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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 REACTPHYSICS3D_PAIR_H
+#define REACTPHYSICS3D_PAIR_H
+
+// Libraries
+#include "configuration.h"
+#include "memory/MemoryAllocator.h"
+#include "containers/containers_common.h"
+#include <cstring>
+#include <iterator>
+
+namespace reactphysics3d {
+
+// Class Pair
+/**
+ * This class represents a simple generic pair
+  */
+template<typename T1, typename T2>
+class Pair {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// First element of the pair
+        T1 first;
+
+        /// Second element of the pair
+        T2 second;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Pair(const T1& item1, const T2& item2) : first(item1), second(item2) {
+
+        }
+
+        /// Copy constructor
+        Pair(const Pair<T1, T2>& pair) : first(pair.first), second(pair.second) {
+
+        }
+
+        /// Destructor
+        ~Pair() = default;
+
+        /// Overloaded equality operator
+        bool operator==(const Pair<T1, T2>& pair) const {
+            return first == pair.first && second == pair.second;
+        }
+
+        /// Overloaded not equal operator
+        bool operator!=(const Pair<T1, T2>& pair) const {
+            return !((*this) == pair);
+        }
+
+        /// Overloaded assignment operator
+        Pair<T1, T2>& operator=(const Pair<T1, T2>& pair) {
+            first = pair.first;
+            second = pair.second;
+        }
+};
+
+}
+
+// Hash function for struct VerticesPair
+namespace std {
+
+  template <typename T1, typename T2> struct hash<reactphysics3d::Pair<T1, T2>> {
+
+    size_t operator()(const reactphysics3d::Pair<T1, T2>& pair) const {
+
+        std::size_t seed = 0;
+        reactphysics3d::hash_combine<T1>(seed, pair.first);
+        reactphysics3d::hash_combine<T2>(seed, pair.second);
+
+        return seed;
+    }
+  };
+}
+
+
+#endif

src/containers/Set.h

@@ -0,0 +1,712 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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 REACTPHYSICS3D_SET_H
+#define REACTPHYSICS3D_SET_H
+
+// Libraries
+#include "memory/MemoryAllocator.h"
+#include "mathematics/mathematics_functions.h"
+#include <cstring>
+#include <stdexcept>
+#include <functional>
+#include <limits>
+
+
+namespace reactphysics3d {
+
+// Class Set
+/**
+ * This class represents a simple generic set. This set is implemented
+ * with a hash table.
+  */
+template<typename V>
+class Set {
+
+    private:
+
+        /// An entry of the set
+        struct Entry {
+
+            size_t hashCode;			// Hash code of the entry
+            int next;					// Index of the next entry
+            V* value;					// Pointer to the value stored in the entry
+
+            /// Constructor
+            Entry() {
+                next = -1;
+                value = nullptr;
+            }
+
+            /// Constructor
+            Entry(size_t hashcode, int nextEntry) {
+                hashCode = hashcode;
+                next = nextEntry;
+                value = nullptr;
+            }
+
+            /// Destructor
+            ~Entry() {
+
+                assert(value == nullptr);
+            }
+
+        };
+
+        // -------------------- Constants -------------------- //
+
+        /// Number of prime numbers in array
+        static constexpr int NB_PRIMES = 70;
+
+        /// Array of prime numbers for the size of the set
+        static const int PRIMES[NB_PRIMES];
+
+        /// Largest prime number
+        static int LARGEST_PRIME;
+
+        // -------------------- Attributes -------------------- //
+
+        /// Current number of used entries in the set
+        int mNbUsedEntries;
+
+        /// Number of free entries among the used ones
+        int mNbFreeEntries;
+
+        /// Current capacity of the set
+        int mCapacity;
+
+        /// Array with all the buckets
+        int* mBuckets;
+
+        /// Array with all the entries
+        Entry* mEntries;
+
+        /// Memory allocator
+        MemoryAllocator& mAllocator;
+
+        /// Index to the fist free entry
+        int mFreeIndex;
+
+        // -------------------- Methods -------------------- //
+
+        /// Initialize the set
+        void initialize(int capacity) {
+
+            // Compute the next larger prime size
+            mCapacity = getPrimeSize(capacity);
+
+            // Allocate memory for the buckets
+            mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+            // Allocate memory for the entries
+            mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+            // Initialize the buckets and entries
+            for (int i=0; i<mCapacity; i++) {
+
+                mBuckets[i] = -1;
+
+                // Construct the entry
+                new (&mEntries[i]) Entry();
+            }
+
+            mNbUsedEntries = 0;
+            mNbFreeEntries = 0;
+            mFreeIndex = -1;
+        }
+
+        /// Expand the capacity of the set
+        void expand(int newCapacity) {
+
+            assert(newCapacity > mCapacity);
+            assert(isPrimeNumber(newCapacity));
+
+            // Allocate memory for the buckets
+            int* newBuckets = static_cast<int*>(mAllocator.allocate(newCapacity * sizeof(int)));
+
+            // Allocate memory for the entries
+            Entry* newEntries = static_cast<Entry*>(mAllocator.allocate(newCapacity * sizeof(Entry)));
+
+            // Initialize the new buckets
+            for (int i=0; i<newCapacity; i++) {
+                newBuckets[i] = -1;
+            }
+
+            // Copy the old entries to the new allocated memory location
+            std::memcpy(newEntries, mEntries, mNbUsedEntries * sizeof(Entry));
+
+            // Construct the new entries
+            for (int i=mNbUsedEntries; i<newCapacity; i++) {
+
+                // Construct the entry
+                new (static_cast<void*>(&newEntries[i])) Entry();
+            }
+
+            // For each used entry
+            for (int i=0; i<mNbUsedEntries; i++) {
+
+                // If the entry is not free
+                if (newEntries[i].hashCode != -1) {
+
+                    // Get the corresponding bucket
+                    int bucket = newEntries[i].hashCode % newCapacity;
+
+                    newEntries[i].next = newBuckets[bucket];
+                    newBuckets[bucket] = i;
+                }
+            }
+
+            // Release previously allocated memory
+            mAllocator.release(mBuckets, mCapacity * sizeof(int));
+            mAllocator.release(mEntries, mCapacity * sizeof(Entry));
+
+            mCapacity = newCapacity;
+            mBuckets = newBuckets;
+            mEntries = newEntries;
+        }
+
+        /// Return the index of the entry with a given value or -1 if there is no entry with this value
+        int findEntry(const V& value) const {
+
+            if (mCapacity > 0) {
+
+               size_t hashCode = std::hash<V>()(value);
+               int bucket = hashCode % mCapacity;
+
+               for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+                   if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) {
+                       return i;
+                   }
+               }
+            }
+
+            return -1;
+        }
+
+        /// Return the prime number that is larger or equal to the number in parameter
+        /// for the size of the set
+        static int getPrimeSize(int number) {
+
+            // Check if the next larger prime number is in the precomputed array of primes
+            for (int i = 0; i < NB_PRIMES; i++) {
+                if (PRIMES[i] >= number) return PRIMES[i];
+            }
+
+            // Manually compute the next larger prime number
+            for (int i = (number | 1); i < std::numeric_limits<int>::max(); i+=2) {
+
+                if (isPrimeNumber(i)) {
+                    return i;
+                }
+            }
+
+            return number;
+        }
+
+        /// Clear and reset the set
+        void reset() {
+
+            // If elements have been allocated
+            if (mCapacity > 0) {
+
+                // Clear the list
+                clear();
+
+                // Destroy the entries
+                for (int i=0; i < mCapacity; i++) {
+                    mEntries[i].~Entry();
+                }
+
+                mAllocator.release(mBuckets, mCapacity * sizeof(int));
+                mAllocator.release(mEntries, mCapacity * sizeof(Entry));
+
+                mNbUsedEntries = 0;
+                mNbFreeEntries = 0;
+                mCapacity = 0;
+                mBuckets = nullptr;
+                mEntries = nullptr;
+                mFreeIndex = -1;
+            }
+        }
+
+    public:
+
+        /// Class Iterator
+        /**
+         * This class represents an iterator for the Set
+         */
+        class Iterator {
+
+            private:
+
+                /// Array of entries
+                const Entry* mEntries;
+
+                /// Capacity of the map
+                int mCapacity;
+
+                /// Number of used entries in the map
+                int mNbUsedEntries;
+
+                /// Index of the current entry
+                int mCurrentEntry;
+
+                /// Advance the iterator
+                void advance() {
+
+                    // If we are trying to move past the end
+                    assert(mCurrentEntry < mNbUsedEntries);
+
+                    for (mCurrentEntry += 1; mCurrentEntry < mNbUsedEntries; mCurrentEntry++) {
+
+                        // If the entry is not empty
+                        if (mEntries[mCurrentEntry].value != nullptr) {
+
+                           // We have found the next non empty entry
+                           return;
+                        }
+                    }
+
+                    // We have not find a non empty entry, we return an iterator to the end
+                    mCurrentEntry = mCapacity;
+                }
+
+            public:
+
+                // Iterator traits
+                using value_type = V;
+                using difference_type = std::ptrdiff_t;
+                using pointer = V*;
+                using reference = V&;
+                using iterator_category = std::forward_iterator_tag;
+
+                /// Constructor
+                Iterator() = default;
+
+                /// Constructor
+                Iterator(const Entry* entries, int capacity, int nbUsedEntries, int currentEntry)
+                     :mEntries(entries), mCapacity(capacity), mNbUsedEntries(nbUsedEntries), mCurrentEntry(currentEntry) {
+
+                }
+
+                /// Copy constructor
+                Iterator(const Iterator& it)
+                     :mEntries(it.mEntries), mCapacity(it.mCapacity), mNbUsedEntries(it.mNbUsedEntries), mCurrentEntry(it.mCurrentEntry) {
+
+                }
+
+                /// Deferencable
+                reference operator*() const {
+                    assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries);
+                    assert(mEntries[mCurrentEntry].value != nullptr);
+                    return *(mEntries[mCurrentEntry].value);
+                }
+
+                /// Deferencable
+                pointer operator->() const {
+                    assert(mCurrentEntry >= 0 && mCurrentEntry < mNbUsedEntries);
+                    assert(mEntries[mCurrentEntry].value != nullptr);
+                    return mEntries[mCurrentEntry].value;
+                }
+
+                /// Post increment (it++)
+                Iterator& operator++() {
+                    advance();
+                    return *this;
+                }
+
+                /// Pre increment (++it)
+                Iterator operator++(int number) {
+                    Iterator tmp = *this;
+                    advance();
+                    return tmp;
+                }
+
+                /// Equality operator (it == end())
+                bool operator==(const Iterator& iterator) const {
+                    return mCurrentEntry == iterator.mCurrentEntry && mEntries == iterator.mEntries;
+                }
+
+                /// Inequality operator (it != end())
+                bool operator!=(const Iterator& iterator) const {
+                    return !(*this == iterator);
+                }
+        };
+
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Set(MemoryAllocator& allocator, size_t capacity = 0)
+            : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr),
+              mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) {
+
+            // If the largest prime has not been computed yet
+            if (LARGEST_PRIME == -1) {
+
+                // Compute the largest prime number (largest map capacity)
+                LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2);
+            }
+
+            if (capacity > 0) {
+
+               initialize(capacity);
+            }
+        }
+
+        /// Copy constructor
+        Set(const Set<V>& set)
+          :mNbUsedEntries(set.mNbUsedEntries), mNbFreeEntries(set.mNbFreeEntries), mCapacity(set.mCapacity),
+           mAllocator(set.mAllocator), mFreeIndex(set.mFreeIndex) {
+
+            // Allocate memory for the buckets
+            mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+            // Allocate memory for the entries
+            mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+            // Copy the buckets
+            std::memcpy(mBuckets, set.mBuckets, mCapacity * sizeof(int));
+
+            // Copy the entries
+            for (int i=0; i < mCapacity; i++) {
+
+                new (&mEntries[i]) Entry(set.mEntries[i].hashCode, set.mEntries[i].next);
+
+                if (set.mEntries[i].value != nullptr) {
+                   mEntries[i].value = static_cast<V*>(mAllocator.allocate(sizeof(V)));
+                   new (mEntries[i].value) V(*(set.mEntries[i].value));
+                }
+            }
+        }
+
+        /// Destructor
+        ~Set() {
+
+            reset();
+        }
+
+        /// Allocate memory for a given number of elements
+        void reserve(size_t capacity) {
+
+           if (capacity <= mCapacity) return;
+
+           if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) {
+               capacity = LARGEST_PRIME;
+           }
+           else {
+               capacity = getPrimeSize(capacity);
+           }
+
+           expand(capacity);
+        }
+
+        /// Return true if the set contains a given value
+        bool contains(const V& value) const {
+            return findEntry(value) != -1;
+        }
+
+        /// Add a value into the set
+        void add(const V& value) {
+
+            if (mCapacity == 0) {
+                initialize(0);
+            }
+
+            // Compute the hash code of the value
+            size_t hashCode = std::hash<V>()(value);
+
+            // Compute the corresponding bucket index
+            int bucket = hashCode % mCapacity;
+
+            // Check if the item is already in the set
+            for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+
+                // If there is already an item with the same value in the set
+                if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) {
+
+                    return;
+                }
+            }
+
+            size_t entryIndex;
+
+            // If there are free entries to use
+            if (mNbFreeEntries > 0) {
+                assert(mFreeIndex >= 0);
+                entryIndex = mFreeIndex;
+                mFreeIndex = mEntries[entryIndex].next;
+                mNbFreeEntries--;
+            }
+            else {
+
+                // If we need to allocator more entries
+                if (mNbUsedEntries == mCapacity) {
+
+                    // Allocate more memory
+                    reserve(mCapacity * 2);
+
+                    // Recompute the bucket index
+                    bucket = hashCode % mCapacity;
+                }
+
+                entryIndex = mNbUsedEntries;
+                mNbUsedEntries++;
+            }
+
+            assert(mEntries[entryIndex].value == nullptr);
+            mEntries[entryIndex].hashCode = hashCode;
+            mEntries[entryIndex].next = mBuckets[bucket];
+            mEntries[entryIndex].value = static_cast<V*>(mAllocator.allocate(sizeof(V)));
+            assert(mEntries[entryIndex].value != nullptr);
+            new (mEntries[entryIndex].value) V(value);
+            mBuckets[bucket] = entryIndex;
+        }
+
+        /// Remove the element pointed by some iterator
+        /// This method returns an iterator pointing to the
+        /// element after the one that has been removed
+        Iterator remove(const Iterator& it) {
+
+            return remove(*it);
+        }
+
+        /// Remove the element from the set with a given value
+        /// This method returns an iterator pointing to the
+        /// element after the one that has been removed
+        Iterator remove(const V& value) {
+
+            if (mCapacity > 0) {
+
+                size_t hashcode = std::hash<V>()(value);
+                int bucket = hashcode % mCapacity;
+                int last = -1;
+                for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) {
+
+                    if (mEntries[i].hashCode == hashcode && (*mEntries[i].value) == value) {
+
+                        if (last < 0 ) {
+                           mBuckets[bucket] = mEntries[i].next;
+                        }
+                        else {
+                           mEntries[last].next = mEntries[i].next;
+                        }
+
+                        // Release memory for the value if any
+                        if (mEntries[i].value != nullptr) {
+                            mEntries[i].value->~V();
+                            mAllocator.release(mEntries[i].value, sizeof(V));
+                            mEntries[i].value = nullptr;
+                        }
+                        mEntries[i].hashCode = -1;
+                        mEntries[i].next = mFreeIndex;
+                        mFreeIndex = i;
+                        mNbFreeEntries++;
+
+                        // Find the next valid entry to return an iterator
+                        for (i += 1; i < mNbUsedEntries; i++) {
+
+                            // If the entry is not empty
+                            if (mEntries[i].value != nullptr) {
+
+                               // We have found the next non empty entry
+                               return Iterator(mEntries, mCapacity, mNbUsedEntries, i);
+                            }
+                        }
+
+                        return end();
+                    }
+                }
+            }
+
+            return end();
+        }
+
+        /// Clear the set
+        void clear() {
+
+            if (mNbUsedEntries > 0) {
+
+                for (int i=0; i < mCapacity; i++) {
+                    mBuckets[i] = -1;
+                    mEntries[i].next = -1;
+                    if (mEntries[i].value != nullptr) {
+                        mEntries[i].value->~V();
+                        mAllocator.release(mEntries[i].value, sizeof(V));
+                        mEntries[i].value = nullptr;
+                    }
+                }
+
+                mFreeIndex = -1;
+                mNbUsedEntries = 0;
+                mNbFreeEntries = 0;
+            }
+
+            assert(size() == 0);
+        }
+
+        /// Return the number of elements in the set
+        int size() const {
+            return mNbUsedEntries - mNbFreeEntries;
+        }
+
+        /// Return the capacity of the set
+        int capacity() const {
+            return mCapacity;
+        }
+
+        /// Try to find an item of the set given a key.
+        /// The method returns an iterator to the found item or
+        /// an iterator pointing to the end if not found
+        Iterator find(const V& value) const {
+
+            int bucket;
+            int entry = -1;
+
+            if (mCapacity > 0) {
+
+               size_t hashCode = std::hash<V>()(value);
+               bucket = hashCode % mCapacity;
+
+               for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+                   if (mEntries[i].hashCode == hashCode && *(mEntries[i].value) == value) {
+                       entry = i;
+                       break;
+                   }
+               }
+            }
+
+            if (entry == -1) {
+                return end();
+            }
+
+            assert(mEntries[entry].value != nullptr);
+
+            return Iterator(mEntries, mCapacity, mNbUsedEntries, entry);
+        }
+
+        /// Overloaded equality operator
+        bool operator==(const Set<V>& set) const {
+
+            if (size() != set.size()) return false;
+
+            for (auto it = begin(); it != end(); ++it) {
+                if(!set.contains(*it)) {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// Overloaded not equal operator
+        bool operator!=(const Set<V>& set) const {
+
+            return !((*this) == set);
+        }
+
+        /// Overloaded assignment operator
+        Set<V>& operator=(const Set<V>& set) {
+
+            // Check for self assignment
+            if (this != &set) {
+
+                // Reset the set
+                reset();
+
+                if (set.mCapacity > 0) {
+
+                    // Compute the next larger prime size
+                    mCapacity = getPrimeSize(set.mCapacity);
+
+                    // Allocate memory for the buckets
+                    mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+                    // Allocate memory for the entries
+                    mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+                    // Copy the buckets
+                    std::memcpy(mBuckets, set.mBuckets, mCapacity * sizeof(int));
+
+                    // Copy the entries
+                    for (int i=0; i < mCapacity; i++) {
+
+                        new (&mEntries[i]) Entry(set.mEntries[i].hashCode, set.mEntries[i].next);
+
+                        if (set.mEntries[i].value != nullptr) {
+                           mEntries[i].value = static_cast<V*>(mAllocator.allocate(sizeof(V)));
+                           new (mEntries[i].value) V(*(set.mEntries[i].value));
+                        }
+                    }
+
+                    mNbUsedEntries = set.mNbUsedEntries;
+                    mNbFreeEntries = set.mNbFreeEntries;
+                    mFreeIndex = set.mFreeIndex;
+                }
+            }
+
+            return *this;
+        }
+
+        /// Return a begin iterator
+        Iterator begin() const {
+
+            // If the map is empty
+            if (size() == 0) {
+
+                // Return an iterator to the end
+                return end();
+            }
+
+            // Find the first used entry
+            int entry;
+            for (entry=0; entry < mNbUsedEntries; entry++) {
+                if (mEntries[entry].value != nullptr) {
+                    return Iterator(mEntries, mCapacity, mNbUsedEntries, entry);
+                }
+            }
+
+            assert(false);
+        }
+
+        /// Return a end iterator
+        Iterator end() const {
+            return Iterator(mEntries, mCapacity, mNbUsedEntries, mCapacity);
+        }
+};
+
+template<typename V>
+const int Set<V>::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919,
+                             1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591,
+                             17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437,
+                             187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263,
+                             1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559};
+
+template<typename V>
+int Set<V>::LARGEST_PRIME = -1;
+
+}
+
+#endif

src/containers/Stack.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "configuration.h"
+#include "memory/MemoryAllocator.h"
 
 namespace reactphysics3d {
 
@@ -43,6 +44,9 @@ class Stack {
 
         // -------------------- Attributes -------------------- //
 
+        /// Reference to the memory allocator
+        MemoryAllocator& mAllocator;
+
         /// Initial array that contains the elements of the stack
         T mInitArray[capacity];
 
@@ -60,7 +64,8 @@ class Stack {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Stack() : mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) {
+        Stack(MemoryAllocator& allocator)
+              :mAllocator(allocator), mElements(mInitArray), mNbElements(0), mNbAllocatedElements(capacity) {
 
         }
 
@@ -71,7 +76,7 @@ class Stack {
             if (mInitArray != mElements) {
 
                 // Release the memory allocated on the heap
-                free(mElements);
+                mAllocator.release(mElements, mNbAllocatedElements * sizeof(T));
             }
         }
 
@@ -93,12 +98,13 @@ inline void Stack<T, capacity>::push(const T& element) {
     // If we need to allocate more elements
     if (mNbElements == mNbAllocatedElements) {
         T* oldElements = mElements;
+        uint oldNbAllocatedElements = mNbAllocatedElements;
         mNbAllocatedElements *= 2;
-        mElements = (T*) malloc(mNbAllocatedElements * sizeof(T));
+        mElements = static_cast<T*>(mAllocator.allocate(mNbAllocatedElements * sizeof(T)));
         assert(mElements);
         memcpy(mElements, oldElements, mNbElements * sizeof(T));
         if (oldElements != mInitArray) {
-            free(oldElements);
+            mAllocator.release(oldElements, oldNbAllocatedElements * sizeof(T));
         }
     }
 

src/containers/containers_common.h

@@ -0,0 +1,44 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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 REACTPHYSICS3D_CONTAINERS_COMMON_H
+#define REACTPHYSICS3D_CONTAINERS_COMMON_H
+
+// Libraries
+#include <cstddef>
+#include <functional>
+
+namespace reactphysics3d {
+
+/// This method is used to combine two hash values
+template <class T>
+inline void hash_combine(std::size_t& seed, const T& v) {
+    std::hash<T> hasher;
+    seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
+}
+
+}
+
+#endif

src/engine/CollisionWorld.cpp

@@ -33,8 +33,8 @@ using namespace std;
 
 // Constructor
 CollisionWorld::CollisionWorld()
-               : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0),
+               : mCollisionDetection(this, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mCurrentBodyID(0),
-                 mEventListener(nullptr) {
+                 mFreeBodiesIDs(mMemoryManager.getPoolAllocator()), mEventListener(nullptr) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -49,14 +49,11 @@ CollisionWorld::CollisionWorld()
 CollisionWorld::~CollisionWorld() {
 
     // Destroy all the collision bodies that have not been removed
-    std::set<CollisionBody*>::iterator itBodies;
+    for (int i=mBodies.size() - 1 ; i >= 0; i--) {
-    for (itBodies = mBodies.begin(); itBodies != mBodies.end(); ) {
+        destroyCollisionBody(mBodies[i]);
-         std::set<CollisionBody*>::iterator itToRemove = itBodies;
-         ++itBodies;
-        destroyCollisionBody(*itToRemove);
     }
 
-    assert(mBodies.empty());
+    assert(mBodies.size() == 0);
 }
 
 // Create a collision body and add it to the world
@@ -80,7 +77,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
     assert(collisionBody != nullptr);
 
     // Add the collision body to the world
-    mBodies.insert(collisionBody);
+    mBodies.add(collisionBody);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -102,13 +99,13 @@ 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();
 
     // Remove the collision body from the list of bodies
-    mBodies.erase(collisionBody);
+    mBodies.remove(collisionBody);
 
     // Free the object from the memory allocator
     mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody));
@@ -119,9 +116,9 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
 
     // Compute the body ID
     bodyindex bodyID;
-    if (!mFreeBodiesIDs.empty()) {
+    if (mFreeBodiesIDs.size() != 0) {
-        bodyID = mFreeBodiesIDs.back();
+        bodyID = mFreeBodiesIDs[mFreeBodiesIDs.size() - 1];
-        mFreeBodiesIDs.pop_back();
+        mFreeBodiesIDs.removeAt(mFreeBodiesIDs.size() - 1);
     }
     else {
         bodyID = mCurrentBodyID;
@@ -135,7 +132,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
 void CollisionWorld::resetContactManifoldListsOfBodies() {
 
     // For each rigid body of the world
-    for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
+    for (List<CollisionBody*>::Iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
 
         // Reset the contact manifold list of the body
         (*it)->resetContactManifoldsList();

src/engine/CollisionWorld.h

@@ -27,11 +27,9 @@
 #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"
@@ -68,13 +66,13 @@ class CollisionWorld {
         CollisionDetection mCollisionDetection;
 
         /// All the bodies (rigid and soft) of the world
-        std::set<CollisionBody*> mBodies;
+        List<CollisionBody*> mBodies;
 
         /// Current body ID
         bodyindex mCurrentBodyID;
 
         /// List of free ID for rigid bodies
-        std::vector<luint> mFreeBodiesIDs;
+        List<luint> mFreeBodiesIDs;
 
         /// Pointer to an event listener object
         EventListener* mEventListener;
@@ -109,12 +107,6 @@ class CollisionWorld {
         /// Deleted assignment operator
         CollisionWorld& operator=(const CollisionWorld& world) = delete;
 
-        /// Return an iterator to the beginning of the bodies of the physics world
-        std::set<CollisionBody*>::iterator getBodiesBeginIterator();
-
-        /// Return an iterator to the end of the bodies of the physics world
-        std::set<CollisionBody*>::iterator getBodiesEndIterator();
-
         /// Create a collision body
         CollisionBody* createCollisionBody(const Transform& transform);
 
@@ -167,22 +159,6 @@ class CollisionWorld {
         friend class ConvexMeshShape;
 };
 
-// Return an iterator to the beginning of the bodies of the physics world
-/**
- * @return An starting iterator to the set of bodies of the world
- */
-inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesBeginIterator() {
-    return mBodies.begin();
-}
-
-// Return an iterator to the end of the bodies of the physics world
-/**
- * @return An ending iterator to the set of bodies of the world
- */
-inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator() {
-    return mBodies.end();
-}
-
 // Set the collision dispatch configuration
 /// This can be used to replace default collision detection algorithms by your
 /// custom algorithm for instance.

src/engine/ConstraintSolver.h

@@ -31,8 +31,6 @@
 #include "mathematics/mathematics.h"
 #include "constraint/Joint.h"
 #include "Island.h"
-#include <map>
-#include <set>
 
 namespace reactphysics3d {
 

src/engine/ContactSolver.h

@@ -32,8 +32,6 @@
 #include "constraint/Joint.h"
 #include "collision/ContactManifold.h"
 #include "Island.h"
-#include <map>
-#include <set>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {

src/engine/DynamicsWorld.cpp

@@ -44,7 +44,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
                 mContactSolver(mMemoryManager),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
-                mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
+                mIsSleepingEnabled(SLEEPING_ENABLED), mRigidBodies(mMemoryManager.getPoolAllocator()),
+                mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
                 mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
                 mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
                 mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
@@ -67,19 +68,13 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
 DynamicsWorld::~DynamicsWorld() {
 
     // Destroy all the joints that have not been removed
-    std::set<Joint*>::iterator itJoints;
+    for (int i=mJoints.size() - 1; i >= 0; i--) {
-    for (itJoints = mJoints.begin(); itJoints != mJoints.end();) {
+        destroyJoint(mJoints[i]);
-        std::set<Joint*>::iterator itToRemove = itJoints;
-        ++itJoints;
-        destroyJoint(*itToRemove);
     }
 
     // Destroy all the rigid bodies that have not been removed
-    std::set<RigidBody*>::iterator itRigidBodies;
+    for (int i=mRigidBodies.size() - 1; i >= 0; i--) {
-    for (itRigidBodies = mRigidBodies.begin(); itRigidBodies != mRigidBodies.end();) {
+        destroyRigidBody(mRigidBodies[i]);
-        std::set<RigidBody*>::iterator itToRemove = itRigidBodies;
-        ++itRigidBodies;
-        destroyRigidBody(*itToRemove);
     }
 
     assert(mJoints.size() == 0);
@@ -257,7 +252,7 @@ void DynamicsWorld::initVelocityArrays() {
 
     // Initialize the map of body indexes in the velocity arrays
     uint i = 0;
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
         mSplitLinearVelocities[i].setToZero();
         mSplitAngularVelocities[i].setToZero();
@@ -388,7 +383,7 @@ void DynamicsWorld::solvePositionCorrection() {
     PROFILE("DynamicsWorld::solvePositionCorrection()", &mProfiler);
 
     // Do not continue if there is no constraints
-    if (mJoints.empty()) return;
+    if (mJoints.size() == 0) return;
 
     // For each island of the world
     for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
@@ -423,8 +418,8 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
     assert(rigidBody != nullptr);
 
     // Add the rigid body to the physics world
-    mBodies.insert(rigidBody);
+    mBodies.add(rigidBody);
-    mRigidBodies.insert(rigidBody);
+    mRigidBodies.add(rigidBody);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -446,7 +441,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;
@@ -461,8 +456,8 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     rigidBody->~RigidBody();
 
     // Remove the rigid body from the list of rigid bodies
-    mBodies.erase(rigidBody);
+    mBodies.remove(rigidBody);
-    mRigidBodies.erase(rigidBody);
+    mRigidBodies.remove(rigidBody);
 
     // Free the object from the memory allocator
     mMemoryManager.release(MemoryManager::AllocationType::Pool, rigidBody, sizeof(RigidBody));
@@ -536,7 +531,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
     }
 
     // Add the joint into the world
-    mJoints.insert(newJoint);
+    mJoints.add(newJoint);
 
     // Add the joint into the joint list of the bodies involved in the joint
     addJointToBody(newJoint);
@@ -565,7 +560,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
     joint->getBody2()->setIsSleeping(false);
 
     // Remove the joint from the world
-    mJoints.erase(joint);
+    mJoints.remove(joint);
 
     // Remove the joint from the joint list of the bodies involved in the joint
     joint->mBody1->removeJointFromJointsList(mMemoryManager, joint);
@@ -622,11 +617,11 @@ void DynamicsWorld::computeIslands() {
     int nbContactManifolds = 0;
 
     // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
         int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
         nbContactManifolds += nbBodyManifolds;
     }
-    for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
+    for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
         (*it)->mIsAlreadyInIsland = false;
     }
 
@@ -636,7 +631,7 @@ void DynamicsWorld::computeIslands() {
                                                                                       nbBytesStack));
 
     // For each rigid body of the world
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
         RigidBody* body = *it;
 
@@ -818,7 +813,7 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
     if (!mIsSleepingEnabled) {
 
         // For each body of the world
-        std::set<RigidBody*>::iterator it;
+        List<RigidBody*>::Iterator it;
         for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
             // Wake up the rigid body
@@ -828,12 +823,12 @@ 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;
+    Map<Pair<uint, uint>, OverlappingPair*>::Iterator it;
     for (it = mCollisionDetection.mOverlappingPairs.begin();
          it != mCollisionDetection.mOverlappingPairs.end(); ++it) {
 
@@ -845,7 +840,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();
         }

src/engine/DynamicsWorld.h

@@ -66,10 +66,10 @@ class DynamicsWorld : public CollisionWorld {
         bool mIsSleepingEnabled;
 
         /// All the rigid bodies of the physics world
-        std::set<RigidBody*> mRigidBodies;
+        List<RigidBody*> mRigidBodies;
 
         /// All the joints of the world
-        std::set<Joint*> mJoints;
+        List<Joint*> mJoints;
 
         /// Gravity vector of the world
         Vector3 mGravity;
@@ -215,12 +215,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Return the number of joints in the world
         uint getNbJoints() const;
 
-        /// Return an iterator to the beginning of the rigid bodies of the physics world
-        std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
-
-        /// Return an iterator to the end of the rigid bodies of the physics world
-        std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
-
         /// Return true if the sleeping technique is enabled
         bool isSleepingEnabled() const;
 
@@ -249,7 +243,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 -------------------- //
 
@@ -260,7 +254,7 @@ class DynamicsWorld : public CollisionWorld {
 inline void DynamicsWorld::resetBodiesForceAndTorque() {
 
     // For each body of the world
-    std::set<RigidBody*>::iterator it;
+    List<RigidBody*>::Iterator it;
     for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
         (*it)->mExternalForce.setToZero();
         (*it)->mExternalTorque.setToZero();
@@ -370,22 +364,6 @@ inline uint DynamicsWorld::getNbJoints() const {
     return mJoints.size();
 }
 
-// Return an iterator to the beginning of the bodies of the physics world
-/**
- * @return Starting iterator of the set of rigid bodies
- */
-inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
-    return mRigidBodies.begin();
-}
-
-// Return an iterator to the end of the bodies of the physics world
-/**
- * @return Ending iterator of the set of rigid bodies
- */
-inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator() {
-    return mRigidBodies.end();
-}
-
 // Return true if the sleeping technique is enabled
 /**
  * @return True if the sleeping technique is enabled and false otherwise

src/engine/OverlappingPair.cpp

@@ -28,14 +28,17 @@
 #include "OverlappingPair.h"
 #include "collision/ContactManifoldInfo.h"
 #include "collision/NarrowPhaseInfo.h"
+#include "containers/containers_common.h"
 
 using namespace reactphysics3d;
 
+
 // Constructor
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator)
                 : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr),
-                  mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) {
+                  mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
+                  mLastFrameCollisionInfos(mPersistentAllocator) {
     
 }         
 
@@ -150,7 +153,8 @@ void OverlappingPair::reducePotentialContactManifolds() {
 void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) {
 
     // Try to get the corresponding last frame collision info
-    auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2));
+    const ShapeIdPair shapeIdPair(shapeId1, shapeId2);
+    auto it = mLastFrameCollisionInfos.find(shapeIdPair);
 
     // If there is no collision info for those two shapes already
     if (it == mLastFrameCollisionInfos.end()) {
@@ -160,9 +164,7 @@ void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2)
                                                 LastFrameCollisionInfo();
 
         // Add it into the map of collision infos
-        std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it;
+        mLastFrameCollisionInfos.add(Pair<ShapeIdPair, LastFrameCollisionInfo*>(shapeIdPair, collisionInfo));
-        auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo));
-        assert(ret.second);
     }
     else {
 
@@ -185,7 +187,7 @@ void OverlappingPair::clearObsoleteLastFrameCollisionInfos() {
             it->second->~LastFrameCollisionInfo();
             mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
 
-            mLastFrameCollisionInfos.erase(it++);
+            it = mLastFrameCollisionInfos.remove(it);
         }
         else {
             ++it;

src/engine/OverlappingPair.h

@@ -30,14 +30,13 @@
 #include "collision/ContactManifoldSet.h"
 #include "collision/ProxyShape.h"
 #include "collision/shapes/CollisionShape.h"
-#include <map>
+#include "containers/Map.h"
+#include "containers/Pair.h"
+#include "containers/containers_common.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
-// Type for the overlapping pair ID
-using overlappingpairid = std::pair<uint, uint>;
-
 // Structure LastFrameCollisionInfo
 /**
  * This structure contains collision info about the last frame.
@@ -95,6 +94,11 @@ struct LastFrameCollisionInfo {
  */
 class OverlappingPair {
 
+    public:
+
+        using OverlappingPairId = Pair<uint, uint>;
+        using ShapeIdPair = Pair<uint, uint>;
+
     private:
 
         // -------------------- Attributes -------------------- //
@@ -102,13 +106,6 @@ class OverlappingPair {
         /// Set of persistent contact manifolds
         ContactManifoldSet mContactManifoldSet;
 
-        /// Temporal coherence collision data for each overlapping collision shapes of this pair.
-        /// Temporal coherence data store collision information about the last frame.
-        /// If two convex shapes overlap, we have a single collision data but if one shape is concave,
-        /// we might have collision data for several overlapping triangles. The key in the map is the
-        /// shape Ids of the two collision shapes.
-        std::map<std::pair<uint, uint>, LastFrameCollisionInfo*> mLastFrameCollisionInfos;
-
         /// Linked-list of potential contact manifold
         ContactManifoldInfo* mPotentialContactManifolds;
 
@@ -118,6 +115,13 @@ class OverlappingPair {
         /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
         MemoryAllocator& mTempMemoryAllocator;
 
+        /// Temporal coherence collision data for each overlapping collision shapes of this pair.
+        /// Temporal coherence data store collision information about the last frame.
+        /// If two convex shapes overlap, we have a single collision data but if one shape is concave,
+        /// we might have collision data for several overlapping triangles. The key in the map is the
+        /// shape Ids of the two collision shapes.
+        Map<ShapeIdPair, LastFrameCollisionInfo*> mLastFrameCollisionInfos;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -142,7 +146,7 @@ class OverlappingPair {
         ProxyShape* getShape2() const;
 
         /// Return the last frame collision info
-        LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds);
+        LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds);
 
         /// Return the a reference to the contact manifold set
         const ContactManifoldSet& getContactManifoldSet();
@@ -193,7 +197,7 @@ class OverlappingPair {
         void makeLastFrameCollisionInfosObsolete();
 
         /// Return the pair of bodies index
-        static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
+        static OverlappingPairId computeID(ProxyShape* shape1, ProxyShape* shape2);
 
         /// Return the pair of bodies index of the pair
         static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
@@ -219,8 +223,8 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta
 }
 
 // Return the last frame collision info for a given shape id or nullptr if none is found
-inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds) {
+inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeIdPair& shapeIds) {
-    std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds);
+    Map<ShapeIdPair, LastFrameCollisionInfo*>::Iterator it = mLastFrameCollisionInfos.find(shapeIds);
     if (it != mLastFrameCollisionInfos.end()) {
         return it->second;
     }
@@ -240,13 +244,13 @@ inline void OverlappingPair::makeContactsObsolete() {
 }
 
 // Return the pair of bodies index
-inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
+inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
     assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0);
 
     // Construct the pair of body index
-    overlappingpairid pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ?
+    OverlappingPairId pairID = shape1->mBroadPhaseID < shape2->mBroadPhaseID ?
-                             std::make_pair(shape1->mBroadPhaseID, shape2->mBroadPhaseID) :
+                             OverlappingPairId(shape1->mBroadPhaseID, shape2->mBroadPhaseID) :
-                             std::make_pair(shape2->mBroadPhaseID, shape1->mBroadPhaseID);
+                             OverlappingPairId(shape2->mBroadPhaseID, shape1->mBroadPhaseID);
     assert(pairID.first != pairID.second);
     return pairID;
 }
@@ -257,8 +261,8 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
 
     // Construct the pair of body index
     bodyindexpair indexPair = body1->getID() < body2->getID() ?
-                                 std::make_pair(body1->getID(), body2->getID()) :
+                                 bodyindexpair(body1->getID(), body2->getID()) :
-                                 std::make_pair(body2->getID(), body1->getID());
+                                 bodyindexpair(body2->getID(), body1->getID());
     assert(indexPair.first != indexPair.second);
     return indexPair;
 }
@@ -296,7 +300,7 @@ inline void OverlappingPair::reduceContactManifolds() {
 
 // Return the last frame collision info for a given pair of shape ids
 inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const {
-    return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2));
+    return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)];
 }
 
 }

src/mathematics/Ray.h

@@ -44,10 +44,10 @@ struct Ray {
 
         // -------------------- Attributes -------------------- //
 
-        /// First point of the ray (origin)
+        /// First point of the ray (origin) in world-space
         Vector3 point1;
 
-        /// Second point of the ray
+        /// Second point of the ray in world-space
         Vector3 point2;
 
         /// Maximum fraction value

src/mathematics/Vector2.cpp

@@ -25,7 +25,6 @@
 
 // Libraries
 #include "Vector2.h"
-#include <vector>
 
 // Namespaces
 using namespace reactphysics3d;

src/mathematics/Vector3.cpp

@@ -26,7 +26,6 @@
 // Libraries
 #include "Vector3.h"
 #include <iostream>
-#include <vector>
 
 // Namespaces
 using namespace reactphysics3d;

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>

src/mathematics/mathematics_functions.cpp

@@ -28,7 +28,6 @@
 #include "Vector3.h"
 #include "Vector2.h"
 #include <cassert>
-#include <vector>
 
 using namespace reactphysics3d;
 
@@ -226,30 +225,26 @@ List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V
                                                            const List<Vector3>& planesPoints,
                                                            const List<Vector3>& planesNormals,
                                                            MemoryAllocator& allocator) {
-
     assert(planesPoints.size() == planesNormals.size());
 
-    List<Vector3> list1(allocator, 2);
+    List<Vector3> inputVertices(allocator, 2);
-    List<Vector3> list2(allocator, 2);
+    List<Vector3> outputVertices(allocator, 2);
 
-    List<Vector3>* inputVertices = &list1;
+    inputVertices.add(segA);
-    List<Vector3>* outputVertices = &list2;
+    inputVertices.add(segB);
-
-    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->size() == 0) 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& v1 = inputVertices[0];
-        Vector3& v2 = (*inputVertices)[1];
+        Vector3& v2 = inputVertices[1];
 
         decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
         decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -264,40 +259,39 @@ List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V
                 decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
 
                 if (t >= decimal(0) && t <= decimal(1.0)) {
-                    outputVertices->add(v1 + t * (v2 - v1));
+                    outputVertices.add(v1 + t * (v2 - v1));
                 }
                 else {
-                    outputVertices->add(v2);
+                    outputVertices.add(v2);
                 }
             }
             else {
-                outputVertices->add(v1);
+                outputVertices.add(v1);
             }
 
             // Add the second vertex
-            outputVertices->add(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->add(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->add(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
@@ -307,75 +301,73 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
 
     assert(planesPoints.size() == planesNormals.size());
 
-    uint nbMaxElements = polygonVertices.size() + planesPoints.size();
+        uint nbMaxElements = polygonVertices.size() + planesPoints.size();
-    List<Vector3> list1(allocator, nbMaxElements);
+        List<Vector3> inputVertices(allocator, nbMaxElements);
-    List<Vector3> list2(allocator, nbMaxElements);
+        List<Vector3> outputVertices(allocator, nbMaxElements);
 
-    const List<Vector3>* inputVertices = &polygonVertices;
+        inputVertices.addRange(polygonVertices);
-    List<Vector3>* outputVertices = &list2;
 
-    // For each clipping plane
+        // For each clipping plane
-    for (uint p=0; p<planesPoints.size(); p++) {
+        for (uint p=0; p<planesPoints.size(); p++) {
 
-        outputVertices->clear();
+            outputVertices.clear();
 
-        uint nbInputVertices = inputVertices->size();
+            uint nbInputVertices = inputVertices.size();
-        uint vStart = nbInputVertices - 1;
+            uint vStart = nbInputVertices - 1;
 
-        // For each edge of the polygon
+            // For each edge of the polygon
-        for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
+            for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
 
-            const Vector3& v1 = (*inputVertices)[vStart];
+                Vector3& v1 = inputVertices[vStart];
-            const Vector3& v2 = (*inputVertices)[vEnd];
+                Vector3& v2 = inputVertices[vEnd];
 
-            decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
+                decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
-            decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
+                decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
 
-            // If the second vertex is in front of the clippling plane
+                // If the second vertex is in front of the clippling plane
-            if (v2DotN >= decimal(0.0)) {
+                if (v2DotN >= decimal(0.0)) {
 
-                // If the first vertex is not in front of the clippling plane
+                    // If the first vertex is not in front of the clippling plane
-                if (v1DotN < decimal(0.0)) {
+                    if (v1DotN < decimal(0.0)) {
 
-                    // The second point we keep is the intersection between the segment v1, v2 and the clipping plane
+                        // The second 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]);
+                        decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
 
-                    if (t >= decimal(0) && t <= decimal(1.0)) {
+                        if (t >= decimal(0) && t <= decimal(1.0)) {
-                        outputVertices->add(v1 + t * (v2 - v1));
+                            outputVertices.add(v1 + t * (v2 - v1));
+                        }
+                        else {
+                            outputVertices.add(v2);
+                        }
                     }
-                    else {
+
-                        outputVertices->add(v2);
+                    // Add the second vertex
+                    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)) {
+
+                        // 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.add(v1 + t * (v2 - v1));
+                        }
+                        else {
+                            outputVertices.add(v1);
+                        }
                     }
                 }
 
-                // Add the second vertex
+                vStart = vEnd;
-                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)) {
-
-                    // 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->add(v1 + t * (v2 - v1));
-                    }
-                    else {
-                        outputVertices->add(v1);
-                    }
-                }
             }
 
-            vStart = vEnd;
+            inputVertices = outputVertices;
         }
 
-        inputVertices = outputVertices;
+        return outputVertices;
-        outputVertices = p % 2 == 0 ? &list1 : &list2;
-    }
-
-    return *outputVertices;
 }
 
 // Project a point onto a plane that is given by a point and its unit length normal
@@ -383,6 +375,11 @@ Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector
 	return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal;
 }
 
+// Return the distance between a point and a plane (the plane normal must be normalized)
+decimal reactphysics3d::computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint) {
+    return planeNormal.dot(point - planePoint);
+}
+
 // Return true if the given number is prime
 bool reactphysics3d::isPrimeNumber(int number) {
 

src/mathematics/mathematics_functions.h

@@ -32,7 +32,6 @@
 #include <algorithm>
 #include <cassert>
 #include <cmath>
-#include <vector>
 #include "containers/List.h"
 
 /// ReactPhysics3D namespace
@@ -124,6 +123,9 @@ List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const
 /// Project a point onto a plane that is given by a point and its unit length normal
 Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
 
+/// Return the distance between a point and a plane (the plane normal must be normalized)
+decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
+
 /// Return true if the given number is prime
 bool isPrimeNumber(int number);
 

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;

test/main.cpp

@@ -41,6 +41,7 @@
 #include "tests/collision/TestTriangleVertexArray.h"
 #include "tests/containers/TestList.h"
 #include "tests/containers/TestMap.h"
+#include "tests/containers/TestSet.h"
 
 using namespace reactphysics3d;
 
@@ -52,6 +53,7 @@ int main() {
 
     testSuite.addTest(new TestList("List"));
     testSuite.addTest(new TestMap("Map"));
+    testSuite.addTest(new TestSet("Set"));
 
     // ---------- Mathematics tests ---------- //
 

test/tests/collision/TestDynamicAABBTree.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "Test.h"
 #include "collision/broadphase/DynamicAABBTree.h"
-#include <vector>
+#include "memory/MemoryManager.h"
 
 /// Reactphysics3D namespace
 namespace reactphysics3d {
@@ -113,7 +113,7 @@ class TestDynamicAABBTree : public Test {
             // ------------ Create tree ---------- //
 
             // Dynamic AABB Tree
-            DynamicAABBTree tree;
+            DynamicAABBTree tree(MemoryManager::getBaseAllocator());
 			
 #ifdef IS_PROFILING_ACTIVE
 			/// Pointer to the profiler
@@ -169,7 +169,7 @@ class TestDynamicAABBTree : public Test {
             // ------------- Create tree ----------- //
 
             // Dynamic AABB Tree
-            DynamicAABBTree tree;
+            DynamicAABBTree tree(MemoryManager::getBaseAllocator());
 
 #ifdef IS_PROFILING_ACTIVE
 			/// Pointer to the profiler
@@ -368,7 +368,7 @@ class TestDynamicAABBTree : public Test {
             // ------------- Create tree ----------- //
 
             // Dynamic AABB Tree
-            DynamicAABBTree tree;
+            DynamicAABBTree tree(MemoryManager::getBaseAllocator());
 
 #ifdef IS_PROFILING_ACTIVE
 			/// Pointer to the profiler

test/tests/collision/TestPointInside.h

@@ -59,12 +59,17 @@ class TestPointInside : public Test {
         CollisionBody* mCylinderBody;
         CollisionBody* mCompoundBody;
 
+        Vector3 mConvexMeshCubeVertices[8];
+        int mConvexMeshCubeIndices[24];
+        PolygonVertexArray* mConvexMeshPolygonVertexArray;
+        PolyhedronMesh* mConvexMeshPolyhedronMesh;
+        PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces;
+
         // Collision shapes
         BoxShape* mBoxShape;
         SphereShape* mSphereShape;
         CapsuleShape* mCapsuleShape;
         ConvexMeshShape* mConvexMeshShape;
-        ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo;
 
         // Transform
         Transform mBodyTransform;
@@ -76,10 +81,7 @@ class TestPointInside : public Test {
         ProxyShape* mBoxProxyShape;
         ProxyShape* mSphereProxyShape;
         ProxyShape* mCapsuleProxyShape;
-        ProxyShape* mConeProxyShape;
         ProxyShape* mConvexMeshProxyShape;
-        ProxyShape* mConvexMeshProxyShapeEdgesInfo;
-        ProxyShape* mCylinderProxyShape;
 
     public :
 
@@ -104,6 +106,7 @@ class TestPointInside : public Test {
             mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform);
             mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform);
             mCylinderBody = mWorld->createCollisionBody(mBodyTransform);
+            mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform);
             mCompoundBody = mWorld->createCollisionBody(mBodyTransform);
 
             // Collision shape transform
@@ -121,51 +124,44 @@ class TestPointInside : public Test {
             mSphereShape = new SphereShape(3);
             mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform);
 
-            mCapsuleShape = new CapsuleShape(2, 10);
+            mCapsuleShape = new CapsuleShape(3, 10);
             mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
 
-            // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again
+            mConvexMeshCubeVertices[0] = Vector3(-2, -3, 4);
-            /*mConvexMeshShape = new ConvexMeshShape(0.0);             // Box of dimension (2, 3, 4)
+            mConvexMeshCubeVertices[1] = Vector3(2, -3, 4);
-            mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
+            mConvexMeshCubeVertices[2] = Vector3(2, -3, -4);
-            mConvexMeshShape->addVertex(Vector3(2, -3, -4));
+            mConvexMeshCubeVertices[3] = Vector3(-2, -3, -4);
-            mConvexMeshShape->addVertex(Vector3(2, -3, 4));
+            mConvexMeshCubeVertices[4] = Vector3(-2, 3, 4);
-            mConvexMeshShape->addVertex(Vector3(-2, -3, 4));
+            mConvexMeshCubeVertices[5] = Vector3(2, 3, 4);
-            mConvexMeshShape->addVertex(Vector3(-2, 3, -4));
+            mConvexMeshCubeVertices[6] = Vector3(2, 3, -4);
-            mConvexMeshShape->addVertex(Vector3(2, 3, -4));
+            mConvexMeshCubeVertices[7] = Vector3(-2, 3, -4);
-            mConvexMeshShape->addVertex(Vector3(2, 3, 4));
-            mConvexMeshShape->addVertex(Vector3(-2, 3, 4));
-            mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform);
 
-            mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0);
+            mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4));
+            mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4));
+            mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4));
+            mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4));
+            mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4));
+            mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3;
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4));
+
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4));
+            mConvexMeshPolygonFaces = new PolygonVertexArray::PolygonFace[6];
-            mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4));
+            PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces;
-            mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1);
+            for (int f = 0; f < 6; f++) {
-            mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2);
+                face->indexBase = f * 4;
-            mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3);
+                face->nbVertices = 4;
-            mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3);
+                face++;
-            mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5);
+            }
-            mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6);
+            mConvexMeshPolygonVertexArray = new PolygonVertexArray(8, &(mConvexMeshCubeVertices[0]), sizeof(Vector3),
-            mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7);
+                    &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces,
-            mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7);
+                    PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
-            mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4);
+                    PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
-            mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5);
+            mConvexMeshPolyhedronMesh = new PolyhedronMesh(mConvexMeshPolygonVertexArray);
-            mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6);
+            mConvexMeshShape = new ConvexMeshShape(mConvexMeshPolyhedronMesh);
-            mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7);
+            Transform convexMeshTransform(Vector3(10, 0, 0), Quaternion::identity());
-            mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true);
+            mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform);
-            mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
-                                                                     mConvexMeshShapeBodyEdgesInfo,
-                                                                     mShapeTransform);
-            */
 
             // Compound shape is a capsule and a sphere
             Vector3 positionShape2(Vector3(4, 2, -3));
-            Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
+            Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 * PI / 8, 1.5 * PI/ 3, PI / 13);
             Transform shapeTransform2(positionShape2, orientationShape2);
             mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
             mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
@@ -177,8 +173,10 @@ class TestPointInside : public Test {
             delete mBoxShape;
             delete mSphereShape;
             delete mCapsuleShape;
-            //delete mConvexMeshShape;
+            delete mConvexMeshShape;
-            //delete mConvexMeshShapeBodyEdgesInfo;
+            delete mConvexMeshPolygonFaces;
+            delete mConvexMeshPolygonVertexArray;
+            delete mConvexMeshPolyhedronMesh;
         }
 
         /// Run the tests
@@ -328,24 +326,24 @@ class TestPointInside : public Test {
             test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4)));
             test(mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5)));
 
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6)));
-            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
+            test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7)));
 
             // Tests with ProxyCapsuleShape
             test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
@@ -375,33 +373,30 @@ class TestPointInside : public Test {
             test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4)));
             test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5)));
 
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -13.1, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 13.1, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 5, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 5, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, 2.6)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, 5, -2.7)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -3.1)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -5, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -5, 0)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, 2.6)));
-            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
+            test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.5, -5, -2.7)));
         }
 
         /// Test the ProxyConvexMeshShape::testPointInside() and
         /// CollisionBody::testPointInside() methods
         void testConvexMesh() {
 
-            // ----- Tests without using edges information ----- //
-
-            /*
             // Tests with CollisionBody
             test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
             test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
@@ -453,68 +448,12 @@ class TestPointInside : public Test {
             test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
             test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
             test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
-
-            // ----- Tests using edges information ----- //
-
-            // Tests with CollisionBody
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
-            test(mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
-
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
-            test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
-
-            // Tests with ProxyConvexMeshShape
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
-            test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
-
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
-            test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
-            */
         }
 
         /// Test the CollisionBody::testPointInside() method
         void testCompound() {
 
             // Points on the capsule
-            // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now
             test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
             test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
             test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));

test/tests/collision/TestRaycast.h

@@ -211,29 +211,28 @@ class TestRaycast : public Test {
             mCapsuleShape = new CapsuleShape(2, 5);
             mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
 
-            // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again
+            mPolyhedronVertices[0] = Vector3(-2, -3, 4);
-            // Box of extents (2, 3, 4)
+            mPolyhedronVertices[1] = Vector3(2, -3, 4);
-            mPolyhedronVertices[0] = Vector3(-2, -3, -4);
+            mPolyhedronVertices[2] = Vector3(2, -3, -4);
-            mPolyhedronVertices[1] = Vector3(2, -3, -4);
+            mPolyhedronVertices[3] = Vector3(-2, -3, -4);
-            mPolyhedronVertices[2] = Vector3(2, -3, 4);
+            mPolyhedronVertices[4] = Vector3(-2, 3, 4);
-            mPolyhedronVertices[3] = Vector3(-2, -3, 4);
+            mPolyhedronVertices[5] = Vector3(2, 3, 4);
-            mPolyhedronVertices[4] = Vector3(-2, 3, -4);
+            mPolyhedronVertices[6] = Vector3(2, 3, -4);
-            mPolyhedronVertices[5] = Vector3(2, 3, -4);
+            mPolyhedronVertices[7] = Vector3(-2, 3, -4);
-            mPolyhedronVertices[6] = Vector3(2, 3, 4);
-            mPolyhedronVertices[7] = Vector3(-2, 3, 4);
 
-            mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3;
+            mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 3; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 1;
-            mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2;
+            mPolyhedronIndices[4] = 4; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 7;
-            mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1;
+            mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 1; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 4;
-            mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4;
+            mPolyhedronIndices[12] = 1; mPolyhedronIndices[13] = 2; mPolyhedronIndices[14] = 6; mPolyhedronIndices[15] = 5;
-            mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7;
+            mPolyhedronIndices[16] = 2; mPolyhedronIndices[17] = 3; mPolyhedronIndices[18] = 7; mPolyhedronIndices[19] = 6;
-            mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7;
+            mPolyhedronIndices[20] = 0; mPolyhedronIndices[21] = 4; mPolyhedronIndices[22] = 7; mPolyhedronIndices[23] = 3;
 
             // Polygon faces descriptions for the polyhedron
-            for (int f=0; f < 8; f++) {
+            PolygonVertexArray::PolygonFace* face = mPolygonFaces;
-                PolygonVertexArray::PolygonFace& face = mPolygonFaces[f];
+            for (int f = 0; f < 6; f++) {
-                face.indexBase = f * 4;
+                face->indexBase = f * 4;
-                face.nbVertices = 4;
+                face->nbVertices = 4;
+                face++;
             }
 
             // Create the polygon vertex array
@@ -1304,12 +1303,14 @@ class TestRaycast : public Test {
             Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4);
             Ray ray(point1, point2);
             Vector3 hitPoint = mLocalShapeToWorld * Vector3(1, 2, 4);
+            Transform inverse = mLocalShapeToWorld.getInverse();
 
             mCallback.shapeToTest = mConvexMeshProxyShape;
 
             // CollisionWorld::raycast()
             mCallback.reset();
             mWorld->raycast(ray, &mCallback);
+            Vector3 localTest = inverse * mCallback.raycastInfo.worldPoint;
             test(mCallback.isHit);
             test(mCallback.raycastInfo.body == mConvexMeshBody);
             test(mCallback.raycastInfo.proxyShape == mConvexMeshProxyShape);

test/tests/containers/TestList.h

@@ -62,9 +62,10 @@ class TestList : public Test {
             testAddRemoveClear();
             testAssignment();
             testIndexing();
+            testFind();
             testEquality();
             testReserve();
-            testIteration();
+            testIterators();
         }
 
         void testConstructors() {
@@ -142,27 +143,52 @@ class TestList : public Test {
             list3.add(3);
             list3.add(4);
 
-            list3.remove(3);
+            auto it = list3.removeAt(3);
             test(list3.size() == 3);
             test(list3.capacity() == 4);
+            test(it == list3.end());
             test(list3[0] = 1);
             test(list3[1] = 2);
             test(list3[2] = 3);
 
-            list3.remove(1);
+            it = list3.removeAt(1);
             test(list3.size() == 2);
             test(list3.capacity() == 4);
             test(list3[0] = 1);
             test(list3[1] = 3);
+            test(*it = 3);
 
-            list3.remove(0);
+            list3.removeAt(0);
             test(list3.size() == 1);
             test(list3.capacity() == 4);
             test(list3[0] = 3);
 
-            list3.remove(0);
+            it = list3.removeAt(0);
             test(list3.size() == 0);
             test(list3.capacity() == 4);
+            test(it == list3.end());
+
+            list3.add(1);
+            list3.add(2);
+            list3.add(3);
+            it = list3.begin();
+            list3.remove(it);
+            test(list3.size() == 2);
+            test(list3[0] == 2);
+            test(list3[1] == 3);
+            it = list3.find(3);
+            list3.remove(it);
+            test(list3.size() == 1);
+            test(list3[0] == 2);
+
+            list3.add(5);
+            list3.add(6);
+            list3.add(7);
+            it = list3.remove(7);
+            test(it == list3.end());
+            test(list3.size() == 3);
+            it = list3.remove(5);
+            test((*it) == 6);
 
             // ----- Test addRange() ----- //
 
@@ -270,6 +296,20 @@ class TestList : public Test {
             test(list1[1] == 8);
         }
 
+        void testFind() {
+
+            List<int> list1(mAllocator);
+            list1.add(1);
+            list1.add(2);
+            list1.add(3);
+            list1.add(4);
+            list1.add(5);
+
+            test(list1.find(1) == list1.begin());
+            test(*(list1.find(2)) == 2);
+            test(*(list1.find(5)) == 5);
+        }
+
         void testEquality() {
 
             List<int> list1(mAllocator);
@@ -320,8 +360,45 @@ class TestList : public Test {
             test(list1[1] == 2);
         }
 
-        void testIteration() {
+        void testIterators() {
-            // TODO : Implement this
+
+            List<int> list1(mAllocator);
+
+            test(list1.begin() == list1.end());
+
+            list1.add(5);
+            list1.add(6);
+            list1.add(8);
+            list1.add(-1);
+
+            List<int>::Iterator itBegin = list1.begin();
+            List<int>::Iterator itEnd = list1.end();
+            List<int>::Iterator it = list1.begin();
+
+            test(itBegin == it);
+            test(*it == 5);
+            test(*(it++) == 5);
+            test(*it == 6);
+            test(*(it--) == 6);
+            test(*it == 5);
+            test(*(++it) == 6);
+            test(*it == 6);
+            test(*(--it) == 5);
+            test(*it == 5);
+            test(it == itBegin);
+
+            it = list1.end();
+            test(it == itEnd);
+            it--;
+            test(*it == -1);
+            it++;
+            test(it == itEnd);
+
+            List<int> list2(mAllocator);
+            for (auto it = list1.begin(); it != list1.end(); ++it) {
+                list2.add(*it);
+            }
+            test(list1 == list2);
         }
 
  };

test/tests/containers/TestMap.h

@@ -31,6 +31,30 @@
 #include "containers/Map.h"
 #include "memory/DefaultAllocator.h"
 
+// Key to test map with always same hash values
+namespace reactphysics3d {
+    struct TestKey {
+        int key;
+
+        TestKey(int k) :key(k) {}
+
+        bool operator==(const TestKey& testKey) const {
+            return key == testKey.key;
+        }
+    };
+}
+
+// Hash function for struct VerticesPair
+namespace std {
+
+  template <> struct hash<reactphysics3d::TestKey> {
+
+    size_t operator()(const reactphysics3d::TestKey& key) const {
+        return 1;
+    }
+  };
+}
+
 /// Reactphysics3D namespace
 namespace reactphysics3d {
 
@@ -62,10 +86,11 @@ class TestMap : public Test {
             testReserve();
             testAddRemoveClear();
             testContainsKey();
+            testFind();
             testIndexing();
             testEquality();
             testAssignment();
-            testIteration();
+            testIterators();
         }
 
         void testConstructors() {
@@ -81,15 +106,14 @@ class TestMap : public Test {
             test(map2.size() == 0);
 
             // ----- Copy Constructors ----- //
-/*
             Map<int, std::string> map3(map1);
             test(map3.capacity() == map1.capacity());
             test(map3.size() == map1.size());
 
             Map<int, int> map4(mAllocator);
-            map4.add(std::make_pair(1, 10));
+            map4.add(Pair<int, int>(1, 10));
-            map4.add(std::make_pair(2, 20));
+            map4.add(Pair<int, int>(2, 20));
-            map4.add(std::make_pair(3, 30));
+            map4.add(Pair<int, int>(3, 30));
             test(map4.capacity() >= 3);
             test(map4.size() == 3);
 
@@ -99,7 +123,6 @@ class TestMap : public Test {
             test(map5[1] == 10);
             test(map5[2] == 20);
             test(map5[3] == 30);
-            */
         }
 
         void testReserve() {
@@ -107,8 +130,8 @@ class TestMap : public Test {
             Map<int, std::string> map1(mAllocator);
             map1.reserve(15);
             test(map1.capacity() >= 15);
-            map1.add(std::make_pair(1, "test1"));
+            map1.add(Pair<int, std::string>(1, "test1"));
-            map1.add(std::make_pair(2, "test2"));
+            map1.add(Pair<int, std::string>(2, "test2"));
             test(map1.capacity() >= 15);
 
             map1.reserve(10);
@@ -125,9 +148,9 @@ class TestMap : public Test {
             // ----- Test add() ----- //
 
             Map<int, int> map1(mAllocator);
-            map1.add(std::make_pair(1, 10));
+            map1.add(Pair<int, int>(1, 10));
-            map1.add(std::make_pair(8, 80));
+            map1.add(Pair<int, int>(8, 80));
-            map1.add(std::make_pair(13, 130));
+            map1.add(Pair<int, int>(13, 130));
             test(map1[1] == 10);
             test(map1[8] == 80);
             test(map1[13] == 130);
@@ -135,7 +158,7 @@ class TestMap : public Test {
 
             Map<int, int> map2(mAllocator, 15);
             for (int i = 0; i < 1000000; i++) {
-                map2.add(std::make_pair(i, i * 100));
+                map2.add(Pair<int, int>(i, i * 100));
             }
             bool isValid = true;
             for (int i = 0; i < 1000000; i++) {
@@ -144,26 +167,38 @@ class TestMap : public Test {
             test(isValid);
 
             map1.remove(1);
-            map1.add(std::make_pair(1, 10));
+            map1.add(Pair<int, int>(1, 10));
             test(map1.size() == 3);
             test(map1[1] == 10);
 
+            map1.add(Pair<int, int>(56, 34));
+            test(map1[56] == 34);
+            test(map1.size() == 4);
+            map1.add(Pair<int, int>(56, 13), true);
+            test(map1[56] == 13);
+            test(map1.size() == 4);
+
             // ----- Test remove() ----- //
 
             map1.remove(1);
             test(!map1.containsKey(1));
             test(map1.containsKey(8));
             test(map1.containsKey(13));
-            test(map1.size() == 2);
+            test(map1.size() == 3);
 
             map1.remove(13);
-            test(!map1.containsKey(8));
+            test(map1.containsKey(8));
-            test(map1.containsKey(13));
+            test(!map1.containsKey(13));
-            test(map1.size() == 1);
+            test(map1.size() == 2);
 
             map1.remove(8);
             test(!map1.containsKey(8));
+            test(map1.size() == 1);
+
+            auto it = map1.remove(56);
+            test(!map1.containsKey(56));
             test(map1.size() == 0);
+            test(it == map1.end());
 
             isValid = true;
             for (int i = 0; i < 1000000; i++) {
@@ -177,19 +212,36 @@ class TestMap : public Test {
 
             Map<int, int> map3(mAllocator);
             for (int i=0; i < 1000000; i++) {
-                map3.add(std::make_pair(i, i * 10));
+                map3.add(Pair<int, int>(i, i * 10));
                 map3.remove(i);
             }
 
+            map3.add(Pair<int, int>(1, 10));
+            map3.add(Pair<int, int>(2, 20));
+            map3.add(Pair<int, int>(3, 30));
+            test(map3.size() == 3);
+            it = map3.begin();
+            map3.remove(it++);
+            test(!map3.containsKey(1));
+            test(map3.size() == 2);
+            test(it->second == 20);
+
+            map3.add(Pair<int, int>(56, 32));
+            map3.add(Pair<int, int>(23, 89));
+            for (it = map3.begin(); it != map3.end();) {
+                it = map3.remove(it);
+            }
+            test(map3.size() == 0);
+
             // ----- Test clear() ----- //
 
             Map<int, int> map4(mAllocator);
-            map4.add(std::make_pair(2, 20));
+            map4.add(Pair<int, int>(2, 20));
-            map4.add(std::make_pair(4, 40));
+            map4.add(Pair<int, int>(4, 40));
-            map4.add(std::make_pair(6, 60));
+            map4.add(Pair<int, int>(6, 60));
             map4.clear();
             test(map4.size() == 0);
-            map4.add(std::make_pair(2, 20));
+            map4.add(Pair<int, int>(2, 20));
             test(map4.size() == 1);
             test(map4[2] == 20);
             map4.clear();
@@ -198,6 +250,24 @@ class TestMap : public Test {
             Map<int, int> map5(mAllocator);
             map5.clear();
             test(map5.size() == 0);
+
+            // ----- Test map with always same hash value for keys ----- //
+
+            Map<TestKey, int> map6(mAllocator);
+            for (int i=0; i < 1000; i++) {
+                map6.add(Pair<TestKey, int>(TestKey(i), i));
+            }
+            bool isTestValid = true;
+            for (int i=0; i < 1000; i++) {
+                if (map6[TestKey(i)] != i) {
+                    isTestValid = false;
+                }
+            }
+            test(isTestValid);
+            for (int i=0; i < 1000; i++) {
+                map6.remove(TestKey(i));
+            }
+            test(map6.size() == 0);
         }
 
         void testContainsKey() {
@@ -208,9 +278,9 @@ class TestMap : public Test {
             test(!map1.containsKey(4));
             test(!map1.containsKey(6));
 
-            map1.add(std::make_pair(2, 20));
+            map1.add(Pair<int, int>(2, 20));
-            map1.add(std::make_pair(4, 40));
+            map1.add(Pair<int, int>(4, 40));
-            map1.add(std::make_pair(6, 60));
+            map1.add(Pair<int, int>(6, 60));
 
             test(map1.containsKey(2));
             test(map1.containsKey(4));
@@ -229,9 +299,9 @@ class TestMap : public Test {
         void testIndexing() {
 
             Map<int, int> map1(mAllocator);
-            map1.add(std::make_pair(2, 20));
+            map1.add(Pair<int, int>(2, 20));
-            map1.add(std::make_pair(4, 40));
+            map1.add(Pair<int, int>(4, 40));
-            map1.add(std::make_pair(6, 60));
+            map1.add(Pair<int, int>(6, 60));
             test(map1[2] == 20);
             test(map1[4] == 40);
             test(map1[6] == 60);
@@ -245,6 +315,26 @@ class TestMap : public Test {
             test(map1[6] == 30);
         }
 
+        void testFind() {
+
+            Map<int, int> map1(mAllocator);
+            map1.add(Pair<int, int>(2, 20));
+            map1.add(Pair<int, int>(4, 40));
+            map1.add(Pair<int, int>(6, 60));
+            test(map1.find(2)->second == 20);
+            test(map1.find(4)->second == 40);
+            test(map1.find(6)->second == 60);
+            test(map1.find(45) == map1.end());
+
+            map1[2] = 10;
+            map1[4] = 20;
+            map1[6] = 30;
+
+            test(map1.find(2)->second == 10);
+            test(map1.find(4)->second == 20);
+            test(map1.find(6)->second == 30);
+        }
+
         void testEquality() {
 
             Map<std::string, int> map1(mAllocator, 10);
@@ -252,13 +342,13 @@ class TestMap : public Test {
 
             test(map1 == map2);
 
-            map1.add(std::make_pair("a", 1));
+            map1.add(Pair<std::string, int>("a", 1));
-            map1.add(std::make_pair("b", 2));
+            map1.add(Pair<std::string, int>("b", 2));
-            map1.add(std::make_pair("c", 3));
+            map1.add(Pair<std::string, int>("c", 3));
 
-            map2.add(std::make_pair("a", 1));
+            map2.add(Pair<std::string, int>("a", 1));
-            map2.add(std::make_pair("b", 2));
+            map2.add(Pair<std::string, int>("b", 2));
-            map2.add(std::make_pair("c", 4));
+            map2.add(Pair<std::string, int>("c", 4));
 
             test(map1 == map1);
             test(map2 == map2);
@@ -269,7 +359,7 @@ class TestMap : public Test {
             test(map1 == map2);
 
             Map<std::string, int> map3(mAllocator);
-            map3.add(std::make_pair("a", 1));
+            map3.add(Pair<std::string, int>("a", 1));
 
             test(map1 != map3);
             test(map2 != map3);
@@ -278,13 +368,14 @@ class TestMap : public Test {
         void testAssignment() {
 
            Map<int, int> map1(mAllocator);
-           map1.add(std::make_pair(1, 3));
+           map1.add(Pair<int, int>(1, 3));
-           map1.add(std::make_pair(2, 6));
+           map1.add(Pair<int, int>(2, 6));
-           map1.add(std::make_pair(10, 30));
+           map1.add(Pair<int, int>(10, 30));
-/*
+
            Map<int, int> map2(mAllocator);
            map2 = map1;
            test(map2.size() == map1.size());
+           test(map1 == map2);
            test(map2[1] == 3);
            test(map2[2] == 6);
            test(map2[10] == 30);
@@ -292,6 +383,7 @@ class TestMap : public Test {
            Map<int, int> map3(mAllocator, 100);
            map3 = map1;
            test(map3.size() == map1.size());
+           test(map3 == map1);
            test(map3[1] == 3);
            test(map3[2] == 6);
            test(map3[10] == 30);
@@ -299,18 +391,40 @@ class TestMap : public Test {
            Map<int, int> map4(mAllocator);
            map3 = map4;
            test(map3.size() == 0);
-*/
+           test(map3 == map4);
+
            Map<int, int> map5(mAllocator);
-           map5.add(std::make_pair(7, 8));
+           map5.add(Pair<int, int>(7, 8));
-           map5.add(std::make_pair(19, 70));
+           map5.add(Pair<int, int>(19, 70));
            map1 = map5;
            test(map5.size() == map1.size());
+           test(map5 == map1);
            test(map1[7] == 8);
            test(map1[19] == 70);
         }
 
-        void testIteration() {
+        void testIterators() {
 
+            Map<int, int> map1(mAllocator);
+
+            test(map1.begin() == map1.end());
+
+            map1.add(Pair<int, int>(1, 5));
+            map1.add(Pair<int, int>(2, 6));
+            map1.add(Pair<int, int>(3, 8));
+            map1.add(Pair<int, int>(4, -1));
+
+            Map<int, int>::Iterator itBegin = map1.begin();
+            Map<int, int>::Iterator it = map1.begin();
+
+            test(itBegin == it);
+
+            int size = 0;
+            for (auto it = map1.begin(); it != map1.end(); ++it) {
+                test(map1.containsKey(it->first));
+                size++;
+            }
+            test(map1.size() == size);
         }
  };
 

test/tests/containers/TestSet.h

@@ -0,0 +1,417 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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 TEST_SET_H
+#define TEST_SET_H
+
+// Libraries
+#include "Test.h"
+#include "containers/Set.h"
+#include "memory/DefaultAllocator.h"
+
+// Key to test map with always same hash values
+namespace reactphysics3d {
+    struct TestValueSet {
+        int key;
+
+        TestValueSet(int k) :key(k) {}
+
+        bool operator==(const TestValueSet& testValue) const {
+            return key == testValue.key;
+        }
+    };
+}
+
+// Hash function for struct VerticesPair
+namespace std {
+
+  template <> struct hash<reactphysics3d::TestValueSet> {
+
+    size_t operator()(const reactphysics3d::TestValueSet& value) const {
+        return 1;
+    }
+  };
+}
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestSet
+/**
+ * Unit test for the Set class
+ */
+class TestSet : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        DefaultAllocator mAllocator;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestSet(const std::string& name) : Test(name) {
+
+        }
+
+        /// Run the tests
+        void run() {
+
+            testConstructors();
+            testReserve();
+            testAddRemoveClear();
+            testContains();
+            testFind();
+            testEquality();
+            testAssignment();
+            testIterators();
+        }
+
+        void testConstructors() {
+
+            // ----- Constructors ----- //
+
+            Set<std::string> set1(mAllocator);
+            test(set1.capacity() == 0);
+            test(set1.size() == 0);
+
+            Set<std::string> set2(mAllocator, 100);
+            test(set2.capacity() >= 100);
+            test(set2.size() == 0);
+
+            // ----- Copy Constructors ----- //
+            Set<std::string> set3(set1);
+            test(set3.capacity() == set1.capacity());
+            test(set3.size() == set1.size());
+
+            Set<int> set4(mAllocator);
+            set4.add(10);
+            set4.add(20);
+            set4.add(30);
+            test(set4.capacity() >= 3);
+            test(set4.size() == 3);
+            set4.add(30);
+            test(set4.size() == 3);
+
+            Set<int> set5(set4);
+            test(set5.capacity() == set4.capacity());
+            test(set5.size() == set4.size());
+            test(set5.contains(10));
+            test(set5.contains(20));
+            test(set5.contains(30));
+        }
+
+        void testReserve() {
+
+            Set<std::string> set1(mAllocator);
+            set1.reserve(15);
+            test(set1.capacity() >= 15);
+            set1.add("test1");
+            set1.add("test2");
+            test(set1.capacity() >= 15);
+
+            set1.reserve(10);
+            test(set1.capacity() >= 15);
+
+            set1.reserve(100);
+            test(set1.capacity() >= 100);
+            test(set1.contains("test1"));
+            test(set1.contains("test2"));
+        }
+
+        void testAddRemoveClear() {
+
+            // ----- Test add() ----- //
+
+            Set<int> set1(mAllocator);
+            set1.add(10);
+            set1.add(80);
+            set1.add(130);
+            test(set1.contains(10));
+            test(set1.contains(80));
+            test(set1.contains(130));
+            test(set1.size() == 3);
+
+            Set<int> set2(mAllocator, 15);
+            for (int i = 0; i < 1000000; i++) {
+                set2.add(i);
+            }
+            bool isValid = true;
+            for (int i = 0; i < 1000000; i++) {
+                if (!set2.contains(i)) isValid = false;
+            }
+            test(isValid);
+
+            set1.remove(10);
+            set1.add(10);
+            test(set1.size() == 3);
+            test(set1.contains(10));
+
+            set1.add(34);
+            test(set1.contains(34));
+            test(set1.size() == 4);
+
+            // ----- Test remove() ----- //
+
+            set1.remove(10);
+            test(!set1.contains(10));
+            test(set1.contains(80));
+            test(set1.contains(130));
+            test(set1.contains(34));
+            test(set1.size() == 3);
+
+            set1.remove(80);
+            test(!set1.contains(80));
+            test(set1.contains(130));
+            test(set1.contains(34));
+            test(set1.size() == 2);
+
+            set1.remove(130);
+            test(!set1.contains(130));
+            test(set1.contains(34));
+            test(set1.size() == 1);
+
+            set1.remove(34);
+            test(!set1.contains(34));
+            test(set1.size() == 0);
+
+            isValid = true;
+            for (int i = 0; i < 1000000; i++) {
+                set2.remove(i);
+            }
+            for (int i = 0; i < 1000000; i++) {
+                if (set2.contains(i)) isValid = false;
+            }
+            test(isValid);
+            test(set2.size() == 0);
+
+            Set<int> set3(mAllocator);
+            for (int i=0; i < 1000000; i++) {
+                set3.add(i);
+                set3.remove(i);
+            }
+
+            set3.add(1);
+            set3.add(2);
+            set3.add(3);
+            test(set3.size() == 3);
+            auto it = set3.begin();
+            set3.remove(it++);
+            test(!set3.contains(1));
+            test(set3.size() == 2);
+            test(*it == 2);
+
+            set3.add(6);
+            set3.add(7);
+            set3.add(8);
+            for (it = set3.begin(); it != set3.end();) {
+               it = set3.remove(it);
+            }
+            test(set3.size() == 0);
+
+            // ----- Test clear() ----- //
+
+            Set<int> set4(mAllocator);
+            set4.add(2);
+            set4.add(4);
+            set4.add(6);
+            set4.clear();
+            test(set4.size() == 0);
+            set4.add(2);
+            test(set4.size() == 1);
+            test(set4.contains(2));
+            set4.clear();
+            test(set4.size() == 0);
+
+            Set<int> set5(mAllocator);
+            set5.clear();
+            test(set5.size() == 0);
+
+            // ----- Test map with always same hash value for keys ----- //
+
+            Set<TestValueSet> set6(mAllocator);
+            for (int i=0; i < 1000; i++) {
+                set6.add(TestValueSet(i));
+            }
+            bool isTestValid = true;
+            for (int i=0; i < 1000; i++) {
+                if (!set6.contains(TestValueSet(i))) {
+                    isTestValid = false;
+                }
+            }
+            test(isTestValid);
+            for (int i=0; i < 1000; i++) {
+                set6.remove(TestValueSet(i));
+            }
+            test(set6.size() == 0);
+        }
+
+        void testContains() {
+
+            Set<int> set1(mAllocator);
+
+            test(!set1.contains(2));
+            test(!set1.contains(4));
+            test(!set1.contains(6));
+
+            set1.add(2);
+            set1.add(4);
+            set1.add(6);
+
+            test(set1.contains(2));
+            test(set1.contains(4));
+            test(set1.contains(6));
+
+            set1.remove(4);
+            test(!set1.contains(4));
+            test(set1.contains(2));
+            test(set1.contains(6));
+
+            set1.clear();
+            test(!set1.contains(2));
+            test(!set1.contains(6));
+        }
+
+        void testFind() {
+
+            Set<int> set1(mAllocator);
+            set1.add(2);
+            set1.add(4);
+            set1.add(6);
+            test(set1.find(2) != set1.end());
+            test(set1.find(4) != set1.end());
+            test(set1.find(6) != set1.end());
+            test(set1.find(45) == set1.end());
+
+            set1.remove(2);
+
+            test(set1.find(2) == set1.end());
+        }
+
+        void testEquality() {
+
+            Set<std::string> set1(mAllocator, 10);
+            Set<std::string> set2(mAllocator, 2);
+
+            test(set1 == set2);
+
+            set1.add("a");
+            set1.add("b");
+            set1.add("c");
+
+            set2.add("a");
+            set2.add("b");
+            set2.add("h");
+
+            test(set1 == set1);
+            test(set2 == set2);
+            test(set1 != set2);
+            test(set2 != set1);
+
+            set1.add("a");
+            set2.remove("h");
+            set2.add("c");
+
+            test(set1 == set2);
+            test(set2 == set1);
+
+            Set<std::string> set3(mAllocator);
+            set3.add("a");
+
+            test(set1 != set3);
+            test(set2 != set3);
+            test(set3 != set1);
+            test(set3 != set2);
+        }
+
+        void testAssignment() {
+
+           Set<int> set1(mAllocator);
+           set1.add(1);
+           set1.add(2);
+           set1.add(10);
+
+           Set<int> set2(mAllocator);
+           set2 = set1;
+           test(set2.size() == set1.size());
+           test(set2.contains(1));
+           test(set2.contains(2));
+           test(set2.contains(10));
+           test(set1 == set2);
+
+           Set<int> set3(mAllocator, 100);
+           set3 = set1;
+           test(set3.size() == set1.size());
+           test(set3 == set1);
+           test(set3.contains(1));
+           test(set3.contains(2));
+           test(set3.contains(10));
+
+           Set<int> set4(mAllocator);
+           set3 = set4;
+           test(set3.size() == 0);
+           test(set3 == set4);
+
+           Set<int> set5(mAllocator);
+           set5.add(7);
+           set5.add(19);
+           set1 = set5;
+           test(set5.size() == set1.size());
+           test(set1 == set5);
+           test(set1.contains(7));
+           test(set1.contains(19));
+        }
+
+        void testIterators() {
+
+            Set<int> set1(mAllocator);
+
+            test(set1.begin() == set1.end());
+
+            set1.add(1);
+            set1.add(2);
+            set1.add(3);
+            set1.add(4);
+
+            Set<int>::Iterator itBegin = set1.begin();
+            Set<int>::Iterator it = set1.begin();
+
+            test(itBegin == it);
+
+            int size = 0;
+            for (auto it = set1.begin(); it != set1.end(); ++it) {
+                test(set1.contains(*it));
+                size++;
+            }
+            test(set1.size() == size);
+        }
+ };
+
+}
+
+#endif

test/tests/mathematics/TestMathematicsFunctions.h

@@ -103,6 +103,16 @@ class TestMathematicsFunctions : public Test {
             test(!sameSign(4, -7));
             test(!sameSign(-4, 53));
 
+            // Test computePointToPlaneDistance()
+            Vector3 p(8, 4, 0);
+            Vector3 n1(1, 0, 0);
+            Vector3 n2(-1, 0, 0);
+            Vector3 q1(1, 54, 0);
+            Vector3 q2(8, 17, 0);
+            test(approxEqual(computePointToPlaneDistance(q1, n1, p), decimal(-7)));
+            test(approxEqual(computePointToPlaneDistance(q1, n2, p), decimal(7)));
+            test(approxEqual(computePointToPlaneDistance(q2, n2, p), decimal(0.0)));
+
             // Test computeBarycentricCoordinatesInTriangle()
             Vector3 a(0, 0, 0);
             Vector3 b(5, 0, 0);

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();
 }
 

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());
 }
 

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());
 }
 

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());
 }
 

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());
 }
 

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());
 }
 

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());
 }
 

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();
 }
 

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>();

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;

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