a

8 files changed, 58 insertions, 43 deletions

diff --git a/src/global-coords.hpp b/src/global-coords.hpp
index 31481ba0..6ae3a37c 100644
--- a/src/global-coords.hpp
+++ b/src/global-coords.hpp
@@ -100,7 +100,7 @@ public:
 
     constexpr local_coords local() const noexcept;
     constexpr chunk_coords chunk() const noexcept;
-    constexpr operator chunk_coords_() const noexcept;
+    constexpr operator chunk_coords_() const noexcept; // todo make invoking this take less typing
     constexpr raw_coords_ raw() const noexcept;
     constexpr raw_coords raw() noexcept;
     constexpr int8_t z() const noexcept;
diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 5a9356f2..cbb4a8ff 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -36,26 +36,6 @@ constexpr bbox<T> bbox_union(bbox<T> bb1, bbox<T> bb2)
     return { Math::min(bb1.min, bb2.min), Math::max(bb1.max, bb2.max) };
 }
 
-constexpr auto get_bbox(chunk_coords_ ch_1, local_coords pos1, Vector2b off1,
-                        chunk_coords_ ch_2, local_coords pos2, Vector2b off2,
-                        Vector2ub size, uint32_t dist0)
-{
-    auto c = (Vector2i(ch_2.x, ch_2.y) - Vector2i(ch_1.x, ch_1.y)) * chunk_size;
-    auto t = (Vector2i(pos2) - Vector2i(pos1)) * iTILE_SIZE2;
-    auto o = Vector2i(off2) - Vector2i(off1);
-    auto cto = Vector2i(c + t + o);
-    auto dist = Math::max(1u, (uint32_t)(Vector2(cto).length() + 0.5f));
-    auto center0 = Vector2i(pos1) * iTILE_SIZE2 + Vector2i(off1);
-    auto min0 = center0 - Vector2i(size/2u), max0 = min0 + Vector2i(size);
-    auto min1 = min0 + cto, max1 = max0 + cto;
-
-    return Pair<bbox<float>, uint32_t>{
-        { .min = Vector2(Math::min(min0, min1)),
-          .max = Vector2(Math::max(max0, max1)) },
-        dist0 + dist,
-    };
-}
-
 constexpr auto directions = [] constexpr
 {
     struct pair { Vector2i dir; uint32_t len; };
@@ -165,6 +145,7 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
 
     if (!from_offset.isZero())
     {
+        // todo also add 4 subdivisions within the tile the same way
         auto bb = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
         if (path_search::is_passable(w, chunk_coords_{from}, bb, own_id, p))
         {
@@ -177,33 +158,47 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
 
     while (!Q.empty())
     {
+        fm_assert(nodes.size() < 500);
+
         std::pop_heap(Q.begin(), Q.end(), heap_comparator);
         const auto id = Q.back();
-        fm_debug_assert(id < nodes.size());
+        Q.pop_back();
 
+        fm_debug_assert(id < nodes.size());
         auto& node = nodes[id];
-        Q.pop_back();
         fm_debug_assert(node.dist != (uint32_t)-1);
 
+        Debug{} << "node" << id << node.coord.to_signed3() << node.dist;
+
         const auto bb0 = bbox_from_pos(Vector2(node.coord.local()), node.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;
+            fm_assert(len);
+            fm_assert(dist);
             if (dist >= max_dist)
                 continue;
 
-            auto [it, found_] = indexes.try_emplace({.coord = new_coord, .offset = new_offset}, (uint32_t)-1);
-            auto& new_id = it.value();
-            const auto found = found_ && it->second != (uint32_t)-1;
-            const auto cur_dist = found ? nodes[new_id].dist : (uint32_t)-1;
+            const auto sz = nodes.size();
+            auto [it, found] = 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 {
+                    .dist = dist, .prev = id,
+                    .coord = new_coord, .offset = new_offset,
+                };
+                nodes.push_back(new_node);
+            }
 
-            fm_debug_assert(!found || nodes[new_id].prev != (uint32_t)-1);
-            fm_debug_assert(!found || nodes[new_id].dist < max_dist);
+            auto& node = nodes[new_idx];
 
-            if (dist >= cur_dist)
+            if (found && dist >= node.dist)
                 continue;
+            node.dist = dist;
 
             auto e = make_edge({node.coord, node.offset}, {new_coord, new_offset});
             if (auto [it, found] = edges.try_emplace(e, edge_status::unknown); !found)
@@ -222,16 +217,19 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
                 }
             }
 
-            new_id = (uint32_t)nodes.size();
-            auto new_node = astar::visited {
-                .dist = dist, .prev = id,
-                .coord = new_coord, .offset = new_offset,
-            };
-            nodes.push_back(new_node);
+            if (!found)
+                Debug{} << " new" << new_idx << node.coord.to_signed3() << node.dist;
+            else
+                Debug{} << " old" << new_idx << node.coord.to_signed3() << node.dist;
+
+            Q.push_back(new_idx);
             std::push_heap(Q.begin(), Q.end(), heap_comparator);
+
         }
     }
 
+    Debug {} << "done";
+
     // todo...
     return result;
 }
diff --git a/src/path-search-result.cpp b/src/path-search-result.cpp
index 5b4577bf..4e921abd 100644
--- a/src/path-search-result.cpp
+++ b/src/path-search-result.cpp
@@ -26,10 +26,12 @@ path_search_result::path_search_result()
 
 path_search_result::~path_search_result() noexcept
 {
-    fm_debug_assert(_node);
-    _node->vec.clear();
-    _node->_next = std::move(_pool);
-    _pool = std::move(_node);
+    if (_node) [[likely]]
+    {
+        _node->vec.clear();
+        _node->_next = std::move(_pool);
+        _pool = std::move(_node);
+    }
 }
 
 path_search_result::path_search_result(const path_search_result& x) noexcept
diff --git a/src/path-search.cpp b/src/path-search.cpp
index c55dc0ba..c543fc9f 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -21,8 +21,6 @@ constexpr auto never_continue_ = path_search::pred{never_continue_1};
 constexpr auto always_continue_1 = [](collision_data) constexpr { return path_search_continue::pass; };
 constexpr auto always_continue_ = path_search::pred{always_continue_1};
 
-
-
 } // namespace
 
 auto path_search::never_continue() noexcept -> const pred& { return never_continue_; }
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 1d48857a..b37afa34 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -123,7 +123,7 @@ public:
 
     // 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 = (uint32_t)-1,
+                                point from, point to, uint32_t max_dist,
                                 const pred& p = path_search::never_continue());
 };
 
diff --git a/test/app.hpp b/test/app.hpp
index fbce503b..ef98a8b4 100644
--- a/test/app.hpp
+++ b/test/app.hpp
@@ -43,6 +43,7 @@ struct test_app final : private FM_APPLICATION
     static void test_path_search();
     static void test_hash();
     static void test_path_search_node_pool();
+    static void test_dijkstra();
     static void zzz_test_misc();
 };
 } // namespace floormat
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
new file mode 100644
index 00000000..d34e28bf
--- /dev/null
+++ b/test/dijkstra.cpp
@@ -0,0 +1,15 @@
+#include "app.hpp"
+#include "path-search.hpp"
+
+namespace floormat {
+
+void test_app::test_dijkstra()
+{
+    auto w = world();
+    auto a = astar();
+
+    a.Dijkstra(w, {}, 0, {{0, 0, 0}, {}}, {{1, 1, 0}, {}},
+               1*TILE_MAX_DIM*iTILE_SIZE2.x());
+}
+
+} // namespace floormat
diff --git a/test/main.cpp b/test/main.cpp
index 928ff565..57dd8fc5 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -33,6 +33,7 @@ int test_app::exec()
     test_math();
     test_hash();
     test_path_search_node_pool();
+    test_dijkstra();
     zzz_test_misc();
 
     return 0;