a

2 files changed, 84 insertions, 42 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;
 };