a

3 files changed, 36 insertions, 39 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index cec44225..37d8e562 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -7,14 +7,12 @@
 namespace floormat {
 
 template<typename T> using bbox = path_search::bbox<T>;
+using visited = astar::visited;
 
 namespace {
 
-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>
@@ -100,7 +98,7 @@ void astar::reserve(size_t capacity)
 {
     nodes.reserve(capacity);
     indexes.reserve(capacity);
-#ifndef FM_ASTAR_NO_EDGE_CACHE
+#if !FM_ASTAR_NO_EDGE_CACHE
     edges.reserve(capacity*4);
 #endif
     Q.reserve(capacity);
@@ -110,7 +108,7 @@ void astar::clear()
 {
     nodes.clear();
     indexes.clear();
-#ifndef FM_ASTAR_NO_EDGE_CACHE
+#if !FM_ASTAR_NO_EDGE_CACHE
     edges.clear();
 #endif
     Q.clear();
@@ -231,13 +229,14 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
     while (!Q.empty())
     {
         const auto id = pop_from_heap();
+        point n_pt;
         global_coords n_coord;
         Vector2b n_offset;
         uint32_t n_dist;
-        {
-            auto& n = nodes[id];
+        {   auto& n = nodes[id];
             n_coord = n.coord;
             n_offset = n.offset;
+            n_pt = {n_coord, n_offset};
             n_dist = n.dist;
         }
 
@@ -266,39 +265,27 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
 
             const auto new_pt = point{.coord = new_coord, .offset = new_offset};
 
+            auto new_idx = (uint32_t)-1;
+            bool fresh = true;
+
             if (auto it = indexes.find(new_pt); it != indexes.end())
-                if (nodes[it->second].dist >= dist)
-                    continue;
+            {
+                new_idx = it->second;
+                fresh = false;
 
-#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;
+                if (nodes[new_idx].dist <= dist)
+                    continue;
             }
-#endif
 
-            const auto sz = nodes.size();
-            auto [it, fresh] = indexes.try_emplace(new_pt, sz);
-            const auto new_idx = it.value();
-
-            if (new_idx == sz)
-            {
-                auto new_node = visited {
-                    .dist = dist, .prev = id,
-                    .coord = new_coord, .offset = new_offset,
-                };
-                nodes.push_back(new_node);
+#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;
             }
-
-            auto& node = nodes[new_idx];
-
-            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});
+#else
+            auto e = make_edge(n_pt, {new_coord, new_offset});
             if (auto [it, fresh] = edges.try_emplace(e, edge_status::unknown); fresh)
             {
                 auto& status = it.value();
@@ -306,7 +293,7 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
                 auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
                 auto bb = bbox_union(bb1, bb0);
 
-                if (path_search::is_passable(w, chunk_coords_(node.coord), bb, own_id, p))
+                if (path_search::is_passable(w, chunk_coords_(new_coord), bb, own_id, p))
                     status = edge_status::good;
                 else
                 {
@@ -316,6 +303,16 @@ path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id o
             }
 #endif
 
+            if (fresh)
+            {   const auto sz = nodes.size();
+                new_idx = indexes[new_pt] = (uint32_t)sz;
+                auto new_node = visited {
+                    .dist = dist, .prev = id,
+                    .coord = new_coord, .offset = new_offset,
+                };
+                nodes.push_back(new_node);
+            }
+
 #ifndef FM_NO_DEBUG
             if (debug >= 3) [[unlikely]]
                 DBG_nospace << (fresh ? "" : " old")
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 733f5374..232410d5 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -81,8 +81,6 @@ struct astar
                                 object_id own_id, uint32_t max_dist, Vector2ub own_size,
                                 int debug = 0, const pred& p = path_search::never_continue());
 
-#define FM_ASTAR_NO_EDGE_CACHE
-
 private:
     static constexpr auto div_factor = (int8_t)path_search::div_factor;
     static constexpr auto initial_capacity = TILE_COUNT * 16 * div_factor*div_factor;
@@ -90,6 +88,8 @@ private:
     void add_to_heap(uint32_t id);
     uint32_t pop_from_heap();
 
+#define FM_ASTAR_NO_EDGE_CACHE 1
+
 #ifndef FM_ASTAR_NO_EDGE_CACHE
     struct edge
     {
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
index d5ab6798..2cf32451 100644
--- a/test/dijkstra.cpp
+++ b/test/dijkstra.cpp
@@ -36,7 +36,7 @@ void test_app::test_dijkstra()
     auto w = world();
     auto a = astar();
 
-    constexpr auto wcx = 1, wcy = 1, wtx = 8, wty = 8, wox = 0, woy = 0;
+    constexpr auto wcx = 1, wcy = 1, wtx = 8, wty = 8, wox = 3, woy = 3;
     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};