a

5 files changed, 72 insertions, 61 deletions

diff --git a/src/path-search-astar.cpp b/src/path-search-astar.cpp
index 6ab3753c..ba73d23c 100644
--- a/src/path-search-astar.cpp
+++ b/src/path-search-astar.cpp
@@ -14,11 +14,11 @@ size_t astar_hash::operator()(const astar_edge& e) const
 
 bool astar_edge::operator==(const astar_edge&) const noexcept = default;
 
-astar_edge::astar_edge(global_coords ch1, Vector2b off1,
-                       global_coords ch2, Vector2b off2) :
+astar_edge::astar_edge(global_coords coord1, Vector2b off1,
+                       global_coords coord2, Vector2b off2) :
     astar_edge {
-        chunk_coords_{ch1}, ch1.local(), off1,
-        chunk_coords_{ch2}, ch2.local(), off2,
+        chunk_coords_{coord1}, coord1.local(), off1,
+        chunk_coords_{coord2}, coord2.local(), off2,
     }
 {
 }
@@ -44,9 +44,9 @@ void astar::reserve(size_t count)
     seen.reserve(2*count);
 }
 
-bool astar::add_seen(const astar_edge& e)
+bool astar::add_visited(const astar_edge& value)
 {
-    return seen.insert(e).second;
+    return seen.insert(value).second;
 }
 
 void astar::push(const astar_edge& value, uint32_t dist)
@@ -55,12 +55,12 @@ void astar::push(const astar_edge& value, uint32_t dist)
     std::push_heap(Q.begin(), Q.end());
 }
 
-astar_edge astar::pop()
+astar_edge_tuple astar::pop()
 {
     fm_debug_assert(!Q.empty());
     auto ret = Q.back();
     std::pop_heap(Q.begin(), Q.end());
-    return ret.e;
+    return ret;
 }
 
 void astar::clear()
diff --git a/src/path-search-astar.hpp b/src/path-search-astar.hpp
index 59494bea..494f96c7 100644
--- a/src/path-search-astar.hpp
+++ b/src/path-search-astar.hpp
@@ -17,12 +17,11 @@ struct astar_edge
     friend struct astar_equal;
     bool operator==(const astar_edge&) const noexcept;
 
-    astar_edge(global_coords ch1, Vector2b off1, global_coords ch2, Vector2b off2);
+    astar_edge(global_coords coord1, Vector2b off1, global_coords coord2, Vector2b off2);
     astar_edge(chunk_coords_ ch1, local_coords t1, Vector2b off1,
                chunk_coords_ ch2, local_coords t2, Vector2b off2);
     size_t hash() const;
 
-private:
     int16_t from_cx, from_cy, to_cx, to_cy;
     int8_t from_cz, to_cz;
     uint8_t from_t, to_t;
@@ -44,9 +43,9 @@ struct astar final
 {
     void reserve(size_t count);
     bool empty() const { return Q.empty(); }
-    [[nodiscard]] bool add_seen(const astar_edge& value);
+    [[nodiscard]] bool add_visited(const astar_edge& value);
     void push(const astar_edge& value, uint32_t dist);
-    astar_edge pop();
+    astar_edge_tuple pop();
     void clear();
 
 private:
diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index bbd76087..eee01a47 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -1,6 +1,6 @@
 #include "path-search.hpp"
-#include "compat/math.hpp"
 #include "object.hpp"
+#include "compat/math.hpp"
 #include <Corrade/Containers/PairStl.h>
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/Math/Functions.h>
@@ -31,47 +31,62 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id
     astar.clear();
     astar.reserve(TILE_COUNT*(size_t)(subdivide_factor*subdivide_factor));
 
+    static constexpr auto eps = (uint32_t)math::ceil(math::sqrt((Vector2(div_size)/4).product()));
+    static_assert(eps > 1 && eps < TILE_SIZE2.x());
+
     const auto pos0 = Vector2(from.local()) * TILE_SIZE2;
     const auto start_bbox = bbox<float>{pos0 - Vector2(own_size/2), pos0 + Vector2(own_size)};
+    const auto from_offset_f = Vector2(from_offset);
+    const auto from_offset_len = Math::max(eps, (uint32_t)Math::ceil(from_offset_f.length()));
+
+    struct tuple
+    {
+        bbox<float> bbox;
+        uint32_t dist;
+    };
 
-    constexpr auto div = Vector2ub{subdivide_factor};
-    constexpr auto sub_size = Vector2i(Vector2ub(iTILE_SIZE2) / div);
-    constexpr auto sqrt_2 = (float)math::sqrt(2);
-
-    constexpr Vector2i directions[8] = {
-        iTILE_SIZE2 * Vector2i{ -1,  0 },
-        iTILE_SIZE2 * Vector2i{  0, -1 },
-        iTILE_SIZE2 * Vector2i{  1,  0 },
-        iTILE_SIZE2 * Vector2i{  0,  1 },
-        iTILE_SIZE2 * Vector2i{  1,  1 },
-        iTILE_SIZE2 * Vector2i{ -1, -1 },
-        iTILE_SIZE2 * Vector2i{ -1,  1 },
-        iTILE_SIZE2 * Vector2i{  1, -1 },
+    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) -> tuple
+    {
+        constexpr auto chunk_size = iTILE_SIZE2 * TILE_MAX_DIM;
+        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)Math::ceil(Vector2(cto).length()));
+        auto center0 = Vector2i(pos1) * iTILE_SIZE2 + Vector2i(off1);
+        auto min0 = center0 - Vector2i(size/2), max0 = min0 + Vector2i(size);
+        auto min1 = min0 + cto, max1 = max0 + cto;
+        fm_debug_assert(dist > eps);
+
+        return {
+            { .min = Vector2(Math::min(min0, min1)),
+              .max = Vector2(Math::max(max0, max1)) },
+            dist0 + dist,
+        };
     };
-    const auto bb0 = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
-    //if (is_passable(w, chunk_coords_{from}, bb0.min, bb0.max, own_id, p))
-    //    push_heap(_astar.Q, {from, from_offset, from, {}, 0});
 
     path_search_result result;
     fm_debug_assert(result._node); // todo
     auto& path = result._node->vec; path.clear();
 
-    {
-        auto [pos2, _offset2] = object::normalize_coords(from, from_offset, {});
-    }
+    constexpr auto div = Vector2i(subdivide_factor);
+    constexpr auto div_size = Vector2i(iTILE_SIZE2 / div);
+    constexpr auto tile_start = Vector2i(iTILE_SIZE2/-2);
 
-    for (const auto& dir : directions)
-    {
-        auto pos = object::normalize_coords(from, from_offset, dir);
-    }
+    astar.push(astar_edge{from, from_offset, from, from_offset}, 0);
+
+    const auto ch0 = chunk_coords_{from};
+    const auto bb0 = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
+    if (from_offset_len >= eps && is_passable(w, ch0, bb0, own_id, p))
+        astar.push({from, from_offset, from, from_offset}, from_offset_len);
 
     while (!astar.empty())
     {
+        auto [prev, dist0] = astar.pop();
     }
 
-    auto [cmin, cmax] = Math::minmax(Vector2i(from.chunk()) - Vector2i(1, 1),
-                                     Vector2i(to.chunk()) + Vector2i(1, 1));
-
     // todo...
     return result;
 }
diff --git a/src/path-search.cpp b/src/path-search.cpp
index e1c58742..3a193522 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -60,6 +60,7 @@ constexpr Pair<Vector2i, Vector2i> get_value(Vector2ub sz, Vector2ub div, rotati
     }
 }
 
+#if 0
 [[maybe_unused]] constexpr bool test_offsets()
 {
     constexpr auto sz_  = Vector2ub(path_search::min_size);
@@ -87,9 +88,10 @@ constexpr Pair<Vector2i, Vector2i> get_value(Vector2ub sz, Vector2ub div, rotati
 
     return true;
 }
-
 static_assert(test_offsets());
+#endif
 
+#if 0
 [[maybe_unused]] constexpr bool test_offsets2()
 {
     using enum rotation;
@@ -115,18 +117,8 @@ static_assert(test_offsets());
 
     return true;
 }
-
 static_assert(test_offsets2());
-
-constexpr auto tile_bbox(local_coords tile, Vector2b offset, Vector2ub size)
-{
-    constexpr auto tile_start = TILE_SIZE2*.5f;
-    auto size_ = Vector2(size), half_size = Vector2(size/2);
-    auto pos = tile_start + Vector2(tile) * TILE_SIZE2 + Vector2(offset);
-    auto bb = path_search::bbox<float>{pos - half_size, pos + size_};
-    return bb;
-};
-
+#endif
 
 } // namespace
 
@@ -166,13 +158,6 @@ bool path_search::is_passable_1(chunk& c, Vector2 min, Vector2 max, object_id ow
     return is_passable;
 }
 
-bool path_search::is_passable(world& w, chunk_coords_ ch0, Vector2 min, Vector2 max, object_id own_id, const pred& p)
-{
-    auto* c = w.at(ch0);
-    auto neighbors = w.neighbors(ch0);
-    return is_passable_(c, neighbors, min, max, own_id, p);
-}
-
 bool path_search::is_passable_(chunk* c0, const std::array<world::neighbor_pair, 8>& neighbors,
                                Vector2 min, Vector2 max, object_id own_id, const pred& p)
 {
@@ -212,6 +197,13 @@ bool path_search::is_passable_(chunk* c0, const std::array<world::neighbor_pair,
     return true;
 }
 
+bool path_search::is_passable(world& w, chunk_coords_ ch0, Vector2 min, Vector2 max, object_id own_id, const pred& p)
+{
+    auto* c = w.at(ch0);
+    auto neighbors = w.neighbors(ch0);
+    return is_passable_(c, neighbors, min, max, own_id, p);
+}
+
 bool path_search::is_passable(world& w, global_coords coord, Vector2b offset, Vector2ub size_,
                               object_id own_id, const pred& p)
 {
@@ -221,6 +213,11 @@ bool path_search::is_passable(world& w, global_coords coord, Vector2b offset, Ve
     return is_passable(w, coord, min, max, own_id, p);
 }
 
+bool path_search::is_passable(world& w, chunk_coords_ ch, const bbox<float>& bb, object_id own_id, const pred& p)
+{
+    return is_passable(w, ch, bb.min, bb.max, own_id, p);
+}
+
 auto path_search::neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r) -> bbox<float>
 {
     own_size = Math::max(own_size, Vector2ub(min_size));
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 4d977554..29c32a22 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -1,5 +1,4 @@
 #pragma once
-#include "tile-defs.hpp"
 #include "global-coords.hpp"
 #include "object-id.hpp"
 #include "rotation.hpp"
@@ -41,8 +40,8 @@ class path_search final
 
 public:
     static constexpr int subdivide_factor = 4;
-    static constexpr auto min_size = iTILE_SIZE2 / subdivide_factor;
-    static constexpr size_t tile_count = Vector2i(subdivide_factor * TILE_MAX_DIM).product();
+    static constexpr auto div_size = iTILE_SIZE2 / subdivide_factor;
+    static constexpr auto min_size = div_size / 2;
 
     struct neighbors final
     {
@@ -89,6 +88,7 @@ public:
                              Vector2 min, Vector2 max, object_id own_id, const pred& p = never_continue());
     static bool is_passable(world& w, chunk_coords_ ch0, Vector2 min, Vector2 max, object_id own_id, const pred& p = never_continue());
     static bool is_passable(world& w, global_coords coord, Vector2b offset, Vector2ub size, object_id own_id, const pred& p = never_continue());
+    static bool is_passable(world& w, chunk_coords_ ch0, const bbox<float>& bb, object_id own_id, const pred& p = never_continue());
 
     static bbox<float> neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r);
     template<typename T> requires std::is_arithmetic_v<T> static bbox<T> bbox_union(bbox<T> bb, Vector2i coord, Vector2b offset, Vector2ub size);