a

8 files changed, 149 insertions, 110 deletions

diff --git a/src/global-coords.cpp b/src/global-coords.cpp
index 694447c1..f8d5daed 100644
--- a/src/global-coords.cpp
+++ b/src/global-coords.cpp
@@ -1,5 +1,5 @@
 #include "global-coords.hpp"
-#include "compat/defs.hpp"
+#include "point.hpp"
 #include <array>
 #include <algorithm>
 
diff --git a/src/global-coords.hpp b/src/global-coords.hpp
index 6ae3a37c..017d7c90 100644
--- a/src/global-coords.hpp
+++ b/src/global-coords.hpp
@@ -1,7 +1,6 @@
 #pragma once
 #include "local-coords.hpp"
 #include "compat/assert.hpp"
-#include <compare>
 #include <concepts>
 #include <Magnum/Magnum.h>
 #include <Magnum/Math/Vector2.h>
@@ -116,15 +115,6 @@ public:
     constexpr Vector2i operator-(global_coords other) const noexcept;
 };
 
-struct point
-{
-    global_coords coord;
-    Vector2b offset;
-
-    constexpr bool operator==(const point&) const = default;
-    friend constexpr std::strong_ordering operator<=>(const point& a, const point& b) noexcept;
-};
-
 constexpr local_coords global_coords::local() const noexcept
 {
     return { uint8_t(x & 0x0f), uint8_t(y & 0x0f), };
@@ -187,22 +177,4 @@ constexpr Vector2i global_coords::operator-(global_coords other) const noexcept
     return to_signed() - other.to_signed();
 }
 
-constexpr std::strong_ordering operator<=>(const point& p1, const point& p2) noexcept
-{
-    auto c1 = p1.coord.to_signed3(), c2 = p2.coord.to_signed3();
-
-    if (auto val = c1.z() <=> c2.z(); val != std::strong_ordering::equal)
-        return val;
-    if (auto val = c1.y() <=> c2.y(); val != std::strong_ordering::equal)
-        return val;
-    if (auto val = c1.x() <=> c2.x(); val != std::strong_ordering::equal)
-        return val;
-    if (auto val = p1.offset.y() <=> p2.offset.y(); val != std::strong_ordering::equal)
-        return val;
-    if (auto val = p1.offset.x() <=> p2.offset.x(); val != std::strong_ordering::equal)
-        return val;
-
-    return std::strong_ordering::equal;
-}
-
 } // namespace floormat
diff --git a/src/object.cpp b/src/object.cpp
index bed5ef88..ee21c14f 100644
--- a/src/object.cpp
+++ b/src/object.cpp
@@ -111,6 +111,7 @@ void object::rotate(size_t, rotation new_r)
     const_cast<rotation&>(r) = new_r;
 }
 
+// todo rewrite using bitwise ops
 point object::normalize_coords(global_coords coord, Vector2b cur_offset, Vector2i new_offset)
 {
     auto off_tmp = Vector2i(cur_offset) + new_offset;
@@ -119,7 +120,7 @@ point object::normalize_coords(global_coords coord, Vector2b cur_offset, Vector2
     for (auto i = 0uz; i < 2; i++)
     {
         auto sign = Math::sign(off_new[i]);
-        auto absval = std::abs(off_new[i]);
+        auto absval = Math::abs(off_new[i]);
         if (absval > half_tile[i])
         {
             Vector2i v(0);
diff --git a/src/object.hpp b/src/object.hpp
index 41ea1f97..3832572b 100644
--- a/src/object.hpp
+++ b/src/object.hpp
@@ -5,6 +5,7 @@
 #include "src/pass-mode.hpp"
 #include "src/object-type.hpp"
 #include "src/object-id.hpp"
+#include "src/point.hpp"
 #include <memory>
 #include <vector>
 
diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 5e7b7e79..79765473 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -1,7 +1,6 @@
 #include "path-search.hpp"
 #include "object.hpp"
-#include "compat/math.hpp"
-#include <Corrade/Containers/PairStl.h>
+#include "point.hpp"
 #include <Magnum/Math/Vector2.h>
 #include <Magnum/Math/Functions.h>
 
@@ -73,15 +72,92 @@ constexpr bbox<T> bbox_from_pos(Math::Vector<2, T> pos, Vector2b offset, Vector2
     return bb;
 }
 
-bool add_start_node(std::vector<astar::visited>& nodes,
-                    tsl::robin_map<point, uint32_t, astar::point_hash>& indexes,
-                    std::vector<uint32_t>& Q)
+class heap_comparator
 {
+    const std::vector<astar::visited>& nodes; // NOLINT
 
+public:
+    heap_comparator(const std::vector<astar::visited>& nodes) : nodes{nodes} {}
+
+    inline bool operator()(uint32_t a, uint32_t b) const
+    {
+        fm_debug_assert(std::max(a, b) < nodes.size());
+        const auto& n1 = nodes[a];
+        const auto& n2 = nodes[b];
+        return n2.dist < n1.dist;
+    }
+};
+
+class box
+{
+    using visited = astar::visited;
+    std::vector<visited>& vec; // NOLINT
+    uint32_t id;
+
+public:
+    fm_DECLARE_DELETED_COPY_ASSIGNMENT(box);
+    fm_DECLARE_DELETED_MOVE_ASSIGNMENT(box);
+
+    visited& operator*() { return vec[id]; }
+    visited* operator->() { return &vec[id]; }
+
+    box(std::vector<visited>& vec, uint32_t id) : vec{vec}, id{id} {}
+};
+
+uint32_t distance(point a, point b)
+{
+    Vector2i dist;
+    dist += Math::abs(a.coord - b.coord)*iTILE_SIZE2;
+    dist += Vector2i(a.offset - b.offset);
+    return (uint32_t)Math::sqrt(dist.dot());
 }
 
 } // namespace
 
+astar::astar()
+{
+    indexes.max_load_factor(.4f);
+    reserve(initial_capacity);
+}
+
+void astar::reserve(size_t capacity)
+{
+    nodes.reserve(capacity);
+    indexes.reserve(capacity);
+    edges.reserve(capacity*4);
+    Q.reserve(capacity);
+}
+
+void astar::clear()
+{
+    nodes.clear();
+    indexes.clear();
+    edges.clear();
+    Q.clear();
+}
+
+void astar::add_to_heap(uint32_t id)
+{
+    Q.push_back(id);
+    std::push_heap(Q.begin(), Q.end(), heap_comparator(nodes));
+}
+
+uint32_t astar::pop_from_heap()
+{
+    std::pop_heap(Q.begin(), Q.end(), heap_comparator(nodes));
+    const auto id = Q.back();
+    Q.pop_back();
+    return id;
+}
+
+auto astar::make_edge(const point& a, const point& b) -> edge
+{
+    if (a < b)
+        return { a.coord, b.coord, a.offset, b.offset };
+    else
+        return { b.coord, a.coord, b.offset, a.offset };
+}
+
 bool astar::edge::operator==(const floormat::astar::edge& other) const = default;
 
 size_t astar::point_hash::operator()(point pt) const
@@ -111,7 +187,6 @@ size_t astar::edge_hash::operator()(const edge& e) const
 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)
 {
-    const auto heap_comparator = make_heap_comparator();
     const auto [from, from_offset] = from_;
     const auto [to, to_offset] = to_;
 
@@ -139,8 +214,7 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
 
     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);
+    add_to_heap(0);
 
     if (!from_offset.isZero())
     {
@@ -152,20 +226,20 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
         {
             indexes[{from, {}}] = idx;
             nodes.push_back({.dist = from_offset_len, .prev = 0, .coord = from, .offset = {}});
-            Q.push_back(idx++);
-            std::push_heap(Q.begin(), Q.end(), heap_comparator);
+            add_to_heap(idx++);
         }
-
     }
 
+    auto closest = (uint32_t)-1;
+
     while (!Q.empty())
     {
-        std::pop_heap(Q.begin(), Q.end(), heap_comparator);
-        const auto id = Q.back();
-        Q.pop_back();
-
+        const auto id = pop_from_heap();
         auto node = box{nodes, id};
 
+        if (auto d = distance({node->coord, node->offset}, {to, to_offset}); d < closest)
+            closest = d;
+
         //Debug{} << "node" << id << "|" << node.coord.to_signed3() << node.offset << "|" << node.dist;
 
         const auto bb0 = bbox_from_pos(Vector2(node->coord.local()), node->offset, own_size);
@@ -215,14 +289,13 @@ path_search_result astar::Dijkstra(world& w, Vector2ub own_size, const object_id
 
             //Debug{} << (fresh ? "   new" : "  old") << new_idx << "|" << node.coord.to_signed3() << node.offset << "|" << node.dist;
 
-            Q.push_back(new_idx);
-            std::push_heap(Q.begin(), Q.end(), heap_comparator);
-
+            add_to_heap(new_idx);
         }
     }
 
     //DBG << "done!" << nodes.size() << "nodes," << indexes.size() << "indexes," << edges.size() << "edges.";
 
+    Debug{} << "closest" << closest;
     // todo...
     return result;
 }
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 1ad1120d..6d8da1da 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -6,6 +6,7 @@
 #include "compat/function2.fwd.hpp"
 #include "path-search-result.hpp"
 #include "compat/int-hash.hpp"
+#include "point.hpp"
 #include <bit>
 #include <array>
 #include <Magnum/Math/Vector2.h>
@@ -62,8 +63,8 @@ struct astar
         uint32_t prev = (uint32_t)-1;
         global_coords coord;
         Vector2b offset;
-
     };
+
     struct edge
     {
         global_coords from, to;
@@ -72,71 +73,20 @@ struct astar
         bool operator==(const edge& other) const;
     };
 
-    class box
-    {
-        std::vector<visited>& vec; // NOLINT
-        uint32_t id;
-
-    public:
-        fm_DECLARE_DELETED_COPY_ASSIGNMENT(box);
-        fm_DECLARE_DELETED_MOVE_ASSIGNMENT(box);
-
-        visited& operator*() { return vec[id]; }
-        visited* operator->() { return &vec[id]; }
-
-        box(std::vector<visited>& vec, uint32_t id) : vec{vec}, id{id} {}
-    };
-
-    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; };
-
     using pred = path_search::pred;
+    enum class edge_status : uint8_t { unknown, good, bad, };
     template<typename T> using bbox = path_search::bbox<T>;
+    struct point_hash { size_t operator()(point pt) const; };
+    struct edge_hash { size_t operator()(const edge& e) const; };
 
     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*4);
-        Q.reserve(capacity);
-    }
-    astar()
-    {
-        indexes.max_load_factor(.4f);
-        reserve(initial_capacity);
-    }
-    void clear()
-    {
-        nodes.clear();
-        indexes.clear();
-        edges.clear();
-        Q.clear();
-    }
-    auto make_heap_comparator()
-    {
-        return [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;
-        };
-    }
+    astar();
+    void reserve(size_t capacity);
+    void clear();
+    void add_to_heap(uint32_t id);
+    uint32_t pop_from_heap();
+    static edge make_edge(const point& a, const point& b);
 
     // todo add simple bresenham short-circuit
     path_search_result Dijkstra(world& w, Vector2ub own_size, object_id own_id,
diff --git a/src/point.hpp b/src/point.hpp
new file mode 100644
index 00000000..99c4acdf
--- /dev/null
+++ b/src/point.hpp
@@ -0,0 +1,42 @@
+#pragma once
+#include "global-coords.hpp"
+#include <compare>
+
+namespace floormat {
+
+// todo pack this within 8 bytes
+struct point
+{
+    global_coords coord;
+    Vector2b offset;
+
+    constexpr bool operator==(const point&) const = default;
+    friend constexpr std::strong_ordering operator<=>(const point& a, const point& b) noexcept;
+};
+
+constexpr std::strong_ordering operator<=>(const point& p1, const point& p2) noexcept
+{
+    auto c1 = p1.coord.to_signed3(), c2 = p2.coord.to_signed3();
+
+    if (auto val = c1.z() <=> c2.z(); val != std::strong_ordering::equal)
+        return val;
+    if (auto val = c1.y() <=> c2.y(); val != std::strong_ordering::equal)
+        return val;
+    if (auto val = c1.x() <=> c2.x(); val != std::strong_ordering::equal)
+        return val;
+    if (auto val = p1.offset.y() <=> p2.offset.y(); val != std::strong_ordering::equal)
+        return val;
+    if (auto val = p1.offset.x() <=> p2.offset.x(); val != std::strong_ordering::equal)
+        return val;
+
+    return std::strong_ordering::equal;
+}
+
+struct packed_point
+{
+    uint64_t cx : 12, cy : 12, cz : 4,
+             tx : 8, ty : 8,
+             ox : 4, oy : 4;
+};
+
+} // namespace floormat
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
index afd2a85a..9b0ee1b6 100644
--- a/test/dijkstra.cpp
+++ b/test/dijkstra.cpp
@@ -30,7 +30,7 @@ void test_app::test_dijkstra()
     auto a = astar();
 
     bench_run("Dijkstra", [&] {
-      a.Dijkstra(w, {}, 0, {{0, 0, 0}, {}}, {{1, 1, 0}, {}},
+      a.Dijkstra(w, {}, 0, {{0, 0, 0}, {0, 0}}, {{1, 1, 0}, {7, 9}},
                  1*TILE_MAX_DIM*iTILE_SIZE2.x());
     });
 }