a

1 files changed, 62 insertions, 26 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;