implement simplifying A* result path

6 files changed, 72 insertions, 95 deletions

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();