implement simplifying A* result path

8 files changed, 88 insertions, 119 deletions

diff --git a/editor/tests/path-test.cpp b/editor/tests/path-test.cpp
index cc7dc796..ac95bcd9 100644
--- a/editor/tests/path-test.cpp
+++ b/editor/tests/path-test.cpp
@@ -36,12 +36,8 @@ struct path_test final : base_test
     struct result_s
     {
         point from, to;
-        std::vector<point> path;
-        float time;
-        uint32_t cost, distance;
-        bool found : 1;
+        path_search_result res;
     } result;
-
     bool has_result : 1 = false, has_pending : 1 = false;
 };
 
@@ -116,7 +112,7 @@ void path_test::draw_overlay(app& a)
     auto last = a.point_screen_pos(result.from);
     draw.AddCircleFilled({last.x(), last.y()}, dot_radius, dot_color);
 
-    for (auto pt : result.path)
+    for (auto pt : result.res.path())
     {
         auto pos = a.point_screen_pos(pt);
         draw.AddLine({pos.x(), pos.y()}, {last.x(), last.y()}, line_color, line_thickness);
@@ -124,9 +120,9 @@ void path_test::draw_overlay(app& a)
         last = pos;
     }
 
-    if (!result.found && !result.path.empty())
+    if (!result.res.is_found() && !result.res.path().isEmpty())
     {
-        auto pos = a.point_screen_pos(result.path.back());
+        auto pos = a.point_screen_pos(result.res.path().back());
         constexpr float spacing = 12, size1 = 7, size2 = 3, spacing2 = spacing + size2;
 
         draw.AddLine({pos.x() - spacing2, pos.y() - spacing2},
@@ -161,11 +157,7 @@ void path_test::update_pre(app& a, const Ns&)
     result = {
         .from = pending.from,
         .to = pending.to,
-        .path = move(res.raw_path().vec),
-        .time = res.time(),
-        .cost = res.cost(),
-        .distance = res.distance(),
-        .found = res.is_found(),
+        .res = move(res),
     };
 }
 
@@ -176,14 +168,14 @@ void path_test::draw_ui(app&, float)
     constexpr auto colflags_0 = colflags_1 | ImGuiTableColumnFlags_WidthFixed;
 
     char buf[128];
-    const auto& res = result;
+    const auto& res = result.res;
 
     if (!has_result)
         return;
 
-    auto from_c = Vector3i(res.from.chunk3()), to_c = Vector3i(res.to.chunk3());
-    auto from_l = Vector2i(res.from.local()), to_l = Vector2i(res.to.local());
-    auto from_p = Vector2i(res.from.offset()), to_p = Vector2i(res.to.offset());
+    auto from_c = Vector3i(result.from.chunk3()), to_c = Vector3i(result.to.chunk3());
+    auto from_l = Vector2i(result.from.local()), to_l = Vector2i(result.to.local());
+    auto from_p = Vector2i(result.from.offset()), to_p = Vector2i(result.to.offset());
 
     constexpr auto print_coord = [](auto&& buf, Vector3i c, Vector2i l, Vector2i p)
     {
@@ -212,7 +204,7 @@ void path_test::draw_ui(app&, float)
         text(buf);
 
         do_column("found?");
-        if (res.found)
+        if (res.is_found())
         {
             auto b = push_style_color(ImGuiCol_Text, 0x00ff00ff_rgbaf);
             text("yes");
@@ -226,21 +218,21 @@ void path_test::draw_ui(app&, float)
             {
                 auto b = push_style_color(ImGuiCol_Text, 0xffff00ff_rgbaf);
                 do_column("dist");
-                std::snprintf(buf, std::size(buf), "%d", (int)res.distance);
+                std::snprintf(buf, std::size(buf), "%d", (int)res.distance());
                 text(buf);
             }
         }
 
         do_column("cost");
-        std::snprintf(buf, std::size(buf), "%d", (int)res.cost);
+        std::snprintf(buf, std::size(buf), "%d", (int)res.cost());
         text(buf);
 
         do_column("length");
-        std::snprintf(buf, std::size(buf), "%d", (int)res.path.size());
+        std::snprintf(buf, std::size(buf), "%d", (int)res.path().size());
         text(buf);
 
         do_column("time");
-        std::snprintf(buf, std::size(buf), "%.1f ms", (double)(1000 * res.time));
+        std::snprintf(buf, std::size(buf), "%.1f ms", (double)(1000 * res.time()));
         text(buf);
     }
 }
diff --git a/src/point.inl b/src/point.inl
index be5b3180..0184c37c 100644
--- a/src/point.inl
+++ b/src/point.inl
@@ -7,17 +7,13 @@ namespace floormat {
 
 constexpr uint32_t point::distance(point a, point b)
 {
-    Vector2i dist;
-    dist += (a.coord() - b.coord())*iTILE_SIZE2;
-    dist += Vector2i(a.offset()) - Vector2i(b.offset());
+    Vector2i dist = a - b;
     return (uint32_t)Math::ceil(Math::sqrt(Vector2(dist).dot()));
 }
 
 constexpr uint32_t point::distance_l2(point a, point b)
 {
-    Vector2i dist;
-    dist += (a.coord() - b.coord())*iTILE_SIZE2;
-    dist += Vector2i(a.offset()) - Vector2i(b.offset());
+    Vector2i dist = a - b;
     return (uint32_t)Math::abs(dist).sum();
 }
 
diff --git a/src/search-astar.cpp b/src/search-astar.cpp
index f11acfe9..1f1663f9 100644
--- a/src/search-astar.cpp
+++ b/src/search-astar.cpp
@@ -33,6 +33,39 @@ using Search::min_size;
 
 namespace {
 
+void simplify_path(ArrayView<const point> src, std::vector<point>& dest)
+{
+    const auto size = (uint32_t)src.size();
+    fm_assert(size >= 2);
+    dest.reserve(size);
+    auto last_vec = src[1] - src[0];
+
+    dest.clear();
+    dest.reserve(src.size());
+
+    if (last_vec.isZero()) [[unlikely]]
+    {
+        fm_assert(size <= 2);
+        return;
+    }
+
+    dest.push_back(src[1]);
+
+    for (auto i = 2u; i < size; i++)
+    {
+        const auto pos = src[i];
+        const auto vec = pos - src[i-1];
+        if (vec != last_vec || Math::abs(vec).max() != div_size.x())
+        {
+            dest.push_back(pos);
+            last_vec = vec;
+        }
+    }
+
+    if (dest.back() != src.back())
+        dest.push_back(src.back());
+}
+
 constexpr bbox<Int> bbox_from_pos1(point pt, Vector2ui size)
 {
     auto center = Vector2i(pt.local()) * iTILE_SIZE2 + Vector2i(pt.offset());
@@ -94,27 +127,33 @@ struct heap_comparator
     inline bool operator()(uint32_t a, uint32_t b) const { return nodes[b].dist < nodes[a].dist; }
 };
 
-void set_result_from_idx(path_search_result& result, const Array<visited>& nodes,
+void set_result_from_idx(path_search_result& result,
+                         Array<point>& temp_nodes, const Array<visited>& nodes,
                          point to, const uint32_t idx)
 {
-    fm_debug_assert(idx != (uint32_t)-1);
+    uint32_t len = 0;
+    for (auto i = idx; i != (uint32_t)-1; i = nodes[i].prev)
+        len++;
 
-    auto& path = result.raw_path().vec;
-    path.clear();
+    fm_debug_assert(idx != (uint32_t)-1);
+    arrayResize(temp_nodes, 0);
+    arrayReserve(temp_nodes, len+1);
 
     const auto& to_node = nodes[idx];
     if (result.is_found() && to != to_node.pt)
-        path.push_back(to);
+        arrayAppend(temp_nodes, to);
     result.set_cost(to_node.dist);
 
     auto i = idx;
     do {
         const auto& node = nodes[i];
-        path.push_back(node.pt);
+        arrayAppend(temp_nodes, node.pt);
         i = node.prev;
     } while (i != (uint32_t)-1);
 
-    std::reverse(path.begin(), path.end());
+    std::reverse(temp_nodes.begin(), temp_nodes.end());
+    simplify_path(temp_nodes, result.raw_path().vec);
+    arrayResize(temp_nodes, 0);
 }
 
 } // namespace
@@ -304,13 +343,13 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to,
     {
         result.set_found(true);
         result.set_distance(0);
-        set_result_from_idx(result, nodes, to, goal_idx);
+        set_result_from_idx(result, temp_nodes, nodes, to, goal_idx);
     }
     else if (closest_idx != (uint32_t)-1)
     {
         result.set_found(false);
         result.set_distance(closest_dist);
-        set_result_from_idx(result, nodes, to, closest_idx);
+        set_result_from_idx(result, temp_nodes, nodes, to, closest_idx);
     }
 
     result.set_time(timeline.currentFrameTime());
diff --git a/src/search-astar.hpp b/src/search-astar.hpp
index c31b348f..61553f80 100644
--- a/src/search-astar.hpp
+++ b/src/search-astar.hpp
@@ -38,6 +38,7 @@ private:
     safe_ptr<struct Search::cache> _cache;
     Array<visited> nodes;
     Array<uint32_t> Q;
+    Array<point> temp_nodes;
 };
 
 } // namespace floormat
diff --git a/src/search-node.hpp b/src/search-node.hpp
index 06bef04f..ebe33b9a 100644
--- a/src/search-node.hpp
+++ b/src/search-node.hpp
@@ -7,12 +7,6 @@
 
 namespace floormat {
 
-struct path_search_result::pair
-{
-    point pt;
-    uint32_t length = 0;
-};
-
 struct path_search_result::node
 {
     friend struct path_search_result;
@@ -23,7 +17,6 @@ struct path_search_result::node
     fm_DECLARE_DEFAULT_MOVE_ASSIGNMENT_(node);
 
     std::vector<point> vec;
-    std::vector<pair> vec_;
 
 private:
     Pointer<node> _next;
diff --git a/src/search-result.cpp b/src/search-result.cpp
index 2c5b5e53..c2065735 100644
--- a/src/search-result.cpp
+++ b/src/search-result.cpp
@@ -9,56 +9,6 @@
 
 namespace floormat {
 
-namespace {
-
-struct off_pair
-{
-    unsigned val;
-    bool is_bad;
-    constexpr bool operator==(const off_pair&) const = default;
-};
-
-template<unsigned N> constexpr off_pair offset_is_bad(Int x)
-{
-    switch (x)
-    {
-    case Int{-1}:
-    case Int{ 1}:
-    case Int{ 0}:
-        return { (unsigned(Int{1} + x)) << N, false };
-    default:
-        return { (unsigned)-1, true };
-    }
-}
-#if 0
-static_assert((2 | 2 << 8) == (offset_is_bad<0>(Int{1}).val | offset_is_bad<8>(Int{1}).val));
-static_assert((2 | 1 << 8) == (offset_is_bad<0>(Int{1}).val | offset_is_bad<8>(Int{0}).val));
-static_assert((1 | 1 << 8) == (offset_is_bad<0>(Int{0}).val | offset_is_bad<8>(Int{0}).val));
-static_assert((0 | 2 << 8) == (offset_is_bad<0>(Int{-1}).val | offset_is_bad<8>(Int{1}).val));
-static_assert((unsigned)-1 == (offset_is_bad<0>(Int{4242}).val | offset_is_bad<8>(Int{1}).val));
-#endif
-
-void simplify_path(const std::vector<point>& src, std::vector<path_search_result::pair>& dest)
-{
-    dest.clear();
-    dest.reserve(src.size());
-    fm_assert(!src.empty());
-    fm_assert(src.size() >= 2);
-    const auto size = (uint32_t)src.size();
-    dest.push_back({src[0], 0});
-
-    auto last = src[0];
-    auto cur = src[1] - src[0];
-    size_t len = 1;
-
-    for (auto i = 1u; i < size; i++)
-    {
-
-    }
-}
-
-} // namespace
-
 Pointer<path_search_result::node> path_search_result::_pool; // NOLINT
 
 path_search_result::path_search_result()
@@ -108,8 +58,26 @@ path_search_result& path_search_result::operator=(const path_search_result& x) n
     return *this;
 }
 
-path_search_result::path_search_result(path_search_result&&) noexcept = default;
-path_search_result& path_search_result::operator=(path_search_result&&) noexcept = default;
+path_search_result::path_search_result(path_search_result&& other) noexcept
+{
+    *this = move(other);
+}
+
+path_search_result& path_search_result::operator=(path_search_result&& other) noexcept
+{
+    if (_node)
+    {
+        _node->vec.clear();
+        _node->_next = move(_pool);
+        _pool = move(_node);
+    }
+    _node = move(other._node);
+    _time = other._time;
+    _cost = other._cost;
+    _distance = other._distance;
+    _found = other._found;
+    return *this;
+}
 
 size_t path_search_result::size() const { return _node->vec.size(); }
 path_search_result::node::node() noexcept = default;
@@ -122,24 +90,8 @@ bool path_search_result::is_found() const { return _found; }
 void path_search_result::set_found(bool value) { _found = value; }
 uint32_t path_search_result::distance() const { return _distance; }
 void path_search_result::set_distance(uint32_t dist) { _distance = dist; }
-bool path_search_result::is_simplified() const { /*fm_assert(!_node->vec.empty());*/ return !_node->vec_.empty(); }
-auto path_search_result::simplified_path() -> ArrayView<const pair>
-{
-    if (!_node->vec_.empty())
-        return { _node->vec_.data(), _node->vec_.size() };
-    else
-    {
-        //fm_assert(!_node->vec.empty());
-        simplify_path(_node->vec, _node->vec_);
-        fm_assert(!_node->vec_.empty());
-        return { _node->vec_.data(), _node->vec_.size() };
-    }
-}
-
 path_search_result::operator bool() const { return !_node->vec.empty(); }
-
-vector_wrapper<point, vector_wrapper_repr::ref> path_search_result::raw_path() { fm_assert(_node); return {_node->vec}; }
-auto path_search_result::raw_simplified_path() -> vector_wrapper<pair, vector_wrapper_repr::ref> { fm_assert(_node); return {_node->vec_}; }
 ArrayView<const point> path_search_result::path() const { fm_assert(_node); return {_node->vec.data(), _node->vec.size()}; }
+vector_wrapper<point, vector_wrapper_repr::ref> path_search_result::raw_path() { fm_assert(_node); return {_node->vec}; }
 
 } // namespace floormat
diff --git a/src/search-result.hpp b/src/search-result.hpp
index d22a6cf5..97d40242 100644
--- a/src/search-result.hpp
+++ b/src/search-result.hpp
@@ -10,7 +10,6 @@ struct point;
 struct path_search_result final
 {
     friend struct test_app;
-    struct pair;
     struct node;
 
     size_t size() const;
@@ -22,12 +21,9 @@ struct path_search_result final
     void set_found(bool value);
     uint32_t distance() const;
     void set_distance(uint32_t dist);
-    bool is_simplified() const;
 
     vector_wrapper<point, vector_wrapper_repr::ref> raw_path();
-    vector_wrapper<pair, vector_wrapper_repr::ref> raw_simplified_path();
     ArrayView<const point> path() const;
-    ArrayView<const pair> simplified_path();
     explicit operator bool() const;
 
     path_search_result();
diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp
index f8764292..4d9f2187 100644
--- a/test/dijkstra.cpp
+++ b/test/dijkstra.cpp
@@ -64,7 +64,7 @@ void test_app::test_dijkstra()
 
         fm_assert(!result.is_found());
         fm_assert(!result.path().isEmpty());
-        fm_assert(result.size() > 4);
+        fm_assert(result.size() > 2);
         fm_assert(result.cost() > 1000);
         fm_assert(result.cost() < 3000);
         fm_assert(result.distance() > min);
@@ -79,7 +79,7 @@ void test_app::test_dijkstra()
 
         fm_assert(result.is_found());
         fm_assert(!result.path().isEmpty());
-        fm_assert(result.size() > 4);
+        fm_assert(result.size() > 2);
         fm_assert(result.cost() > 1000);
         fm_assert(result.cost() < 3000);
         fm_assert(result.distance() == 0);