a

3 files changed, 105 insertions, 53 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 5b43e5c9..b60aef27 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -14,6 +14,7 @@ constexpr auto chunk_size = iTILE_SIZE2 * TILE_MAX_DIM;
 constexpr auto div_size = path_search::div_size;
 constexpr auto min_size = path_search::min_size;
 constexpr auto goal_distance = div_size;
+using visited = astar::visited;
 
 template<typename T>
 requires std::is_arithmetic_v<T>
@@ -74,10 +75,10 @@ constexpr bbox<T> bbox_from_pos(Math::Vector<2, T> pos, Vector2b offset, Vector2
 
 class heap_comparator
 {
-    const std::vector<astar::visited>& nodes; // NOLINT
+    const std::vector<visited>& nodes; // NOLINT
 
 public:
-    heap_comparator(const std::vector<astar::visited>& nodes) : nodes{nodes} {}
+    heap_comparator(const std::vector<visited>& nodes) : nodes{nodes} {}
 
     inline bool operator()(uint32_t a, uint32_t b) const
     {
@@ -108,7 +109,9 @@ void astar::reserve(size_t capacity)
 {
     nodes.reserve(capacity);
     indexes.reserve(capacity);
+#ifndef FM_ASTAR_NO_EDGE_CACHE
     edges.reserve(capacity*4);
+#endif
     Q.reserve(capacity);
 }
 
@@ -116,7 +119,9 @@ void astar::clear()
 {
     nodes.clear();
     indexes.clear();
+#ifndef FM_ASTAR_NO_EDGE_CACHE
     edges.clear();
+#endif
     Q.clear();
 }
 
@@ -134,6 +139,7 @@ uint32_t astar::pop_from_heap()
     return id;
 }
 
+#ifndef FM_ASTAR_NO_EDGE_CACHE
 auto astar::make_edge(const point& a, const point& b) -> edge
 {
     if (a < b)
@@ -142,35 +148,39 @@ auto astar::make_edge(const point& a, const point& b) -> edge
         return { b.coord, a.coord, b.offset, a.offset };
 }
 
-bool astar::edge::operator==(const floormat::astar::edge& other) const = default;
-
-size_t astar::point_hash::operator()(point pt) const
+size_t astar::edge_hash::operator()(const edge& e) const
 {
-    static_assert(sizeof(global_coords) == 8);
+    static_assert(sizeof e == 8 + 8 + 2 + 2);
 #ifdef FLOORMAT_64
     static_assert(sizeof nullptr > 4);
-    return fnvhash_64(&pt, sizeof pt);
+    return fnvhash_64(&e, sizeof e);
 #else
     static_assert(sizeof nullptr == 4);
-    return fnvhash_32(&pt, sizeof pt);
+    return fnvhash_32(&e, sizeof e);
 #endif
 }
 
-size_t astar::edge_hash::operator()(const edge& e) const
+bool astar::edge::operator==(const floormat::astar::edge& other) const = default;
+#endif
+
+size_t astar::point_hash::operator()(point pt) const
 {
-    static_assert(sizeof e == 8 + 8 + 2 + 2);
+    static_assert(sizeof(global_coords) == 8);
 #ifdef FLOORMAT_64
     static_assert(sizeof nullptr > 4);
-    return fnvhash_64(&e, sizeof e);
+    return fnvhash_64(&pt, sizeof pt);
 #else
     static_assert(sizeof nullptr == 4);
-    return fnvhash_32(&e, sizeof e);
+    return fnvhash_32(&pt, sizeof pt);
 #endif
 }
 
 path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id own_id, uint32_t max_dist,
                                    Vector2ub own_size, int debug, const pred& p)
 {
+#ifdef FM_NO_DEBUG
+    (void)debug;
+#endif
     const auto [from, from_offset] = from_;
     const auto [to, to_offset] = to_;
 
@@ -191,7 +201,8 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
 
     clear();
 
-    const auto start_bbox = bbox_from_pos(Vector2(from.local()), from_offset, own_size);
+    const auto from_local = Vector2i(from.local());
+    const auto start_bbox = bbox_from_pos(Vector2(from_local), from_offset, own_size);
 
     path_search_result result;
     auto& path = result._node->vec; path.clear();
@@ -204,14 +215,19 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
     {
         const auto from_offset_len = Math::max(1u, (uint32_t)(Vector2(from_offset).length() + 0.5f));
         uint32_t idx = 1;
-        // todo also add 4 subdivisions within the tile the same way
-        if (auto bb = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
-            path_search::is_passable(w, chunk_coords_{from}, bb, own_id, p))
-        {
-            indexes[{from, {}}] = idx;
-            nodes.push_back({.dist = from_offset_len, .prev = 0, .coord = from, .offset = {}});
-            add_to_heap(idx++);
-        }
+        constexpr int8_t div_min = (div_factor+1)/-2, div_max = div_factor/2;
+        for (int8_t y = div_min; y < div_max; y++)
+            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, chunk_coords_{from}, bb, own_id, p))
+                {
+                    indexes[{from, offset}] = idx;
+                    nodes.push_back({.dist = from_offset_len, .prev = 0, .coord = from, .offset = offset});
+                    add_to_heap(idx++);
+                }
+            }
     }
 
     auto closest = distance({from, from_offset}, {to, to_offset});
@@ -238,13 +254,15 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
             closest_path_len = n_dist;
         }
 
+#ifndef FM_NO_DEBUG
         if (debug >= 2) [[unlikely]]
             DBG_nospace << "node"
                         << " px:" << closest << " path:" << closest_path_len
                         << " pos:" << closest_pos.coord.to_signed()
                         << ";" << closest_pos.offset;
-        const auto bb0 = bbox_from_pos(Vector2(n_coord.local()), n_offset, own_size);
+#endif
 
+        const auto bb0 = bbox_from_pos(Vector2(n_coord.local()), n_offset, own_size);
         for (auto [vec, len] : directions)
         {
             auto [new_coord, new_offset] = object::normalize_coords(n_coord, n_offset, vec);
@@ -252,13 +270,22 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
             if (dist >= max_dist)
                 continue;
 
+            #ifdef FM_ASTAR_NO_EDGE_CACHE
+            {  auto vec_ = Vector2(vec);
+               auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
+               auto bb = bbox_union(bb1, bb0);
+               if (!path_search::is_passable(w, chunk_coords_(new_coord), bb, own_id, p))
+                   continue;
+            }
+            #endif
+
             const auto sz = nodes.size();
             auto [it, fresh] = indexes.try_emplace({.coord = new_coord, .offset = new_offset}, sz);
             const auto new_idx = it.value();
 
             if (new_idx == sz)
             {
-                auto new_node = astar::visited {
+                auto new_node = visited {
                     .dist = dist, .prev = id,
                     .coord = new_coord, .offset = new_offset,
                 };
@@ -270,7 +297,7 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
             if (!fresh && dist >= node.dist)
                 continue;
             node.dist = dist;
-
+#ifndef FM_ASTAR_NO_EDGE_CACHE
             auto e = make_edge({node.coord, node.offset}, {new_coord, new_offset});
             if (auto [it, fresh] = edges.try_emplace(e, edge_status::unknown); fresh)
             {
@@ -287,22 +314,31 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_,object_id ow
                     continue;
                 }
             }
+#endif
 
+#ifndef FM_NO_DEBUG
             if (debug >= 3) [[unlikely]]
                 DBG_nospace << (fresh ? "" : " old")
                             << " path:" << closest_path_len
                             << " pos:" << closest_pos.coord.to_signed()
                             << ";" << closest_pos.offset;
+#endif
 
             add_to_heap(new_idx);
         }
     }
 
     fm_debug_assert(nodes.size() == indexes.size());
-    if (debug)
+#ifndef FM_NO_DEBUG
+    if (debug >= 1)
         DBG_nospace << "dijkstra: closest px:" << closest << " path:" << closest_path_len
                     << " pos:" << closest_pos.coord.to_signed() << ";" << closest_pos.offset
-                    << " nodes:" << nodes.size() << " edges:" << edges.size();
+                    << " nodes:" << nodes.size()
+#ifndef FM_ASTAR_NO_EDGE_CACHE
+                    << " edges:" << edges.size()
+#endif
+            ;
+#endif
 
     // todo...
     return result;
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 35c00404..2705b1f6 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -65,40 +65,46 @@ struct astar
         Vector2b offset;
     };
 
-    struct edge
-    {
-        global_coords from, to;
-        Vector2b from_offset, to_offset;
-
-        bool operator==(const edge& other) const;
-    };
-
     using pred = path_search::pred;
-    enum class edge_status : uint8_t { unknown, good, bad, };
     template<typename T> using bbox = path_search::bbox<T>;
     struct point_hash { size_t operator()(point pt) const; };
-    struct edge_hash { size_t operator()(const edge& e) const; };
 
     fm_DECLARE_DELETED_COPY_ASSIGNMENT(astar);
 
     astar();
     void reserve(size_t capacity);
     void clear();
-    void add_to_heap(uint32_t id);
-    uint32_t pop_from_heap();
-    static edge make_edge(const point& a, const point& b);
 
     // todo add simple bresenham short-circuit
     path_search_result Dijkstra(world& w, point from, point to,
                                 object_id own_id, uint32_t max_dist, Vector2ub own_size,
                                 int debug = 0, const pred& p = path_search::never_continue());
 
-    static constexpr auto div_factor = path_search::div_factor;
-    static constexpr auto initial_capacity = TILE_COUNT * 16 * div_factor*div_factor;
+//#define FM_ASTAR_NO_EDGE_CACHE
 
 private:
-    std::vector<visited> nodes;
+    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();
+
+#ifndef FM_ASTAR_NO_EDGE_CACHE
+    struct edge
+    {
+        global_coords from, to;
+        Vector2b from_offset, to_offset;
+
+        bool operator==(const edge& other) const;
+    };
+    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
+
+    std::vector<visited> nodes;
     tsl::robin_map<point, uint32_t, point_hash> indexes;
     std::vector<uint32_t> Q;
 };
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
index 4163de2b..ff88df71 100644
--- a/test/dijkstra.cpp
+++ b/test/dijkstra.cpp
@@ -1,6 +1,7 @@
 #include "app.hpp"
 #include "path-search.hpp"
 #include "loader/loader.hpp"
+#include <Magnum/Math/Functions.h>
 #include <chrono>
 #include <Corrade/Containers/StringView.h>
 
@@ -35,25 +36,34 @@ void test_app::test_dijkstra()
     auto w = world();
     auto a = astar();
 
+#if 1
+    constexpr auto wcx = 1, wcy = 1, wtx = 8, wty = 8, wox = 0, woy = 0;
+    constexpr auto max_dist = (uint32_t)(Vector2i(Math::abs(wcx)+1, Math::abs(wcy)+1)*TILE_MAX_DIM*iTILE_SIZE2).length();
+    constexpr auto wch = chunk_coords_{chunk_coords_{wcx, wcy,0}};
+    constexpr auto wt = local_coords{wtx, wty};
+    constexpr auto wpos = global_coords{wch, wt};
+
     auto metal2 = tile_image_proto{loader.tile_atlas("metal2", {2, 2}, pass_mode::blocked), 0};
-    auto& ch = w[chunk_coords_{0,0,0}];
+    auto& ch  = w[chunk_coords_{0,0,0}];
+    auto& ch2 = w[wch];
 
-#if 0
     ch[{4, 4}].wall_west()  = metal2;
     ch[{4, 4}].wall_north() = metal2;
-    ch[{8, 8}].wall_west()  = metal2;
-    ch[{8, 8}].wall_north() = metal2;
-    ch[{9, 8}].wall_west()  = metal2;
-    ch[{8, 7}].wall_north() = metal2;
+
+    ch2[{ wtx,   wty   }].wall_west()  = metal2;
+    ch2[{ wtx,   wty   }].wall_north() = metal2;
+    ch2[{ wtx+1, wty   }].wall_west()  = metal2;
+    ch2[{ wtx,   wty -1}].wall_north() = metal2;
 #endif
-    ch.mark_modified();
+
+    fm_assert(ch.is_passability_modified());
 
     bench_run("Dijkstra", [&] {
-      constexpr auto max_dist = Vector2ui(2*TILE_MAX_DIM*iTILE_SIZE2).length();
       a.Dijkstra(w,
-                 {{0, 0, 0},   {0, 0}}, // from
-                 {{16, 16, 0}, {7, 9}}, // to
-                 0, max_dist, {32,32}); // size
+                 {{ 0, 0,  0}, {11, 9}}, // from
+                 {wpos, {wox, woy}},     // to
+                 0, max_dist,  {32,32},  // size
+                 1);
     });
 }