a

3 files changed, 52 insertions, 31 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 79765473..fe0f2b70 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -88,22 +88,6 @@ public:
     }
 };
 
-class box
-{
-    using visited = astar::visited;
-    std::vector<visited>& vec; // NOLINT
-    uint32_t id;
-
-public:
-    fm_DECLARE_DELETED_COPY_ASSIGNMENT(box);
-    fm_DECLARE_DELETED_MOVE_ASSIGNMENT(box);
-
-    visited& operator*() { return vec[id]; }
-    visited* operator->() { return &vec[id]; }
-
-    box(std::vector<visited>& vec, uint32_t id) : vec{vec}, id{id} {}
-};
-
 uint32_t distance(point a, point b)
 {
     Vector2i dist;
@@ -184,8 +168,8 @@ size_t astar::edge_hash::operator()(const edge& e) const
 #endif
 }
 
-path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id own_id,
-                                   point from_, point to_, uint32_t max_dist, const pred& p)
+path_search_result astar::Dijkstra(world& w, point from_, point to_, object_id own_id, uint32_t max_dist,
+                                   Vector2ub own_size, const pred& p)
 {
     const auto [from, from_offset] = from_;
     const auto [to, to_offset] = to_;
@@ -230,24 +214,37 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
         }
     }
 
-    auto closest = (uint32_t)-1;
+    auto closest = distance({from, from_offset}, {to, to_offset});
+    auto closest_pos = point{from, from_offset};
+    uint32_t closest_path_len = 0;
 
     while (!Q.empty())
     {
         const auto id = pop_from_heap();
-        auto node = box{nodes, id};
+        global_coords n_coord;
+        Vector2b n_offset;
+        uint32_t n_dist;
+        {
+            auto& n = nodes[id];
+            n_coord = n.coord;
+            n_offset = n.offset;
+            n_dist = n.dist;
+        }
 
-        if (auto d = distance({node->coord, node->offset}, {to, to_offset}); d < closest)
+        if (auto d = distance({n_coord, n_offset}, {to, to_offset}); d < closest)
+        {
             closest = d;
+            closest_pos = {n_coord, n_offset};
+            closest_path_len = n_dist;
+        }
 
         //Debug{} << "node" << id << "|" << node.coord.to_signed3() << node.offset << "|" << node.dist;
-
-        const auto bb0 = bbox_from_pos(Vector2(node->coord.local()), node->offset, own_size);
+        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(node->coord, node->offset, vec);
-            const auto dist = node->dist + len;
+            auto [new_coord, new_offset] = object::normalize_coords(n_coord, n_offset, vec);
+            const auto dist = n_dist + len;
             if (dist >= max_dist)
                 continue;
 
@@ -293,9 +290,11 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
         }
     }
 
-    //DBG << "done!" << nodes.size() << "nodes," << indexes.size() << "indexes," << edges.size() << "edges.";
+    fm_debug_assert(nodes.size() == indexes.size());
+    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();
 
-    Debug{} << "closest" << closest;
     // todo...
     return result;
 }
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 6d8da1da..f705c251 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -89,8 +89,8 @@ struct astar
     static edge make_edge(const point& a, const point& b);
 
     // todo add simple bresenham short-circuit
-    path_search_result Dijkstra(world& w, Vector2ub own_size, object_id own_id,
-                                point from, point to, uint32_t max_dist,
+    path_search_result Dijkstra(world& w, point from, point to,
+                                object_id own_id, uint32_t max_dist, Vector2ub own_size,
                                 const pred& p = path_search::never_continue());
 
     static constexpr auto div_factor = path_search::div_factor;
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
index 9b0ee1b6..7138db85 100644
--- a/test/dijkstra.cpp
+++ b/test/dijkstra.cpp
@@ -1,5 +1,6 @@
 #include "app.hpp"
 #include "path-search.hpp"
+#include "loader/loader.hpp"
 #include <chrono>
 #include <Corrade/Containers/StringView.h>
 
@@ -13,11 +14,16 @@ void bench_run(StringView name, F&& fun)
 {
     using namespace std::chrono;
     using clock = high_resolution_clock;
+#if 0
     for (int i = 0; i < 20; i++)
         fun();
     const auto t0 = clock::now();
     for (int i = 0; i < 1000; i++)
         fun();
+#else
+    const auto t0 = clock::now();
+    fun();
+#endif
     const auto tm = clock::now() - t0;
     Debug{} << "test" << name << "took" << duration_cast<milliseconds>(tm).count() << "ms.";
 }
@@ -29,9 +35,25 @@ void test_app::test_dijkstra()
     auto w = world();
     auto a = astar();
 
+    auto metal2 = tile_image_proto{loader.tile_atlas("metal2", {2, 2}, pass_mode::blocked), 0};
+    auto& ch = w[chunk_coords_{0,0,0}];
+
+    ch[{4, 4}].wall_west()  = metal2;
+    ch[{4, 4}].wall_north() = metal2;
+#if 1
+    ch[{8, 8}].wall_west()  = metal2;
+    ch[{8, 8}].wall_north() = metal2;
+    ch[{9, 8}].wall_west()  = metal2;
+    ch[{8, 7}].wall_north() = metal2;
+#endif
+    ch.mark_modified();
+
     bench_run("Dijkstra", [&] {
-      a.Dijkstra(w, {}, 0, {{0, 0, 0}, {0, 0}}, {{1, 1, 0}, {7, 9}},
-                 1*TILE_MAX_DIM*iTILE_SIZE2.x());
+      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
     });
 }