2 files changed, 40 insertions, 41 deletions

diff --git a/src/common/Collision/BoundingIntervalHierarchyWrapper.h b/src/common/Collision/BoundingIntervalHierarchyWrapper.h
index 30ff03733eb..85048da58bc 100644
--- a/src/common/Collision/BoundingIntervalHierarchyWrapper.h
+++ b/src/common/Collision/BoundingIntervalHierarchyWrapper.h
@@ -15,13 +15,11 @@
  * with this program. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef _BIH_WRAP
-#define _BIH_WRAP
+#ifndef TRINITYCORE_BOUNDING_INTERVAL_HIERARCHY_WRAPPER_H
+#define TRINITYCORE_BOUNDING_INTERVAL_HIERARCHY_WRAPPER_H
 
 #include "BoundingIntervalHierarchy.h"
-#include <G3D/Table.h>
-#include <G3D/Array.h>
-#include <G3D/Set.h>
+#include <unordered_map>
 
 template<class T, class BoundsFunc = BoundsTrait<T> >
 class BIHWrap
@@ -29,58 +27,68 @@ class BIHWrap
     template<class RayCallback>
     struct MDLCallback
     {
-        const T* const* objects;
+        std::size_t objects_size;
+        T const* const* objects;
         RayCallback& _callback;
-        uint32 objects_size;
 
-        MDLCallback(RayCallback& callback, const T* const* objects_array, uint32 objects_size ) : objects(objects_array), _callback(callback), objects_size(objects_size) { }
+        MDLCallback(RayCallback& callback, T const* const* objects_array, std::size_t objects_size ) : objects_size(objects_size), objects(objects_array), _callback(callback) { }
 
         /// Intersect ray
-        bool operator() (const G3D::Ray& ray, uint32 idx, float& maxDist, bool /*stopAtFirst*/)
+        bool operator() (G3D::Ray const& ray, std::size_t idx, float& maxDist, bool /*stopAtFirst*/)
         {
             if (idx >= objects_size)
                 return false;
-            if (const T* obj = objects[idx])
+            if (T const* obj = objects[idx])
                 return _callback(ray, *obj, maxDist/*, stopAtFirst*/);
             return false;
         }
 
         /// Intersect point
-        void operator() (const G3D::Vector3& p, uint32 idx)
+        void operator() (G3D::Vector3 const& p, std::size_t idx)
         {
             if (idx >= objects_size)
                 return;
-            if (const T* obj = objects[idx])
+            if (T const* obj = objects[idx])
                 _callback(p, *obj);
         }
     };
 
-    typedef G3D::Array<const T*> ObjArray;
-
     BIH m_tree;
-    ObjArray m_objects;
-    G3D::Table<const T*, uint32> m_obj2Idx;
-    G3D::Set<const T*> m_objects_to_push;
+    std::vector<T const*> m_objects;
+    std::unordered_map<T const*, std::size_t> m_obj2Idx;
     int unbalanced_times;
 
 public:
     BIHWrap() : unbalanced_times(0) { }
 
-    void insert(const T& obj)
+    void insert(T const& obj)
     {
+        auto [itr, isNew] = m_obj2Idx.try_emplace(&obj, m_objects.size());
+        if (!isNew)
+            return;
+
+        m_objects.push_back(itr->first);
         ++unbalanced_times;
-        m_objects_to_push.insert(&obj);
     }
 
-    void remove(const T& obj)
+    void remove(T const& obj)
     {
+        auto node = m_obj2Idx.extract(&obj);
+        if (!node)
+            return;
+
+        if (node.key() != m_objects.back())
+        {
+            // update index of last element (will be swapped with removed one)
+            m_obj2Idx.find(m_objects.back())->second = node.mapped();
+
+            // move last into removed element slot
+            m_objects[node.mapped()] = m_objects.back();
+        }
+
+        m_objects.pop_back();
+
         ++unbalanced_times;
-        uint32 Idx = 0;
-        const T * temp;
-        if (m_obj2Idx.getRemove(&obj, temp, Idx))
-            m_objects[Idx] = nullptr;
-        else
-            m_objects_to_push.remove(&obj);
     }
 
     void balance()
@@ -88,30 +96,25 @@ public:
         if (unbalanced_times == 0)
             return;
 
-        unbalanced_times = 0;
-        m_objects.fastClear();
-        m_obj2Idx.getKeys(m_objects);
-        m_objects_to_push.getMembers(m_objects);
-        //assert that m_obj2Idx has all the keys
-
         m_tree.build(m_objects, BoundsFunc());
+        unbalanced_times = 0;
     }
 
     template<typename RayCallback>
-    void intersectRay(const G3D::Ray& ray, RayCallback& intersectCallback, float& maxDist)
+    void intersectRay(G3D::Ray const& ray, RayCallback& intersectCallback, float& maxDist)
     {
         balance();
-        MDLCallback<RayCallback> temp_cb(intersectCallback, m_objects.getCArray(), m_objects.size());
+        MDLCallback<RayCallback> temp_cb(intersectCallback, m_objects.data(), m_objects.size());
         m_tree.intersectRay(ray, temp_cb, maxDist, true);
     }
 
     template<typename IsectCallback>
-    void intersectPoint(const G3D::Vector3& point, IsectCallback& intersectCallback)
+    void intersectPoint(G3D::Vector3 const& point, IsectCallback& intersectCallback)
     {
         balance();
-        MDLCallback<IsectCallback> callback(intersectCallback, m_objects.getCArray(), m_objects.size());
+        MDLCallback<IsectCallback> callback(intersectCallback, m_objects.data(), m_objects.size());
         m_tree.intersectPoint(point, callback);
     }
 };
 
-#endif // _BIH_WRAP
+#endif // TRINITYCORE_BOUNDING_INTERVAL_HIERARCHY_WRAPPER_H
diff --git a/src/common/Collision/DynamicTree.cpp b/src/common/Collision/DynamicTree.cpp
index 1f5875fac0a..073e317e4ae 100644
--- a/src/common/Collision/DynamicTree.cpp
+++ b/src/common/Collision/DynamicTree.cpp
@@ -35,10 +35,6 @@ int CHECK_TREE_PERIOD = 200;
 
 } // namespace
 
-template<> struct HashTrait< GameObjectModel>{
-    static size_t hashCode(GameObjectModel const& g) { return (size_t)(void*)&g; }
-};
-
 template<> struct PositionTrait< GameObjectModel> {
     static void getPosition(GameObjectModel const& g, G3D::Vector3& p) { p = g.getPosition(); }
 };