pathfinding: switch from hash table to array

3 files changed, 184 insertions, 94 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 2ff87b00..d471f693 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -1,6 +1,7 @@
 #include "path-search.hpp"
 #include "object.hpp"
 #include "point.hpp"
+#include <Corrade/Containers/StaticArray.h>
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/Math/Functions.h>
 
@@ -29,7 +30,18 @@ constexpr bbox<T> bbox_union(bbox<T> bb, Vector2i coord, Vector2b offset, Vector
 
 template<typename T>
 requires std::is_arithmetic_v<T>
-constexpr bbox<T> bbox_union(bbox<T> bb1, bbox<T> bb2)
+constexpr auto bbox_from_pos(Math::Vector2<T> pos, Vector2b offset, Vector2ui size)
+{
+    const auto vec = Vector2i(pos) * iTILE_SIZE2 + Vector2i(offset);
+    const auto min = vec - Vector2i(size / 2);
+    const auto max = vec + Vector2i(size);
+    const auto bb = bbox<float>{Vector2(min), Vector2(max)};
+    return bb;
+}
+
+template<typename T>
+requires std::is_arithmetic_v<T>
+constexpr inline bbox<T> bbox_union(bbox<T> bb1, bbox<T> bb2)
 {
     return { Math::min(bb1.min, bb2.min), Math::max(bb1.max, bb2.max) };
 }
@@ -59,17 +71,6 @@ constexpr auto directions = []() constexpr
     return array;
 }();
 
-template<typename T>
-requires std::is_arithmetic_v<T>
-constexpr auto bbox_from_pos(Math::Vector2<T> pos, Vector2b offset, Vector2ui size)
-{
-    const auto vec = Vector2i(pos) * iTILE_SIZE2 + Vector2i(offset);
-    const auto min = vec - Vector2i(size / 2);
-    const auto max = vec + Vector2i(size);
-    const auto bb = bbox<float>{Vector2(min), Vector2(max)};
-    return bb;
-}
-
 struct heap_comparator
 {
     const std::vector<visited>& nodes; // NOLINT
@@ -77,7 +78,7 @@ struct heap_comparator
     inline bool operator()(uint32_t a, uint32_t b) const { return nodes[b].dist < nodes[a].dist; }
 };
 
-uint32_t distance(point a, point b)
+inline uint32_t distance(point a, point b)
 {
     Vector2i dist;
     dist += Math::abs(a.coord() - b.coord())*iTILE_SIZE2;
@@ -89,69 +90,36 @@ uint32_t distance(point a, point b)
 
 astar::astar()
 {
-    indexes.max_load_factor(.4f);
     reserve(initial_capacity);
 }
 
 void astar::reserve(size_t capacity)
 {
     nodes.reserve(capacity);
-    indexes.reserve(capacity);
-#if !FM_ASTAR_NO_EDGE_CACHE
-    edges.reserve(capacity*4);
-#endif
     Q.reserve(capacity);
 }
 
 void astar::clear()
 {
     nodes.clear();
-    indexes.clear();
-#if !FM_ASTAR_NO_EDGE_CACHE
-    edges.clear();
-#endif
     Q.clear();
 }
 
 void astar::add_to_heap(uint32_t id)
 {
     Q.push_back(id);
-    std::push_heap(Q.begin(), Q.end(), heap_comparator(nodes));
+    std::push_heap(Q.begin(), Q.end(), heap_comparator{nodes});
 }
 
 uint32_t astar::pop_from_heap()
 {
-    std::pop_heap(Q.begin(), Q.end(), heap_comparator(nodes));
+    std::pop_heap(Q.begin(), Q.end(), heap_comparator{nodes});
     const auto id = Q.back();
     Q.pop_back();
     return id;
 }
 
-#if !FM_ASTAR_NO_EDGE_CACHE
-auto astar::make_edge(const point& a, const point& b) -> edge
-{
-    if (a < b)
-        return { b, a };
-    else
-        return { a, b };
-}
-
-size_t astar::edge_hash::operator()(const edge& e) const
-{
-    static_assert(sizeof e == 16);
-#ifdef FLOORMAT_64
-    static_assert(sizeof nullptr > 4);
-    return fnvhash_64(&e, sizeof e);
-#else
-    static_assert(sizeof nullptr == 4);
-    return fnvhash_32(&e, sizeof e);
-#endif
-}
-
-bool astar::edge::operator==(const astar::edge& other) const = default;
-#endif
-
-path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id own_id, uint32_t max_dist,
+path_search_result astar::Dijkstra(world& w, const point from_, const point to_, object_id own_id, uint32_t max_dist,
                                    Vector2ub own_size_, int debug, const pred& p)
 {
 #ifdef FM_NO_DEBUG
@@ -159,6 +127,7 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
 #endif
 
     clear();
+    cache.allocate(from_, max_dist);
 
     constexpr auto min_size_ = Vector2ui{Vector2(div_size) * 1.5f + Vector2{.5f}};
     const auto own_size = Math::max(Vector2ui{Vector2{own_size_}*1.5f + Vector2{.5f}}, min_size_);
@@ -183,8 +152,9 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
     path_search_result result;
     auto& path = result.path(); path.clear();
 
-    indexes[from_] = 0;
-    nodes.push_back({.dist = 0, .pt = from_ });
+    cache.allocate(from_, max_dist);
+    cache.add_index(from_offset, from_, 0);
+    nodes.push_back({.dist = 0, .pt = from_, });
     add_to_heap(0);
 
     if (!from_offset.isZero())
@@ -198,13 +168,15 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
             for (int8_t x = div_min; x < div_max; x++)
             {
                 const auto offset = Vector2b(div_size * Vector2i(x, y));
-                if (auto bb = bbox_union(start_bbox, from_local, offset, own_size);
-                    path_search::is_passable(w, from.chunk3(), bb, own_id, p))
-                {
-                    indexes[{from, offset}] = idx;
-                    nodes.push_back({.dist = from_offset_len, .prev = 0, .pt = {from, offset}});
-                    add_to_heap(idx++);
-                }
+                if (offset != from_offset)
+                    if (auto bb = bbox_union(start_bbox, from_local, offset, own_size);
+                        path_search::is_passable(w, from.chunk3(), bb, own_id, p))
+                    {
+                        auto pt = point{from, offset};
+                        cache.add_index(from_offset, pt, idx);
+                        nodes.push_back({.dist = from_offset_len, .prev = 0, .pt = pt, });
+                        add_to_heap(idx++);
+                    }
             }
     }
 
@@ -220,9 +192,6 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
         {   auto& n = nodes[id];
             cur_pt = n.pt;
             cur_dist = n.dist;
-#if FM_ASTAR_NO_EDGE_CACHE
-            (void)cur_pt;
-#endif
         }
 
         if (auto d = distance(cur_pt, to_); d < closest)
@@ -248,21 +217,23 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
 
             const auto new_pt = object::normalize_coords(cur_pt, vec);
             const auto [new_coord, new_offset] = new_pt;
-            const size_t new_pt_hash = new_pt.hash();
+            //const size_t new_pt_hash = new_pt.hash();
 
-            auto new_idx = (uint32_t)-1;
             bool fresh = true;
 
-            if (auto it = indexes.find(new_pt, new_pt_hash); it != indexes.end())
+            auto chunk_idx = cache.get_chunk_index(Vector2i(new_pt.chunk()));
+            auto tile_idx = cache.get_tile_index(from_offset, Vector2i(new_pt.local()), new_offset);
+            auto new_idx = cache.lookup_index(chunk_idx, tile_idx);
+
+            if (new_idx != (uint32_t)-1)
             {
-                new_idx = it->second;
                 fresh = false;
 
                 if (nodes[new_idx].dist <= dist)
                     continue;
             }
 
-#if FM_ASTAR_NO_EDGE_CACHE
+#if 1
             {   auto vec_ = Vector2(vec);
                 auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
                 auto bb = bbox_union(bb1, bb0);
@@ -291,7 +262,8 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
             if (fresh)
             {   const auto sz = nodes.size();
                 new_idx = (uint32_t)sz;
-                indexes.insert({new_pt, new_idx}, new_pt_hash);
+                cache.add_index(chunk_idx, tile_idx, new_idx);
+                //indexes.insert({new_pt, new_idx}, new_pt_hash);
                 auto new_node = visited {
                     .dist = dist, .prev = id,
                     .pt = {new_coord, new_offset},
@@ -311,21 +283,136 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
         }
     }
 
-    fm_debug_assert(nodes.size() == indexes.size());
+    //fm_debug_assert(nodes.size() == indexes.size());
 #ifndef FM_NO_DEBUG
     if (debug >= 1)
         DBG_nospace << "dijkstra: closest px:" << closest
                     << " path:" << closest_path_len
                     << " pos:" << closest_pos
-                    << " nodes:" << nodes.size()
-#if !FM_ASTAR_NO_EDGE_CACHE
-                    << " edges:" << edges.size()
-#endif
-            ;
+                    << " nodes:" << nodes.size();
 #endif
 
     // todo...
     return result;
 }
 
+struct astar::chunk_cache
+{
+    static constexpr size_t dimensions[] = {
+        2 /* offset, no-offset*/,
+        TILE_COUNT,
+        (size_t)div_factor * (size_t)div_factor,
+    };
+    static constexpr size_t size = []() constexpr -> size_t {
+        size_t x = 1;
+        for (auto i : dimensions)
+            x *= i;
+        return x;
+    }();
+    static constexpr size_t rank = sizeof(dimensions)/sizeof(dimensions[0]);
+
+    struct index { uint32_t value = (uint32_t)value; };
+
+    std::array<index, size> indexes = {};
+    std::bitset<size> exists{false};
+};
+
+astar::cache::cache() = default;
+
+Vector2ui astar::cache::get_size_to_allocate(uint32_t max_dist)
+{
+    constexpr auto chunk_size = Vector2ui(iTILE_SIZE2) * TILE_MAX_DIM;
+    constexpr auto rounding   = chunk_size - Vector2ui(1);
+    auto nchunks = (Vector2ui(max_dist) + rounding) / chunk_size;
+    return nchunks + Vector2ui(3);
+}
+
+void astar::cache::allocate(point from, uint32_t max_dist)
+{
+    auto off = get_size_to_allocate(max_dist);
+    start = Vector2i(from.chunk()) - Vector2i(off);
+    size = off * 2u + Vector2ui(1);
+    auto len = size.product();
+    if (len > array.size())
+        array = Array<chunk_cache>{ValueInit, len};
+    else
+    {
+        for (auto i = 0uz; i < len; i++)
+        {
+            array[i].exists = {};
+            array[i].indexes = {}; // todo
+        }
+    }
+}
+
+size_t astar::cache::get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord)
+{
+    auto off_ =  coord - start;
+    fm_assert(off_ >= Vector2i{0, 0});
+    auto off = Vector2ui(off_);
+    fm_assert(off < size);
+    auto index = off.y() * size.x() + off.x();
+    fm_debug_assert(index < size.product());
+    return index;
+}
+
+size_t astar::cache::get_chunk_index(Vector2i chunk) const { return get_chunk_index(start, size, chunk); }
+
+size_t astar::cache::get_tile_index(Vector2b from_offset, Vector2i pos, Vector2b offset_)
+{
+    Vector2i offset{offset_};
+    bool is_offset = false;
+    if (offset % div_size != Vector2i{0, 0})
+    {
+        offset -= Vector2i(from_offset);
+        fm_assert(offset % div_size == Vector2i{0, 0});
+        is_offset = true;
+    }
+    constexpr auto tile_start = div_size * div_factor/-2;
+    offset -= tile_start;
+    fm_debug_assert(offset >= Vector2i{0, 0});
+    auto nth_div = offset / div_size;
+    const size_t idx[] = {
+        (size_t)is_offset,
+        (size_t)pos.y() * TILE_MAX_DIM + (size_t)pos.x(),
+        (size_t)nth_div.y() * div_factor + (size_t)nth_div.x(),
+    };
+    size_t index = 0;
+    for (auto i = 0uz; i < chunk_cache::rank; i++)
+    {
+        size_t k = idx[i];
+        for (auto j = 0uz; j < i; j++)
+            k *= chunk_cache::dimensions[j];
+        index += k;
+    }
+    fm_assert(index < chunk_cache::size);
+    return index;
+}
+
+void astar::cache::add_index(size_t chunk_index, size_t tile_index, uint32_t index)
+{
+    auto& c = array[chunk_index];
+    fm_debug_assert(!c.exists[tile_index]);
+    c.exists[tile_index] = true;
+    c.indexes[tile_index] = {index};
+}
+
+void astar::cache::add_index(Vector2b from_offset, point pt, uint32_t index)
+{
+    auto ch = get_chunk_index(Vector2i(pt.chunk()));
+    auto tile = get_tile_index(from_offset, Vector2i(pt.local()), pt.offset());
+    fm_debug_assert(!array[ch].exists[tile]);
+    array[ch].exists[tile] = true;
+    array[ch].indexes[tile] = {index};
+}
+
+uint32_t astar::cache::lookup_index(size_t chunk_index, size_t tile_index)
+{
+    auto& c = array[chunk_index];
+    if (c.exists[tile_index])
+        return c.indexes[tile_index].value;
+    else
+        return (uint32_t)-1;
+}
+
 } // namespace floormat
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 95bdea23..9456c2e6 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -9,10 +9,11 @@
 #include "point.hpp"
 #include <bit>
 #include <array>
+#include <vector>
+#include <bitset>
+#include <Corrade/Containers/Array.h>
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/DimensionTraits.h>
-#include <tsl/robin_map.h>
-#include <tsl/robin_set.h>
 
 namespace floormat {
 
@@ -78,26 +79,33 @@ private:
     static constexpr auto div_factor = (int8_t)path_search::div_factor;
     static constexpr auto initial_capacity = TILE_COUNT * 16 * div_factor*div_factor;
 
-    void add_to_heap(uint32_t id);
-    uint32_t pop_from_heap();
-
-#define FM_ASTAR_NO_EDGE_CACHE 1
+    struct chunk_cache;
 
-#if !FM_ASTAR_NO_EDGE_CACHE
-    struct edge
+    struct cache
     {
-        point from, to;
-        bool operator==(const edge& other) const;
+        Vector2ui size;
+        Vector2i start{(int)((1u << 31) - 1)};
+        Array<chunk_cache> array;
+
+        cache();
+        fm_DECLARE_DELETED_COPY_ASSIGNMENT(cache);
+
+        size_t get_chunk_index(Vector2i chunk) const;
+        static size_t get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord);
+        static size_t get_tile_index(Vector2b from_offset, Vector2i pos, Vector2b offset);
+        static Vector2ui get_size_to_allocate(uint32_t max_dist);
+
+        void allocate(point from, uint32_t max_dist);
+        void add_index(size_t chunk_index, size_t tile_index, uint32_t index);
+        void add_index(Vector2b from_offset, point pt, uint32_t index);
+        uint32_t lookup_index(size_t chunk_index, size_t tile_index);
     };
-    enum class edge_status : uint8_t { unknown, good, bad, };
-    struct edge_hash { size_t operator()(const edge& e) const; };
-    static edge make_edge(const point& a, const point& b);
 
-    tsl::robin_map<edge, edge_status, edge_hash> edges;
-#endif
+    void add_to_heap(uint32_t id);
+    uint32_t pop_from_heap();
 
+    struct cache cache;
     std::vector<visited> nodes;
-    tsl::robin_map<point, uint32_t, point_hash> indexes;
     std::vector<uint32_t> Q;
 };
 
diff --git a/src/point.hpp b/src/point.hpp
index 10963c22..bd9c66ca 100644
--- a/src/point.hpp
+++ b/src/point.hpp
@@ -20,8 +20,6 @@ struct point
     constexpr point();
     constexpr point(global_coords coord, Vector2b offset);
     constexpr point(chunk_coords_ coord, local_coords tile, Vector2b offset);
-    constexpr point(const point& other);
-    constexpr point& operator=(const point& other);
 
     constexpr bool operator==(const point&) const noexcept;
     friend constexpr std::strong_ordering operator<=>(const point& a, const point& b);
@@ -48,9 +46,6 @@ constexpr point::point(global_coords coord, Vector2b offset) : point{coord.chunk
 constexpr point::point(chunk_coords_ coord, local_coords tile, Vector2b offset) :
     cx{coord.x}, cy{coord.y}, cz{coord.z}, tile{tile}, _offset{offset}
 {}
-constexpr point::point(const point& other) = default;
-constexpr point& point::operator=(const point& other) = default;
-
 constexpr bool point::operator==(const point&) const noexcept = default;
 
 constexpr std::strong_ordering operator<=>(const point& p1, const point& p2)