Remove the use of std::map and fix issues in Map class

src/collision/CollisionDetection.cpp

@@ -48,7 +48,8 @@ using namespace std;
 
 // Constructor
 CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager)
-                   : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
+                   : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr),
+                     mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this),
                      mIsCollisionShapesAdded(false) {
 
     // Set the default collision dispatch configuration
@@ -104,7 +105,7 @@ void CollisionDetection::computeMiddlePhase() {
     PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler);
 
     // For each possible collision pair of bodies
-    map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
 
         OverlappingPair* pair = it->second;
@@ -126,14 +127,14 @@ void CollisionDetection::computeMiddlePhase() {
         // overlapping pair
         if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 
-            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            Map<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator itToRemove = it;
             ++it;
 
             // Destroy the overlapping pair
             itToRemove->second->~OverlappingPair();
 
             mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
-            mOverlappingPairs.erase(itToRemove);
+            mOverlappingPairs.remove(itToRemove);
             continue;
         }
         else {
@@ -320,10 +321,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
         (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
 
     // Compute the overlapping pair ID
-    overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
+    OverlappingPair::OverlappingPairId 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 +332,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
                                               mMemoryManager.getSingleFrameAllocator());
     assert(newPair != nullptr);
 
-#ifndef NDEBUG
+    mOverlappingPairs.add(make_pair(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,11 +345,11 @@ 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<OverlappingPair::OverlappingPairId, 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;
+            Map<OverlappingPair::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
@@ -360,7 +357,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
             // Destroy the overlapping pair
             itToRemove->second->~OverlappingPair();
             mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
-            mOverlappingPairs.erase(itToRemove);
+            mOverlappingPairs.remove(itToRemove);
         }
         else {
             ++it;
@@ -376,7 +373,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<OverlappingPair::OverlappingPairId, 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 +424,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<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         // Process the potential contacts of the overlapping pair
@@ -466,7 +463,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<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         // If there is a user callback
@@ -921,7 +918,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
     computeBroadPhase();
 
     // For each possible collision pair of bodies
-    map<overlappingpairid, OverlappingPair*>::iterator it;
+    Map<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
         OverlappingPair* originalPair = it->second;

src/collision/CollisionDetection.h

@@ -34,9 +34,9 @@
 #include "narrowphase/DefaultCollisionDispatch.h"
 #include "memory/MemoryManager.h"
 #include "constraint/ContactPoint.h"
+#include "containers/Map.h"
 #include <set>
 #include <utility>
-#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -79,7 +79,7 @@ class CollisionDetection {
         NarrowPhaseInfo* mNarrowPhaseInfoList;
 
         /// Broad-phase overlapping pairs
-        std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
+        Map<OverlappingPair::OverlappingPairId, OverlappingPair*> mOverlappingPairs;
 
         /// Broad-phase algorithm
         BroadPhaseAlgorithm mBroadPhaseAlgorithm;

src/collision/HalfEdgeStructure.cpp

@@ -25,37 +25,52 @@
 
 // Libraries
 #include "HalfEdgeStructure.h"
-#include <map>
+#include "containers/Map.h"
+#include "containers/containers_common.h"
 
 using namespace reactphysics3d;
 
+// Hash function for struct VerticesPair
+namespace std {
+
+  template <> struct hash<HalfEdgeStructure::VerticesPair> {
+
+    size_t operator()(const HalfEdgeStructure::VerticesPair& pair) const {
+
+        std::size_t seed = 0;
+        hash_combine<uint>(seed, pair.vertex1);
+        hash_combine<uint>(seed, pair.vertex2);
+
+        return seed;
+    }
+  };
+}
+
 // 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;
-
-    List<edgeKey> currentFaceEdges(mAllocator, mFaces[0].faceVertices.size());
 
     // For each face
     for (uint f=0; f<mFaces.size(); f++) {
 
         Face face = mFaces[f];
 
-        edgeKey firstEdgeKey;
+        VerticesPair 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 +80,19 @@ void HalfEdgeStructure::init() {
                 firstEdgeKey = pairV1V2;
             }
             else if (v >= 1) {
-                nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
+                nextEdges.add(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
             }
             if (v == (face.faceVertices.size() - 1)) {
-                nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey));
+                nextEdges.add(std::make_pair(pairV1V2, firstEdgeKey));
             }
-            edges.insert(std::make_pair(pairV1V2, edge));
+            edges.add(std::make_pair(pairV1V2, edge));
 
-            const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index);
+            const VerticesPair pairV2V1(v2Index, v1Index);
 
-            mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index));
+            mapEdgeToStartVertex.add(std::make_pair(pairV1V2, v1Index), true);
-            mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index));
+            mapEdgeToStartVertex.add(std::make_pair(pairV2V1, v2Index), true);
 
-            mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2));
+            mapFaceIndexToEdgeKey.add(std::make_pair(f, pairV1V2), true);
 
             auto itEdge = edges.find(pairV2V1);
             if (itEdge != edges.end()) {
@@ -87,14 +102,14 @@ void HalfEdgeStructure::init() {
                 itEdge->second.twinEdgeIndex = edgeIndex + 1;
                 edge.twinEdgeIndex = edgeIndex;
 
-                mapEdgeIndexToKey[edgeIndex] = pairV2V1;
+                mapEdgeIndexToKey.add(std::make_pair(edgeIndex, pairV2V1));
-                mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2;
+                mapEdgeIndexToKey.add(std::make_pair(edgeIndex + 1, pairV1V2));
 
                 mVertices[v1Index].edgeIndex = edgeIndex + 1;
                 mVertices[v2Index].edgeIndex = edgeIndex;
 
-                mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
+                mapEdgeToIndex.add(std::make_pair(pairV1V2, edgeIndex + 1));
-                mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
+                mapEdgeToIndex.add(std::make_pair(pairV2V1, edgeIndex));
 
                 mEdges.add(itEdge->second);
                 mEdges.add(edge);

src/collision/HalfEdgeStructure.h

@@ -41,6 +41,25 @@ class HalfEdgeStructure {
 
     public:
 
+        /// Pair of vertices
+        struct VerticesPair {
+
+            uint vertex1;
+            uint vertex2;
+
+            /// Constructor
+            VerticesPair() = default;
+
+            /// Constructor
+            VerticesPair(uint v1, uint v2) : vertex1(v1), vertex2(v2) {}
+
+            /// Equality operator
+            bool operator==(const VerticesPair& pair) const {
+                return vertex1 == pair.vertex1 && vertex2 == pair.vertex2;
+            }
+        };
+
+        /// Edge
         struct Edge {
             uint vertexIndex;       // Index of the vertex at the beginning of the edge
             uint twinEdgeIndex;     // Index of the twin edge
@@ -48,6 +67,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
@@ -59,6 +79,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/shapes/ConvexMeshShape.h

@@ -34,7 +34,6 @@
 #include "collision/PolyhedronMesh.h"
 #include "collision/narrowphase/GJK/GJKAlgorithm.h"
 #include <set>
-#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {

src/containers/Map.h

@@ -30,8 +30,11 @@
 #include "memory/MemoryAllocator.h"
 #include "mathematics/mathematics_functions.h"
 #include <cstring>
+#include <stdexcept>
+#include <functional>
 #include <limits>
 
+
 namespace reactphysics3d {
 
 // Class Map
@@ -387,7 +390,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<std::pair<K,V>*>(mAllocator.allocate(sizeof(std::pair<K, V>)));
+                   new (mEntries[i].keyValue) std::pair<K,V>(*(map.mEntries[i].keyValue));
+                }
+            }
         }
 
         /// Destructor
@@ -417,7 +428,7 @@ class Map {
         }
 
         /// Add an element into the map
-        void add(const std::pair<K,V>& keyValue) {
+        void add(const std::pair<K,V>& keyValue, bool insertIfAlreadyPresent = false) {
 
             if (mCapacity == 0) {
                 initialize(0);
@@ -435,7 +446,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) std::pair<K,V>(keyValue);
+
+                        return;
+                    }
+                    else {
+                        throw std::runtime_error("The key and value pair already exists in the map");
+                    }
                 }
             }
 
@@ -473,6 +496,13 @@ class Map {
             mBuckets[bucket] = entryIndex;
         }
 
+        /// Remove the element pointed by some iterator
+        bool 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) {
 
@@ -605,7 +635,9 @@ 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

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/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

@@ -833,7 +833,7 @@ List<const ContactManifold*> DynamicsWorld::getContactsList() {
     List<const ContactManifold*> contactManifolds(mMemoryManager.getPoolAllocator());
 
     // For each currently overlapping pair of bodies
-    std::map<overlappingpairid, OverlappingPair*>::const_iterator it;
+    Map<OverlappingPair::OverlappingPairId, OverlappingPair*>::Iterator it;
     for (it = mCollisionDetection.mOverlappingPairs.begin();
          it != mCollisionDetection.mOverlappingPairs.end(); ++it) {
 

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(std::make_pair(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++);
+            mLastFrameCollisionInfos.remove(it++);
         }
         else {
             ++it;

src/engine/OverlappingPair.h

@@ -30,14 +30,12 @@
 #include "collision/ContactManifoldSet.h"
 #include "collision/ProxyShape.h"
 #include "collision/shapes/CollisionShape.h"
-#include <map>
+#include "containers/Map.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 +93,38 @@ struct LastFrameCollisionInfo {
  */
 class OverlappingPair {
 
+    public:
+
+        /// Pair of shape ids
+        struct ShapeIdPair {
+
+            uint shapeIdBody1;
+            uint shapeIdBody2;
+
+            /// Constructor
+            ShapeIdPair(uint id1, uint id2) : shapeIdBody1(id1), shapeIdBody2(id2) {}
+
+            /// Equality operator
+            bool operator==(const ShapeIdPair& pair) const {
+                return shapeIdBody1 == pair.shapeIdBody1 && shapeIdBody2 == pair.shapeIdBody2;
+            }
+        };
+
+        /// Pair of broad-phase ids
+        struct OverlappingPairId {
+
+            uint body1Id;
+            uint body2Id;
+
+            /// Constructor
+            OverlappingPairId(uint id1, uint id2) : body1Id(id1), body2Id(id2) {}
+
+            /// Equality operator
+            bool operator==(const OverlappingPairId& pair) const {
+                return body1Id == pair.body1Id && body2Id == pair.body2Id;
+            }
+        };
+
     private:
 
         // -------------------- Attributes -------------------- //
@@ -102,13 +132,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 +141,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 +172,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 +223,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 +249,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,14 +270,14 @@ 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);
+    assert(pairID.body1Id != pairID.body2Id);
     return pairID;
 }
 
@@ -296,10 +326,38 @@ 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)];
 }
 
 }
 
+// Hash function for struct ShapeIdPair
+namespace std {
+
+  template <> struct hash<reactphysics3d::OverlappingPair::ShapeIdPair> {
+
+    size_t operator()(const reactphysics3d::OverlappingPair::ShapeIdPair& pair) const {
+
+        std::size_t seed = 0;
+        reactphysics3d::hash_combine<uint>(seed, pair.shapeIdBody1);
+        reactphysics3d::hash_combine<uint>(seed, pair.shapeIdBody2);
+
+        return seed;
+    }
+  };
+
+  template <> struct hash<reactphysics3d::OverlappingPair::OverlappingPairId> {
+
+    size_t operator()(const reactphysics3d::OverlappingPair::OverlappingPairId& pair) const {
+
+        std::size_t seed = 0;
+        reactphysics3d::hash_combine<uint>(seed, pair.body1Id);
+        reactphysics3d::hash_combine<uint>(seed, pair.body2Id);
+
+        return seed;
+    }
+  };
+}
+
 #endif
 

test/tests/collision/TestRaycast.h

@@ -211,23 +211,21 @@ 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[4] = Vector3(-2, 3, -4);
             mPolyhedronVertices[5] = Vector3(2, 3, -4);
-            mPolyhedronVertices[6] = Vector3(2, 3, 4);
+            mPolyhedronVertices[6] = Vector3(-2, 3, -4);
-            mPolyhedronVertices[7] = Vector3(-2, 3, 4);
+            mPolyhedronVertices[7] = Vector3(-2, -3, -4);
 
             mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3;
-            mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2;
+            mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 4; mPolyhedronIndices[6] = 5; mPolyhedronIndices[7] = 2;
-            mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1;
+            mPolyhedronIndices[8] = 4; mPolyhedronIndices[9] = 7; mPolyhedronIndices[10] = 6; mPolyhedronIndices[11] = 5;
-            mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4;
+            mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 6; mPolyhedronIndices[15] = 7;
-            mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7;
+            mPolyhedronIndices[16] = 2; mPolyhedronIndices[17] = 5; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 3;
-            mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7;
+            mPolyhedronIndices[20] = 1; mPolyhedronIndices[21] = 0; mPolyhedronIndices[22] = 7; mPolyhedronIndices[23] = 4;
 
             // Polygon faces descriptions for the polyhedron
             for (int f=0; f < 8; f++) {

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 {
 
@@ -82,7 +106,6 @@ 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());
@@ -100,7 +123,6 @@ class TestMap : public Test {
             test(map5[1] == 10);
             test(map5[2] == 20);
             test(map5[3] == 30);
-            */
         }
 
         void testReserve() {
@@ -123,8 +145,6 @@ class TestMap : public Test {
 
         void testAddRemoveClear() {
 
-            // TODO : ADD test with values with same hash for keys but different keys
-
             // ----- Test add() ----- //
 
             Map<int, int> map1(mAllocator);
@@ -151,21 +171,32 @@ class TestMap : public Test {
             test(map1.size() == 3);
             test(map1[1] == 10);
 
+            map1.add(std::make_pair(56, 34));
+            test(map1[56] == 34);
+            test(map1.size() == 4);
+            map1.add(std::make_pair(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(13));
-            test(map1.size() == 1);
+            test(map1.size() == 2);
 
             map1.remove(8);
             test(!map1.containsKey(8));
+            test(map1.size() == 1);
+
+            map1.remove(56);
+            test(!map1.containsKey(56));
             test(map1.size() == 0);
 
             isValid = true;
@@ -184,6 +215,16 @@ class TestMap : public Test {
                 map3.remove(i);
             }
 
+            map3.add(std::make_pair(1, 10));
+            map3.add(std::make_pair(2, 20));
+            map3.add(std::make_pair(3, 30));
+            test(map3.size() == 3);
+            auto it = map3.begin();
+            map3.remove(it++);
+            test(!map3.containsKey(1));
+            test(map3.size() == 2);
+            test(it->second == 20);
+
             // ----- Test clear() ----- //
 
             Map<int, int> map4(mAllocator);
@@ -201,6 +242,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(std::make_pair(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() {