wip

10 files changed, 280 insertions, 142 deletions

diff --git a/compat/int-hash.cpp b/compat/int-hash.cpp
index a15fae29..0ab83c6c 100644
--- a/compat/int-hash.cpp
+++ b/compat/int-hash.cpp
@@ -34,6 +34,32 @@ fm_UNROLL_8
 
 } // namespace
 
+uint64_t fnvhash_64(const void* str_, size_t size)
+{
+    const auto *str = (const char*)str_, *end = str + size;
+    uint32_t hash = 0x811c9dc5u;
+fm_UNROLL_4
+    for (; str != end; ++str)
+    {
+        hash *= 0x01000193u;
+        hash ^= (uint8_t)*str;
+    }
+    return hash;
+}
+
+uint64_t fnvhash_32(const void* str_, size_t size)
+{
+    const auto *str = (const char*)str_, *end = str + size;
+    uint64_t hash = 0xcbf29ce484222325u;
+fm_UNROLL_4
+    for (; str != end; ++str)
+    {
+        hash *= 0x100000001b3u;
+        hash ^= (uint8_t)*str;
+    }
+    return hash;
+}
+
 size_t int_hash(uint32_t x) noexcept
 {
     if constexpr(sizeof(size_t) == 4)
diff --git a/compat/int-hash.hpp b/compat/int-hash.hpp
index 2266bb4d..6a6cbcd4 100644
--- a/compat/int-hash.hpp
+++ b/compat/int-hash.hpp
@@ -5,4 +5,7 @@ namespace floormat {
 size_t int_hash(uint32_t x) noexcept;
 size_t int_hash(uint64_t x) noexcept;
 
+uint64_t fnvhash_64(const void* str, size_t size);
+uint64_t fnvhash_32(const void* str, size_t size);
+
 } // namespace floormat
diff --git a/src/path-search-astar.cpp b/src/path-search-astar.cpp
new file mode 100644
index 00000000..6ab3753c
--- /dev/null
+++ b/src/path-search-astar.cpp
@@ -0,0 +1,72 @@
+#include "path-search-astar.hpp"
+#include "compat/int-hash.hpp"
+
+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 ch1, Vector2b off1,
+                       global_coords ch2, Vector2b off2) :
+    astar_edge {
+        chunk_coords_{ch1}, ch1.local(), off1,
+        chunk_coords_{ch2}, ch2.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);
+}
+
+bool operator<(const astar_edge_tuple& a, const astar_edge_tuple& b)
+{
+    return b.dist < a.dist;
+}
+
+void astar::reserve(size_t count)
+{
+    Q.reserve(count);
+    seen.reserve(2*count);
+}
+
+bool astar::add_seen(const astar_edge& e)
+{
+    return seen.insert(e).second;
+}
+
+void astar::push(const astar_edge& value, uint32_t dist)
+{
+    Q.emplace_back(value, dist);
+    std::push_heap(Q.begin(), Q.end());
+}
+
+astar_edge astar::pop()
+{
+    fm_debug_assert(!Q.empty());
+    auto ret = Q.back();
+    std::pop_heap(Q.begin(), Q.end());
+    return ret.e;
+}
+
+void astar::clear()
+{
+    Q.clear();
+    seen.clear();
+}
+
+} // namespace floormat
diff --git a/src/path-search-astar.hpp b/src/path-search-astar.hpp
new file mode 100644
index 00000000..59494bea
--- /dev/null
+++ b/src/path-search-astar.hpp
@@ -0,0 +1,61 @@
+#pragma once
+#include "global-coords.hpp"
+#include <vector>
+
+#include <tsl/robin_set.h>
+
+namespace floormat {
+
+struct astar_edge;
+
+struct astar_hash {
+    size_t operator()(const astar_edge& e) const;
+};
+
+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(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;
+    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
+{
+    void reserve(size_t count);
+    bool empty() const { return Q.empty(); }
+    [[nodiscard]] bool add_seen(const astar_edge& value);
+    void push(const astar_edge& value, uint32_t dist);
+    astar_edge pop();
+    void clear();
+
+private:
+    static constexpr bool StoreHash = true; // todo benchmark it
+    tsl::robin_set<astar_edge,
+                   astar_hash, std::equal_to<>,
+                   std::allocator<astar_edge>,
+                   StoreHash> seen;
+    std::vector<astar_edge_tuple> Q;
+};
+
+} // namespace floormat
diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
new file mode 100644
index 00000000..bbd76087
--- /dev/null
+++ b/src/path-search-dijkstra.cpp
@@ -0,0 +1,94 @@
+#include "path-search.hpp"
+#include "compat/math.hpp"
+#include "object.hpp"
+#include <Corrade/Containers/PairStl.h>
+#include <Magnum/Math/Vector2.h>
+#include <Magnum/Math/Functions.h>
+
+namespace floormat {
+
+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);
+    own_size = Math::max(own_size, Vector2ub(min_size));
+
+    if (from.z() != to.z()) [[unlikely]]
+        return {};
+
+    // todo try removing this eventually
+    if (from.z() != 0) [[unlikely]]
+        return {};
+
+    if (!is_passable(w, from, from_offset, own_size, own_id, p))
+        return {};
+
+    if (!is_passable(w, to, to_offset, own_size, own_id, p))
+        return {};
+
+    astar.clear();
+    astar.reserve(TILE_COUNT*(size_t)(subdivide_factor*subdivide_factor));
+
+    const auto pos0 = Vector2(from.local()) * TILE_SIZE2;
+    const auto start_bbox = bbox<float>{pos0 - Vector2(own_size/2), pos0 + Vector2(own_size)};
+
+    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 },
+    };
+    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, {});
+    }
+
+    for (const auto& dir : directions)
+    {
+        auto pos = object::normalize_coords(from, from_offset, dir);
+    }
+
+    while (!astar.empty())
+    {
+    }
+
+    auto [cmin, cmax] = Math::minmax(Vector2i(from.chunk()) - Vector2i(1, 1),
+                                     Vector2i(to.chunk()) + Vector2i(1, 1));
+
+    // todo...
+    return result;
+}
+
+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);
+}
+
+} // namespace floormat
diff --git a/src/path-search-result.cpp b/src/path-search-result.cpp
index fb2c41ca..5b4577bf 100644
--- a/src/path-search-result.cpp
+++ b/src/path-search-result.cpp
@@ -51,16 +51,16 @@ path_search_result& path_search_result::operator=(const path_search_result& x) n
 
 path_search_result::node::node() noexcept = default;
 size_t path_search_result::size() const { return _node->vec.size(); }
-const global_coords* path_search_result::data() const { return _node->vec.data(); }
+auto path_search_result::data() const -> const pair* { return _node->vec.data(); }
 path_search_result::operator bool() const { return !_node->vec.empty(); }
 
-path_search_result::operator ArrayView<const global_coords>() const
+path_search_result::operator ArrayView<const pair>() const
 {
     fm_debug_assert(_node);
     return {_node->vec.data(), _node->vec.size()};
 }
 
-const global_coords& path_search_result::operator[](size_t index) const
+auto path_search_result::operator[](size_t index) const -> const pair&
 {
     fm_debug_assert(_node);
     fm_debug_assert(index < _node->vec.size());
diff --git a/src/path-search-result.hpp b/src/path-search-result.hpp
index fcd116fc..be7da559 100644
--- a/src/path-search-result.hpp
+++ b/src/path-search-result.hpp
@@ -11,11 +11,13 @@ struct path_search_result final
     friend class path_search;
     friend struct test_app;
 
-    const global_coords* data() const;
-    const global_coords& operator[](size_t index) const;
+    struct pair { global_coords pos; Vector2 offset; };
+
+    const pair* data() const;
+    const pair& operator[](size_t index) const;
     size_t size() const;
 
-    explicit operator ArrayView<const global_coords>() const;
+    explicit operator ArrayView<const pair>() const;
     explicit operator bool() const;
 
 private:
@@ -34,7 +36,7 @@ private:
         fm_DECLARE_DELETED_COPY_ASSIGNMENT(node);
         fm_DECLARE_DEFAULT_MOVE_ASSIGNMENT_(node);
 
-        std::vector<global_coords> vec;
+        std::vector<pair> vec;
 
     private:
         std::unique_ptr<node> _next;
diff --git a/src/path-search.cpp b/src/path-search.cpp
index f7b61c4e..d1da6e99 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -3,12 +3,7 @@
 #include "object.hpp"
 #include "world.hpp"
 #include "RTree-search.hpp"
-#include "compat/math.hpp"
 #include "compat/function2.hpp"
-#include "object.hpp"
-#include <bit>
-#include <algorithm>
-#include <Corrade/Containers/Optional.h>
 #include <Corrade/Containers/PairStl.h>
 #include <Magnum/Math/Range.h>
 
@@ -131,37 +126,22 @@ constexpr auto tile_bbox(local_coords tile, Vector2b offset, Vector2ub size)
     return bb;
 };
 
-void push_heap(std::vector<search_astar::vertex_tuple>& vec, search_astar::vertex_tuple value)
-{
-    vec.push_back(value);
-    std::push_heap(vec.begin(), vec.end());
-}
-
-auto pop_heap(std::vector<search_astar::vertex_tuple>& vec)
-{
-    auto ret = vec.back();
-    std::pop_heap(vec.begin(), vec.end());
-    return ret;
-}
 
 } // namespace
 
-bool operator<(const search_astar::vertex_tuple& a, const search_astar::vertex_tuple& b) noexcept
-{
-    return b.dist <= a.dist;
-}
-
 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_; }
 
-search_astar::vertex_tuple::vertex_tuple(global_coords coord1, Vector2b offset1,
-                                         global_coords coord2, Vector2b offset2,
-                                         float dist) :
-    coord1{coord1}, coord2{coord2}, dist{dist},
-    offset1{offset1}, offset2{offset2}
+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()}
 {
-}
 
 #if 0
 size_t path_search::cache_chunk_index(chunk_coords ch)
@@ -399,83 +379,4 @@ void path_search::fill_cache(world& w, Vector2i cmin, Vector2i cmax, int8_t z,
 }
 #endif
 
-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);
-    own_size = Math::max(own_size, Vector2ub(min_size));
-
-    if (from.z() != to.z()) [[unlikely]]
-        return {};
-
-    // todo try removing this eventually
-    if (from.z() != 0) [[unlikely]]
-        return {};
-
-    if (!is_passable(w, from, from_offset, own_size, own_id, p))
-        return {};
-
-    if (!is_passable(w, to, to_offset, own_size, own_id, p))
-        return {};
-
-    _astar.Q.clear();
-    _astar.Q.reserve(TILE_COUNT*(size_t)(subdivide_factor*subdivide_factor));
-
-    const auto pos0 = Vector2(from.local()) * TILE_SIZE2;
-    const auto start_bbox = bbox<float>{pos0 - Vector2(own_size/2), pos0 + Vector2(own_size)};
-
-    constexpr auto div = Vector2ub{subdivide_factor};
-    constexpr auto sub_size = Vector2(Vector2ub(iTILE_SIZE2) / div);
-    constexpr auto sqrt_2 = (float)math::sqrt(2);
-
-    constexpr Vector2 directions[8] = {
-        sub_size * Vector2{ -1,  0 },
-        sub_size * Vector2{  0, -1 },
-        sub_size * Vector2{  1,  0 },
-        sub_size * Vector2{  0,  1 },
-        sub_size * Vector2{   sqrt_2,  sqrt_2, },
-        sub_size * Vector2{  -sqrt_2, -sqrt_2  },
-        sub_size * Vector2{   sqrt_2, -sqrt_2  },
-        sub_size * Vector2{  -sqrt_2,  sqrt_2  },
-    };
-
-    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;
-    auto& path = result._node->vec;
-    path.clear();
-
-    auto& Q = _astar.Q;
-
-    while (!Q.empty())
-    {
-        auto cur = pop_heap(Q);
-    }
-
-    auto [cmin, cmax] = Math::minmax(Vector2i(from.chunk()) - Vector2i(1, 1),
-                                     Vector2i(to.chunk()) + Vector2i(1, 1));
-
-    // todo...
-    return result;
-}
-
-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);
-}
-
 } // namespace floormat
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 574cd133..a0e77398 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -5,6 +5,7 @@
 #include "rotation.hpp"
 #include "world.hpp"
 #include "compat/function2.fwd.hpp"
+#include "path-search-astar.hpp"
 #include "path-search-result.hpp"
 #include <memory>
 #include <array>
@@ -14,6 +15,7 @@
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/DimensionTraits.h>
 
+
 namespace Corrade::Containers {
 template<typename T> class Optional;
 template<typename T, typename U> class Pair;
@@ -28,25 +30,9 @@ struct chunk;
 struct path_search_result;
 class path_search;
 
-struct search_astar final
-{
-    struct vertex_tuple
-    {
-        vertex_tuple(global_coords coord1, Vector2b offset1,
-                     global_coords coord2, Vector2b offset2,
-                     float dist = INF);
-
-        static constexpr float INF = 1 << 24;
-
-        global_coords coord1, coord2;
-        float dist = INF;
-        Vector2b offset1, offset2;
-
-        friend bool operator<(const vertex_tuple& a, const vertex_tuple& b) noexcept;
-    };
-
-    std::vector<vertex_tuple> Q;
-};
+struct astar_edge;
+struct astar_hash;
+struct astar;
 
 enum class path_search_continue : bool { pass = false, blocked = true };
 
@@ -55,7 +41,7 @@ class path_search final
     friend struct path_search_result;
 
     // todo bucketize by array length
-    search_astar _astar;
+    struct astar astar;
 
 public:
     static constexpr int subdivide_factor = 4;
@@ -73,13 +59,6 @@ public:
         operator ArrayView<const global_coords>() const;
     };
 
-    struct positions
-    {
-        size_t size = 0;
-        std::array<global_coords, 5> coords;
-        std::array<Vector2b, 5> offsets;
-    };
-
 #if 0
     struct chunk_tiles_cache
     {
diff --git a/src/world.hpp b/src/world.hpp
index 56b74898..9fa48b8b 100644
--- a/src/world.hpp
+++ b/src/world.hpp
@@ -17,7 +17,7 @@ struct world final
 {
     static constexpr object_id object_counter_init = 1024;
     static constexpr size_t initial_capacity = 4096;
-    static constexpr float max_load_factor = .5;
+    static constexpr float max_load_factor = .25;
     static constexpr size_t initial_collect_every = 64;
 
 private: