a

4 files changed, 121 insertions, 251 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 2fbc11e0..848f156a 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -113,32 +113,23 @@ size_t astar::point_hash::operator()(point pt) const
 
 size_t astar::edge_hash::operator()(const edge& e) const
 {
-    constexpr auto len = sizeof e;
-    static_assert(len == 8 + 8 + 2 + 2);
+    static_assert(sizeof e == 8 + 8 + 2 + 2);
 #ifdef FLOORMAT_64
     static_assert(sizeof nullptr > 4);
-    return fnvhash_64(&e, len);
+    return fnvhash_64(&e, sizeof e);
 #else
     static_assert(sizeof nullptr == 4);
-    return fnvhash_32(&e, len);
+    return fnvhash_32(&e, sizeof e);
 #endif
 }
 
-constexpr astar::edge make_edge(const point& a, const point& b)
+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)
 {
-    if (a < b)
-        return { a.coord, b.coord, a.offset, b.offset };
-    else
-        return { b.coord, a.coord, b.offset, a.offset };
-}
-
-path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, const object_id own_id,
-                                         const point from_, const point to_, const pred& p)
-{
-    auto heap_comparator = [&A = astar](uint32_t a, uint32_t b) {
-        fm_debug_assert(std::max(a, b) < A.nodes.size());
-        const auto& n1 = A.nodes[a];
-        const auto& n2 = A.nodes[b];
+    auto heap_comparator = [this](uint32_t a, uint32_t b) {
+        fm_debug_assert(std::max(a, b) < nodes.size());
+        const auto& n1 = nodes[a];
+        const auto& n2 = nodes[b];
         return n2.dist < n1.dist;
     };
 
@@ -154,14 +145,14 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, const obj
     if (from.z() != 0) [[unlikely]]
         return {};
 
-    if (!is_passable(w, from, from_offset, own_size, own_id, p))
+    if (!path_search::is_passable(w, from, from_offset, own_size, own_id, p))
         return {};
 
-    if (!is_passable(w, to, to_offset, own_size, own_id, p))
+    if (!path_search::is_passable(w, to, to_offset, own_size, own_id, p))
         return {};
 
-    astar.clear();
-    fm_debug_assert(astar.nodes.empty());
+    clear();
+    fm_debug_assert(nodes.empty());
 
     const auto start_bbox = bbox_from_pos(Vector2(from.local()), from_offset, own_size);
     const auto from_offset_len = Math::max(1u, (uint32_t)(Vector2(from_offset).length() + 0.5f));
@@ -170,67 +161,77 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, const obj
     fm_debug_assert(result._node); // todo
     auto& path = result._node->vec; path.clear();
 
-    astar.indexes[from_] = 0;
-    astar.nodes.push_back({.dist = 0, .coord = from, .offset = from_offset });
-    astar.Q.push_back(0);
-    std::push_heap(astar.Q.begin(), astar.Q.end(), heap_comparator);
+    indexes[from_] = 0;
+    nodes.push_back({.dist = 0, .coord = from, .offset = from_offset });
+    Q.push_back(0);
+    std::push_heap(Q.begin(), Q.end(), heap_comparator);
 
     if (!from_offset.isZero())
     {
         auto bb = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
-        if (is_passable(w, chunk_coords_{from}, bb, own_id, p))
+        if (path_search::is_passable(w, chunk_coords_{from}, bb, own_id, p))
         {
-            astar.indexes[{from, {}}] = 1;
-            astar.nodes.push_back({.dist = from_offset_len, .prev = 0, .coord = from, .offset = {}});
-            astar.Q.push_back(1);
-            std::push_heap(astar.Q.begin(), astar.Q.end(), heap_comparator);
+            indexes[{from, {}}] = 1;
+            nodes.push_back({.dist = from_offset_len, .prev = 0, .coord = from, .offset = {}});
+            Q.push_back(1);
+            std::push_heap(Q.begin(), Q.end(), heap_comparator);
         }
     }
 
-    while (!astar.Q.empty())
+    while (!Q.empty())
     {
-        std::pop_heap(astar.Q.begin(), astar.Q.end(), heap_comparator);
-        const auto id = astar.Q.back();
-        fm_debug_assert(id < astar.nodes.size());
-        auto& node = astar.nodes[id];
-        astar.Q.pop_back();
-        const auto bb0 = bbox_from_pos(Vector2(node.coord.local()), node.offset, own_size);
+        std::pop_heap(Q.begin(), Q.end(), heap_comparator);
+        const auto id = Q.back();
+        fm_debug_assert(id < nodes.size());
 
-        node.expanded = true;
+        auto& node = nodes[id];
+        Q.pop_back();
+        fm_debug_assert(node.dist != (uint32_t)-1);
+
+        const auto bb0 = bbox_from_pos(Vector2(node.coord.local()), node.offset, own_size);
 
         for (auto [vec, len] : directions)
         {
-            auto vec_ = Vector2(vec);
-            const auto new_dist = node.dist + len;
-            auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
-            const auto bb = bbox_union(bb1, bb0);
-
-            if (!is_passable(w, node.coord, bb, own_id, p))
+            auto [new_coord, new_offset] = object::normalize_coords(node.coord, node.offset, vec);
+            const auto dist = node.dist + len;
+            if (dist >= max_dist)
                 continue;
 
-            auto [new_coord, new_offset] = object::normalize_coords(node.coord, node.offset, vec);
+            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;
+
+            fm_debug_assert(!found || nodes[new_id].prev != (uint32_t)-1);
+            fm_debug_assert(!found || nodes[new_id].dist < max_dist);
 
-            if (!astar.edges.insert(make_edge({node.coord, node.offset}, {new_coord, new_offset})).second)
+            if (dist >= cur_dist)
                 continue;
 
-            auto [it, found] = astar.indexes.try_emplace({.coord = new_coord, .offset = new_offset}, (uint32_t)-1);
-            if (!found)
+            auto e = make_edge({node.coord, node.offset}, {new_coord, new_offset});
+            if (auto [it, found] = edges.try_emplace(e, edge_status::unknown); !found)
             {
-                const auto new_id = (uint32_t)astar.nodes.size();
-                it.value() = new_id;
-                auto new_node = astar::visited {
-                    .dist = new_dist, .prev = id,
-                    .coord = new_coord, .offset = new_offset,
-                    .expanded = false,
-                };
-                astar.nodes.push_back(new_node);
+                auto& status = it.value();
+                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_(node.coord), bb, own_id, p))
+                    status = edge_status::good;
+                else
+                {
+                    status = edge_status::bad;
+                    continue;
+                }
             }
-            else if (new_dist >= node.dist)
-                continue;
-            else
-            {
 
-            }
+            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);
+            std::push_heap(Q.begin(), Q.end(), heap_comparator);
         }
     }
 
diff --git a/src/path-search-result.hpp b/src/path-search-result.hpp
index be7da559..c7dde85e 100644
--- a/src/path-search-result.hpp
+++ b/src/path-search-result.hpp
@@ -9,6 +9,7 @@ namespace floormat {
 struct path_search_result final
 {
     friend class path_search;
+    friend class astar;
     friend struct test_app;
 
     struct pair { global_coords pos; Vector2 offset; };
diff --git a/src/path-search.cpp b/src/path-search.cpp
index 8ca930af..c55dc0ba 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -21,110 +21,10 @@ 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};
 
-constexpr Pair<Vector2i, Vector2i> get_value(Vector2ub sz, Vector2ub div, rotation r)
-{
-    const int offset_W = iTILE_SIZE2.x()/(int)div.x(), offset_N = iTILE_SIZE2.y()/(int)div.y();
-
-    const auto r_ = (uint8_t)r;
-    CORRADE_ASSUME(r_ <= (uint8_t)rotation_COUNT);
-
-    switch (r_)
-    {
-    case (uint8_t)rotation::N: {
-        auto min_N = Vector2i(-sz.x()/2,                        -offset_N - sz.y()/2            );
-        auto max_N = Vector2i(min_N.x() + sz.x(),               sz.y() - sz.y()/2               );
-        return {min_N, max_N};
-    }
-    case (uint8_t)rotation::S: {
-        auto min_S = Vector2i(-sz.x()/2,                        -sz.y()                         );
-        auto max_S = Vector2i(min_S.x() + sz.x(),               offset_N + sz.y() - sz.y()/2    );
-        return {min_S, max_S};
-    }
-    case (uint8_t)rotation::W: {
-        auto min_W = Vector2i(-offset_W - sz.x()/2,             -sz.y()/2                       );
-        auto max_W = Vector2i(sz.x() - sz.x()/2,                min_W.y() + sz.y()              );
-        return {min_W, max_W};
-    }
-    case (uint8_t)rotation::E: {
-        auto min_E = Vector2i(-sz.x()/2,                        -sz.y()/2                       );
-        auto max_E = Vector2i(offset_W + sz.x() - sz.x()/2,     min_E.y() + sz.y()              );
-        return {min_E, max_E};
-    }
-    case (uint8_t)rotation_COUNT: {
-        auto min_C = Vector2i(-sz.x()/2,                        -sz.y()/2                       );
-        auto max_C = min_C + Vector2i(sz);
-        return {min_C, max_C};
-    }
-    default:
-        fm_abort("wrong 4-way rotation enum '%d'", (int)r);
-    }
-}
-
-#if 0
-[[maybe_unused]] constexpr bool test_offsets()
-{
-    constexpr auto sz_  = Vector2ub(path_search::min_size);
-    constexpr Vector2i shift = Vector2i(0, 0) * iTILE_SIZE2 + Vector2i(0, 0);
-
-    [[maybe_unused]] constexpr auto N = get_value(sz_, {1,1}, rotation::N);
-    [[maybe_unused]] constexpr auto min_N = N.first() + shift, max_N = N.second() + shift;
-    [[maybe_unused]] constexpr auto N_min_x = min_N.x(), N_min_y = min_N.y();
-    [[maybe_unused]] constexpr auto N_max_x = max_N.x(), N_max_y = max_N.y();
-
-    [[maybe_unused]] constexpr auto E = get_value(sz_, {1,1}, rotation::E);
-    [[maybe_unused]] constexpr auto min_E = E.first() + shift, max_E = E.second() + shift;
-    [[maybe_unused]] constexpr auto E_min_x = min_E.x(), E_min_y = min_E.y();
-    [[maybe_unused]] constexpr auto E_max_x = max_E.x(), E_max_y = max_E.y();
-
-    [[maybe_unused]] constexpr auto S = get_value(sz_, {1,1}, rotation::S);
-    [[maybe_unused]] constexpr auto min_S = S.first() + shift, max_S = S.second() + shift;
-    [[maybe_unused]] constexpr auto S_min_x = min_S.x(), S_min_y = min_S.y();
-    [[maybe_unused]] constexpr auto S_max_x = max_S.x(), S_max_y = max_S.y();
-
-    [[maybe_unused]] constexpr auto W = get_value(sz_, {1,1}, rotation::W);
-    [[maybe_unused]] constexpr auto min_W = W.first() + shift, max_W = W.second() + shift;
-    [[maybe_unused]] constexpr auto W_min_x = min_W.x(), W_min_y = min_W.y();
-    [[maybe_unused]] constexpr auto W_max_x = max_W.x(), W_max_y = max_W.y();
-
-    return true;
-}
-static_assert(test_offsets());
-#endif
 
-#if 0
-[[maybe_unused]] constexpr bool test_offsets2()
-{
-    using enum rotation;
-    constexpr auto tile_start = iTILE_SIZE2/-2;
-    constexpr auto sz = Vector2ub(8, 16);
-
-    {
-        constexpr auto bb = get_value(sz, Vector2ub(div), N);
-        constexpr auto min = tile_start + bb.first(), max = tile_start + bb.second();
-        static_assert(min.x() == -32 - sz.x()/2);
-        static_assert(max.x() == -32 + sz.x()/2);
-        static_assert(min.y() == -48 - sz.y()/2);
-        static_assert(max.y() == -32 + sz.y()/2);
-    }
-    {
-        constexpr auto bb = get_value(sz, Vector2ub(div), W);
-        constexpr auto min = tile_start + bb.first(), max = tile_start + bb.second();
-        static_assert(min.x() == -32 - 16 - sz.x()/2);
-        static_assert(max.x() == -32 + sz.x()/2);
-        static_assert(min.y() == -32 - sz.y()/2);
-        static_assert(max.y() == -32 + sz.y()/2);
-    }
-
-    return true;
-}
-static_assert(test_offsets2());
-#endif
 
 } // namespace
 
-
-
-path_search::neighbors::operator ArrayView<const global_coords>() const { return {data.data(), size}; }
 auto path_search::never_continue() noexcept -> const pred& { return never_continue_; }
 auto path_search::always_continue() noexcept -> const pred& { return always_continue_; }
 
@@ -208,42 +108,4 @@ bool path_search::is_passable(world& w, chunk_coords_ ch, const bbox<float>& bb,
     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));
-    const auto shift = coord * iTILE_SIZE2;
-    auto [min, max] = get_value(own_size, div, r);
-    return { Vector2(min + shift), Vector2(max + shift) };
-}
-
-auto path_search::neighbor_tiles(world& w, global_coords coord, Vector2ub size, object_id own_id, const pred& p) -> neighbors
-{
-    auto ch = chunk_coords_{ coord.chunk(), coord.z() };
-    auto pos = Vector2i(coord.local());
-    constexpr auto min_size = Vector2ub(iTILE_SIZE2/4);
-    size = Math::max(size, min_size);
-
-    neighbors ns;
-
-    using enum rotation;
-    constexpr struct {
-        Vector2i off;
-        rotation r = {};
-    } nbs[] = {
-        { {  0, -1 }, N },
-        { {  1,  0 }, E },
-        { {  0,  1 }, S },
-        { { -1,  0 }, W },
-    };
-
-    for (auto [off, r] : nbs)
-    {
-        auto [min, max] = neighbor_tile_bbox(pos, size, { 1, 1 }, r);
-        if (is_passable(w, ch, min, max, own_id, p))
-            ns.data[ns.size++] = { coord + off, {} };
-    }
-
-    return ns;
-}
-
 } // namespace floormat
diff --git a/src/path-search.hpp b/src/path-search.hpp
index adcda8a0..1d48857a 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -24,21 +24,44 @@ namespace floormat {
 struct world;
 struct object;
 struct chunk;
-struct path_search_result;
 
+// todo add pathfinding sub-namespace
+
+struct path_search_result;
 enum class path_search_continue : bool { pass = false, blocked = true };
 
-struct astar
+class path_search final
 {
-    fm_DECLARE_DELETED_COPY_ASSIGNMENT(astar);
+    friend struct path_search_result;
+
+public:
+    static constexpr int subdivide_factor = 4;
+    static constexpr auto div_size = iTILE_SIZE2 / subdivide_factor;
+    static constexpr auto min_size = div_size / 2;
+
+    template<typename T> struct bbox { VectorTypeFor<2, T> min, max; };
+
+    using pred = fu2::function_view<path_search_continue(collision_data) const>;
+
+    static const pred& never_continue() noexcept;
+    static const pred& always_continue() noexcept;
+
+    static bool is_passable_1(chunk& c, Vector2 min, Vector2 max, object_id own_id, const pred& p = never_continue());
+    static bool is_passable_(chunk* c0, const std::array<world::neighbor_pair, 8>& neighbors,
+                             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());
+};
 
+class astar
+{
     struct visited
     {
         uint32_t dist = (uint32_t)-1;
         uint32_t prev = (uint32_t)-1;
         global_coords coord;
         Vector2b offset;
-        bool expanded = false;
 
     };
     struct edge
@@ -49,13 +72,39 @@ struct astar
         bool operator==(const edge& other) const;
     };
 
+    enum class edge_status : uint8_t
+    {
+        // good, bad, I'm the man with the gun.
+        unknown, good, bad,
+    };
+
     struct point_hash { size_t operator()(point pt) const; };
     struct edge_hash { size_t operator()(const edge& e) const; };
 
+    std::vector<visited> nodes;
+    tsl::robin_map<edge, edge_status, edge_hash> edges;
+    tsl::robin_map<point, uint32_t, point_hash> indexes;
+    std::vector<uint32_t> Q;
+
+    using pred = path_search::pred;
+    template<typename T> using bbox = path_search::bbox<T>;
+
+public:
+    fm_DECLARE_DELETED_COPY_ASSIGNMENT(astar);
+
+    static edge make_edge(const point& a, const point& b)
+    {
+        if (a < b)
+            return { a.coord, b.coord, a.offset, b.offset };
+        else
+            return { b.coord, a.coord, b.offset, a.offset };
+    }
+
     void reserve(size_t capacity)
     {
         nodes.reserve(capacity);
         indexes.reserve(capacity);
+        edges.reserve(capacity);
         Q.reserve(capacity);
     }
     astar()
@@ -68,57 +117,14 @@ struct astar
     {
         nodes.clear();
         indexes.clear();
+        edges.clear();
         Q.clear();
     }
 
-    std::vector<visited> nodes;
-    tsl::robin_set<edge, edge_hash> edges;
-    tsl::robin_map<point, uint32_t, point_hash> indexes;
-    std::vector<uint32_t> Q;
-};
-
-class path_search final
-{
-    friend struct path_search_result;
-
-public:
-    static constexpr int subdivide_factor = 4;
-    static constexpr auto div_size = iTILE_SIZE2 / subdivide_factor;
-    static constexpr auto min_size = div_size / 2;
-
-    struct neighbors final
-    {
-        auto begin() const { return data.data(); }
-        auto end() const { return data.data() + size; }
-
-        std::array<global_coords, 5> data;
-        uint8_t size = 0;
-
-        operator ArrayView<const global_coords>() const;
-    };
-
-    template<typename T> struct bbox { VectorTypeFor<2, T> min, max; };
-
-    using pred = fu2::function_view<path_search_continue(collision_data) const>;
-
-    struct astar astar;
-
-    static const pred& never_continue() noexcept;
-    static const pred& always_continue() noexcept;
-
     // todo add simple bresenham short-circuit
-    path_search_result Dijkstra(world& w, Vector2ub own_size, object_id own_id, point from, point to, const pred& p = never_continue());
-
-    static bool is_passable_1(chunk& c, Vector2 min, Vector2 max, object_id own_id, const pred& p = never_continue());
-    static bool is_passable_(chunk* c0, const std::array<world::neighbor_pair, 8>& neighbors,
-                             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());
-
-    // todo move to test/path-search.cpp
-    static bbox<float> neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r);
-    static neighbors neighbor_tiles(world& w, global_coords coord, Vector2ub size, object_id own_id, const pred& p = never_continue());
+    path_search_result Dijkstra(world& w, Vector2ub own_size, object_id own_id,
+                                point from, point to, uint32_t max_dist = (uint32_t)-1,
+                                const pred& p = path_search::never_continue());
 };
 
 } // namespace floormat