a

2 files changed, 89 insertions, 36 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 11aa9cfc..beb87ddd 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -47,7 +47,7 @@ constexpr auto get_bbox(chunk_coords_ ch_1, local_coords pos1, Vector2b off1,
     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 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;
@@ -57,9 +57,9 @@ constexpr auto get_bbox(chunk_coords_ ch_1, local_coords pos1, Vector2b off1,
           .max = Vector2(Math::max(max0, max1)) },
         dist0 + dist,
     };
-};
+}
 
-constexpr auto dirs = [] constexpr
+constexpr auto directions = [] constexpr
 {
     struct pair { Vector2i dir; uint32_t len; };
     constexpr auto len1 = div_size;
@@ -97,19 +97,35 @@ constexpr bbox<T> bbox_from_pos(Math::Vector<2, T> pos, Vector2b offset, Vector2
 
 } // namespace
 
-size_t astar::hash_op::operator()(position coord) const
+bool astar::edge::operator==(const floormat::astar::edge& other) const = default;
+
+size_t astar::point_hash::operator()(point pt) 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);
+#ifdef FLOORMAT_64
+    static_assert(sizeof nullptr > 4);
+    return fnvhash_64(&pt, sizeof pt);
+#else
+    static_assert(sizeof nullptr == 4);
+    return fnvhash_32(&pt, sizeof pt);
+#endif
+}
+
+size_t astar::edge_hash::operator()(const edge& e) const
+{
+    constexpr auto len = sizeof e;
+    static_assert(len == 8 + 8 + 2 + 2);
+#ifdef FLOORMAT_64
+    static_assert(sizeof nullptr > 4);
+    return fnvhash_64(&e, len);
+#else
+    static_assert(sizeof nullptr == 4);
+    return fnvhash_32(&e, len);
+#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)
+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());
@@ -118,6 +134,9 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id
         return n2.dist < n1.dist;
     };
 
+    const auto [from, from_offset] = from_;
+    const auto [to, to_offset] = to_;
+
     own_size = Math::max(own_size, Vector2ub(min_size));
 
     if (from.z() != to.z()) [[unlikely]]
@@ -137,13 +156,13 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id
     fm_debug_assert(astar.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)Math::ceil(Vector2(from_offset).length()));
+    const auto from_offset_len = Math::max(1u, (uint32_t)(Vector2(from_offset).length() + 0.5f));
 
     path_search_result result;
     fm_debug_assert(result._node); // todo
     auto& path = result._node->vec; path.clear();
 
-    astar.indexes[{from, from_offset}] = 0;
+    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);
@@ -160,18 +179,51 @@ path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id
         }
     }
 
+    while (!astar.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);
+
+        node.expanded = true;
 
+        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))
+                continue;
+
+            auto [new_coord, new_offset] = object::normalize_coords(node.coord, node.offset, vec);
+            auto [it, found] = astar.indexes.try_emplace({.coord = new_coord, .offset = new_offset}, (uint32_t)-1);
+            if (!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);
+            }
+            else if (new_dist >= node.dist)
+                continue;
+            else
+            {
+
+            }
+        }
+    }
 
     // todo...
     return result;
 }
 
-path_search_result path_search::Dijkstra(world& w, const object& obj,
-                                         global_coords to, Vector2b to_offset,
-                                         const pred& p)
-{
-    fm_assert(obj.bbox_offset.isZero());
-    return Dijkstra(w, obj.bbox_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 5a8a0f9e..adcda8a0 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -10,7 +10,8 @@
 #include <array>
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/DimensionTraits.h>
-#include <tsl/robin_hash.h>
+#include <tsl/robin_map.h>
+#include <tsl/robin_set.h>
 
 namespace Corrade::Containers {
 //template<typename T> class Optional;
@@ -23,7 +24,6 @@ namespace floormat {
 struct world;
 struct object;
 struct chunk;
-
 struct path_search_result;
 
 enum class path_search_continue : bool { pass = false, blocked = true };
@@ -32,13 +32,6 @@ 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;
@@ -46,11 +39,19 @@ struct astar
         global_coords coord;
         Vector2b offset;
         bool expanded = false;
+
     };
-    struct hash_op
+    struct edge
     {
-        size_t operator()(position coord) const;
+        global_coords from, to;
+        Vector2b from_offset, to_offset;
+
+        bool operator==(const edge& other) const;
     };
+
+    struct point_hash { size_t operator()(point pt) const; };
+    struct edge_hash { size_t operator()(const edge& e) const; };
+
     void reserve(size_t capacity)
     {
         nodes.reserve(capacity);
@@ -71,7 +72,8 @@ struct astar
     }
 
     std::vector<visited> nodes;
-    tsl::robin_map<position, uint32_t, hash_op> indexes;
+    tsl::robin_set<edge, edge_hash> edges;
+    tsl::robin_map<point, uint32_t, point_hash> indexes;
     std::vector<uint32_t> Q;
 };
 
@@ -105,8 +107,7 @@ public:
     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, global_coords from, Vector2b from_offset, global_coords to, Vector2b to_offset, const pred& p = never_continue());
-    path_search_result Dijkstra(world& w, const object& obj, global_coords to, Vector2b to_offset, const pred& p = never_continue());
+    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,