a

5 files changed, 187 insertions, 321 deletions

diff --git a/src/path-search-astar.cpp b/src/path-search-astar.cpp
deleted file mode 100644
index e8aac080..00000000
--- a/src/path-search-astar.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-#include "path-search-astar.hpp"
-#include "compat/int-hash.hpp"
-#include <utility>
-
-namespace floormat {
-
-size_t astar_hash::operator()(const astar_edge& e) const
-{
-    static_assert(sizeof e == 16);
-    if constexpr(sizeof(void*) > 4)
-        return fnvhash_64(&e, sizeof e);
-    else
-        return fnvhash_32(&e, sizeof e);
-}
-
-bool astar_edge::operator==(const astar_edge&) const noexcept = default;
-
-astar_edge::astar_edge(global_coords coord1, Vector2b off1,
-                       global_coords coord2, Vector2b off2) :
-    astar_edge {
-        chunk_coords_{coord1}, coord1.local(), off1,
-        chunk_coords_{coord2}, coord2.local(), off2,
-    }
-{
-}
-
-size_t astar_edge::hash() const
-{
-    static_assert(sizeof *this == 16);
-
-    if constexpr(sizeof nullptr > 4)
-        return fnvhash_64(this, sizeof *this);
-    else
-        return fnvhash_32(this, sizeof *this);
-}
-
-astar_edge astar_edge::swapped() const
-{
-    auto e = *this;
-    std::exchange(e.from_cx,   e.to_cx);
-    std::exchange(e.from_cy,   e.to_cy);
-    std::exchange(e.from_cz,   e.to_cz);
-    std::exchange(e.from_t,    e.to_t);
-    std::exchange(e.from_offx, e.to_offx);
-    std::exchange(e.from_offy, e.to_offy);
-    return e;
-}
-
-bool operator<(const astar_edge_tuple& a, const astar_edge_tuple& b)
-{
-    return b.dist < a.dist;
-}
-
-astar::astar()
-{
-    mins.max_load_factor(0.25f);
-    reserve(4096);
-}
-
-bool astar::add_visited(const astar_edge& value)
-{
-    if (seen.insert(value).second)
-    {
-        auto ret = seen.insert(value.swapped()).second;
-        fm_debug_assert(ret);
-        return true;
-    }
-    return false;
-}
-
-void astar::push(const astar_edge& value, uint32_t dist)
-{
-    Q.emplace_back(value, dist);
-    std::push_heap(Q.begin(), Q.end());
-}
-
-astar_edge_tuple astar::pop()
-{
-    fm_debug_assert(!Q.empty());
-    auto ret = Q.back();
-    std::pop_heap(Q.begin(), Q.end());
-    return ret;
-}
-
-void astar::reserve(size_t count)
-{
-    Q.reserve(count);
-    seen.reserve(2*count);
-}
-
-void astar::clear()
-{
-    Q.clear();
-    seen.clear();
-    mins.clear();
-}
-
-} // namespace floormat
diff --git a/src/path-search-astar.hpp b/src/path-search-astar.hpp
deleted file mode 100644
index bb3089b2..00000000
--- a/src/path-search-astar.hpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#pragma once
-#include "compat/defs.hpp"
-#include "global-coords.hpp"
-#include <vector>
-#include <tsl/robin_set.h>
-#include <tsl/robin_map.h>
-
-namespace floormat {
-
-struct astar_edge;
-
-struct astar_hash {
-    size_t operator()(const astar_edge& e) const;
-};
-
-struct astar_edge
-{
-    bool operator==(const astar_edge&) const noexcept;
-
-    fm_DECLARE_DEFAULT_COPY_ASSIGNMENT_(astar_edge);
-    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;
-    astar_edge swapped() const;
-
-    int16_t from_cx, from_cy, to_cx, to_cy;
-    int8_t from_cz, to_cz;
-    uint8_t from_t, to_t;
-    int8_t from_offx, from_offy, to_offx, to_offy;
-
-    static constexpr auto INF = (uint32_t)-1;
-};
-
-struct astar_edge_tuple
-{
-    static constexpr auto MAX = (uint32_t)-1;
-    friend bool operator<(const astar_edge_tuple& a, const astar_edge_tuple& b);
-
-    astar_edge e;
-    uint32_t dist = MAX;
-};
-
-struct astar final
-{
-    astar();
-    [[nodiscard]] bool add_visited(const astar_edge& value);
-    void push(const astar_edge& value, uint32_t dist);
-    astar_edge_tuple pop();
-
-    bool empty() const { return Q.empty(); }
-    void reserve(size_t count);
-    void clear();
-
-private:
-    struct edge_min { astar_edge prev; uint32_t len; };
-
-    static constexpr bool StoreHash = true; // todo benchmark it
-    tsl::robin_set<astar_edge,
-                   astar_hash, std::equal_to<>,
-                   std::allocator<astar_edge>,
-                   StoreHash> seen;
-    tsl::robin_map<astar_edge, edge_min, astar_hash> mins;
-    std::vector<astar_edge_tuple> Q;
-};
-
-} // namespace floormat
diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 8d877784..11aa9cfc 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -7,12 +7,117 @@
 
 namespace floormat {
 
+template<typename T> using bbox = path_search::bbox<T>;
+
+namespace {
+
+constexpr auto chunk_size = iTILE_SIZE2 * TILE_MAX_DIM;
+constexpr auto div = Vector2i(path_search::subdivide_factor);
+constexpr auto div_size = path_search::div_size;
+constexpr auto min_size = path_search::min_size;
+constexpr auto tile_start = Vector2i(iTILE_SIZE2/-2);
+constexpr auto inf =- (uint32_t)-1;
+
+template<typename T>
+requires std::is_arithmetic_v<T>
+constexpr bbox<T> bbox_union(bbox<T> bb, Vector2i coord, Vector2b offset, Vector2ub size)
+{
+    auto center = coord * iTILE_SIZE2 + Vector2i(offset);
+    auto min = center - Vector2i(size / 2);
+    auto max = center + Vector2i(size);
+    using Vec = VectorTypeFor<2, T>;
+    return {
+        .min = Math::min(Vec(bb.min), Vec(min)),
+        .max = Math::max(Vec(bb.max), Vec(max)),
+    };
+}
+
+template<typename T>
+requires std::is_arithmetic_v<T>
+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)Math::ceil(Vector2(cto).length()));
+    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 dirs = [] constexpr
+{
+    struct pair { Vector2i dir; uint32_t len; };
+    constexpr auto len1 = div_size;
+    constexpr auto len2 = (uint32_t)(math::sqrt((float)len1.dot()) + 0.5f); // NOLINT
+    std::array<pair, 8> array = {{
+        { { -1, -1 }, len2 },
+        { {  1,  1 }, len2 },
+        { { -1,  1 }, len2 },
+        { {  1, -1 }, len2 },
+        { { -1,  0 }, len1.x() },
+        { {  0, -1 }, len1.y() },
+        { {  1,  0 }, len1.x() },
+        { {  0,  1 }, len1.y() },
+    }};
+    for (auto& [vec, len] : array)
+        vec *= div_size;
+#if 0
+    for (auto i = 0uz; i < array.size(); i++)
+        for (auto j = 0uz; j < i; j++)
+            fm_assert(array[i].dir != array[j].dir);
+#endif
+    return array;
+}();
+
+template<typename T>
+requires std::is_arithmetic_v<T>
+constexpr bbox<T> bbox_from_pos(Math::Vector<2, T> pos, Vector2b offset, Vector2ub size)
+{
+    using Vec = VectorTypeFor<2, T>;
+    constexpr auto tile_size = Vec(iTILE_SIZE2);
+    const auto vec = pos * tile_size + Vec(offset);
+    const auto bb = bbox<float>{vec - Vec(size >> 1), vec + Vec(size)};
+    return bb;
+}
+
+} // namespace
+
+size_t astar::hash_op::operator()(position coord) const
+{
+    static_assert(sizeof(global_coords) == 8);
+    if constexpr(sizeof nullptr > 4)
+        return fnvhash_64(&coord, sizeof coord);
+    else
+        return fnvhash_32(&coord, sizeof coord);
+}
+
 path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id own_id,
                                          global_coords from, Vector2b from_offset,
                                          global_coords to, Vector2b to_offset,
                                          const pred& p)
 {
-    fm_assert(from.x <= to.x && from.y <= to.y);
+    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];
+        return n2.dist < n1.dist;
+    };
+
     own_size = Math::max(own_size, Vector2ub(min_size));
 
     if (from.z() != to.z()) [[unlikely]]
@@ -29,110 +134,44 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id
         return {};
 
     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> bb;
-        uint32_t dist;
-    };
+    fm_debug_assert(astar.nodes.empty());
 
-    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 start_bbox = bbox_from_pos(Vector2(from.local()), from_offset, own_size);
+    const auto from_offset_len = Math::max(1u, (uint32_t)Math::ceil(Vector2(from_offset).length()));
 
     path_search_result result;
     fm_debug_assert(result._node); // todo
     auto& path = result._node->vec; path.clear();
 
-    constexpr auto div = Vector2i(subdivide_factor);
-    constexpr auto div_size = Vector2i(iTILE_SIZE2 / div);
-    constexpr auto tile_start = Vector2i(iTILE_SIZE2/-2);
-
-    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);
+    astar.indexes[{from, from_offset}] = 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);
 
-    struct pair { Vector2i dir; uint32_t len; };
-
-    constexpr auto dirs = [] constexpr -> std::array<pair, 8> {
-        constexpr auto len1 = path_search::div_size;
-        constexpr auto len2 = (uint32_t)(math::sqrt((float)len1.dot()) + 0.5f); // NOLINT
-        std::array<pair, 8> array = {{
-            { { -1, -1 }, len2 },
-            { {  1,  1 }, len2 },
-            { { -1,  1 }, len2 },
-            { {  1, -1 }, len2 },
-            { { -1,  0 }, len1.x() },
-            { {  0, -1 }, len1.y() },
-            { {  1,  0 }, len1.x() },
-            { {  0,  1 }, len1.y() },
-        }};
-        for (auto& [vec, len] : array)
-            vec *= path_search::div_size;
-#if 0
-        for (auto i = 0uz; i < array.size(); i++)
-            for (auto j = 0uz; j < i; j++)
-                fm_assert(array[i].dir != array[j].dir);
-#endif
-        return array;
-    }();
-
-    while (!astar.empty())
+    if (!from_offset.isZero())
     {
-        auto [cur, dist0] = astar.pop();
-        if (!astar.add_visited(cur))
-            continue;
-        for (auto [dir, len] : dirs)
+        auto bb = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
+        if (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);
         }
     }
 
+
+
     // todo...
     return result;
 }
 
-path_search_result path_search::Dijkstra(world& w, const object& obj, global_coords to, Vector2b to_offset, const pred& p)
+path_search_result path_search::Dijkstra(world& w, const object& obj,
+                                         global_coords to, Vector2b to_offset,
+                                         const pred& p)
 {
-    constexpr auto full_tile = Vector2ub(iTILE_SIZE2*3/4);
-    auto size = Math::max(obj.bbox_size, full_tile);
-
-    // todo fixme
-    // if bbox_offset is added to obj's offset, then all coordinates in the paths are shifted by bbox_offset.
-    // maybe add handling to subtract bbox_offset from the returned path.
-    // for that it needs to be passed into callees separately.
     fm_assert(obj.bbox_offset.isZero());
-    return Dijkstra(w, size, obj.id, obj.coord, obj.offset, to, to_offset, p);
+    return Dijkstra(w, obj.bbox_size, obj.id, obj.coord, obj.offset, to, to_offset, p);
 }
 
 } // namespace floormat
diff --git a/src/path-search.cpp b/src/path-search.cpp
index 3a193522..8ca930af 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -122,22 +122,12 @@ static_assert(test_offsets2());
 
 } // 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_; }
 
-astar_edge::astar_edge(chunk_coords_ ch1, local_coords t1, Vector2b off1,
-                       chunk_coords_ ch2, local_coords t2, Vector2b off2) :
-    from_cx{ch1.x},         from_cy{ch1.y},
-    to_cx{ch2.x},           to_cy{ch2.y},
-    from_cz{ch1.z},         to_cz{ch2.z},
-    from_t{t1.to_index()},  to_t{t2.to_index()},
-    from_offx{off1.x()},    from_offy{off1.y()},
-    to_offx{off2.x()},      to_offy{off2.y()}
-{
-
-}
-
 bool path_search::is_passable_1(chunk& c, Vector2 min, Vector2 max, object_id own_id, const pred& p)
 {
     auto& rt = *c.rtree();
@@ -246,9 +236,9 @@ auto path_search::neighbor_tiles(world& w, global_coords coord, Vector2ub size,
         { { -1,  0 }, W },
     };
 
-    for (auto [off, dir] : nbs)
+    for (auto [off, r] : nbs)
     {
-        auto [min, max] = neighbor_tile_bbox(pos, size, { 1, 1 }, dir);
+        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, {} };
     }
@@ -256,26 +246,4 @@ auto path_search::neighbor_tiles(world& w, global_coords coord, Vector2ub size,
     return ns;
 }
 
-template<typename T = float>
-requires std::is_arithmetic_v<T>
-auto path_search::bbox_union(bbox<T> bb, Vector2i coord, Vector2b offset, Vector2ub size) -> bbox<T>
-{
-    auto center = coord * iTILE_SIZE2 + Vector2i(offset);
-    auto min = center - Vector2i(size / 2);
-    auto max = center + Vector2i(size);
-    using Vec = VectorTypeFor<2, T>;
-    return {
-        .min = Math::min(Vec(bb.min), Vec(min)),
-        .max = Math::max(Vec(bb.max), Vec(max)),
-    };
-}
-
-template auto path_search::bbox_union(bbox<int> bb, Math::Vector2<int> coord, Vector2b offset, Vector2ub size) -> bbox<int>;
-template auto path_search::bbox_union(bbox<float> bb, Vector2i coord, Vector2b offset, Vector2ub size) -> bbox<float>;
-
-auto path_search::bbox_union(bbox<int> bb1, bbox<int> bb2) -> bbox<int>
-{
-    return { Math::min(bb1.min, bb2.min), Math::max(bb1.max, bb2.max) };
-}
-
 } // namespace floormat
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 29c32a22..5a8a0f9e 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -4,16 +4,18 @@
 #include "rotation.hpp"
 #include "world.hpp"
 #include "compat/function2.fwd.hpp"
-#include "path-search-astar.hpp"
 #include "path-search-result.hpp"
+#include "compat/int-hash.hpp"
+#include <bit>
 #include <array>
-#include <Corrade/Containers/Array.h>
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/DimensionTraits.h>
+#include <tsl/robin_hash.h>
 
 namespace Corrade::Containers {
-template<typename T> class Optional;
-template<typename T, typename U> class Pair;
+//template<typename T> class Optional;
+//template<typename T, typename U> class Pair;
+template<typename T> class ArrayView;
 } // namespace Corrade::Containers
 
 namespace floormat {
@@ -23,21 +25,60 @@ struct object;
 struct chunk;
 
 struct path_search_result;
-class path_search;
-
-struct astar_edge;
-struct astar_hash;
-struct astar;
 
 enum class path_search_continue : bool { pass = false, blocked = true };
 
+struct astar
+{
+    fm_DECLARE_DELETED_COPY_ASSIGNMENT(astar);
+
+    struct position
+    {
+        global_coords coord;
+        Vector2b offset;
+
+        bool operator==(const position&) const = default;
+    };
+    struct visited
+    {
+        uint32_t dist = (uint32_t)-1;
+        uint32_t prev = (uint32_t)-1;
+        global_coords coord;
+        Vector2b offset;
+        bool expanded = false;
+    };
+    struct hash_op
+    {
+        size_t operator()(position coord) const;
+    };
+    void reserve(size_t capacity)
+    {
+        nodes.reserve(capacity);
+        indexes.reserve(capacity);
+        Q.reserve(capacity);
+    }
+    astar()
+    {
+        constexpr auto capacity = TILE_COUNT * 16;
+        indexes.max_load_factor(.4f);
+        reserve(capacity);
+    }
+    void clear()
+    {
+        nodes.clear();
+        indexes.clear();
+        Q.clear();
+    }
+
+    std::vector<visited> nodes;
+    tsl::robin_map<position, uint32_t, hash_op> indexes;
+    std::vector<uint32_t> Q;
+};
+
 class path_search final
 {
     friend struct path_search_result;
 
-    // todo bucketize by array length
-    struct astar astar;
-
 public:
     static constexpr int subdivide_factor = 4;
     static constexpr auto div_size = iTILE_SIZE2 / subdivide_factor;
@@ -54,28 +95,12 @@ public:
         operator ArrayView<const global_coords>() const;
     };
 
-#if 0
-    struct chunk_tiles_cache
-    {
-        std::bitset<tile_count> can_go_north{true}, can_go_west{true};
-    };
-
-    struct chunk_cache
-    {
-        Array<chunk_tiles_cache> array;
-        Vector2i start, size; // in chunks
-    };
-#endif
-
-    struct obj_position { Vector2 center, size; };
-
     template<typename T> struct bbox { VectorTypeFor<2, T> min, max; };
 
-#if 0
-    chunk_cache cache;
-#endif
-
     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;
 
@@ -90,9 +115,8 @@ public:
     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);
-    template<typename T> requires std::is_arithmetic_v<T> static bbox<T> bbox_union(bbox<T> bb, Vector2i coord, Vector2b offset, Vector2ub size);
-    static bbox<int> bbox_union(bbox<int> bb1, bbox<int> bb2);
     static neighbors neighbor_tiles(world& w, global_coords coord, Vector2ub size, object_id own_id, const pred& p = never_continue());
 };