diff options
| -rw-r--r-- | src/dijkstra.cpp | 102 | ||||
| -rw-r--r-- | src/path-search-result.cpp | 15 | ||||
| -rw-r--r-- | src/path-search-result.hpp | 9 | ||||
| -rw-r--r-- | test/app.hpp | 1 | ||||
| -rw-r--r-- | test/dijkstra.cpp | 66 | ||||
| -rw-r--r-- | test/main.cpp | 2 |
6 files changed, 150 insertions, 45 deletions
diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp index e05e2a83..7a2a40a0 100644 --- a/src/dijkstra.cpp +++ b/src/dijkstra.cpp @@ -98,6 +98,29 @@ inline uint32_t distance_l2(point a, point b) return (uint32_t)Math::abs(dist).sum(); } +void set_result_from_idx(path_search_result& result, const std::vector<visited>& nodes, + point to, const uint32_t idx) +{ + fm_debug_assert(idx != (uint32_t)-1); + + auto& path = result.path(); + path.clear(); + + const auto& to_node = nodes[idx]; + if (to != to_node.pt) + path.push_back(to); + result.set_cost(to_node.dist); + + auto i = idx; + do { + const auto& node = nodes[i]; + path.push_back(node.pt); + i = node.prev; + } while (i != (uint32_t)-1); + + std::reverse(path.begin(), path.end()); +} + } // namespace astar::astar() @@ -182,17 +205,16 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o } } - auto closest_dist = distance(from, to); - auto closest_pos = from; - uint32_t closest_path_len = 0; + auto closest_dist = (uint32_t)-1; + uint32_t closest_idx = (uint32_t)-1; auto goal_idx = (uint32_t)-1; while (!Q.empty()) { - const auto id = pop_from_heap(); + const auto cur_idx = pop_from_heap(); point cur_pt; uint32_t cur_dist; - { auto& n = nodes[id]; + { auto& n = nodes[cur_idx]; cur_pt = n.pt; cur_dist = n.dist; } @@ -203,16 +225,15 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o if (auto d = distance(cur_pt, to); d < closest_dist) [[unlikely]] { closest_dist = d; - closest_pos = cur_pt; - closest_path_len = cur_dist; - } + closest_idx = cur_idx; #ifndef FM_NO_DEBUG - if (debug >= 2) [[unlikely]] - DBG_nospace << "node" - << " px:" << closest_dist << " path:" << closest_path_len - << " pos:" << closest_pos; + if (debug >= 2) [[unlikely]] + DBG_nospace << "closest node" + << " px:" << closest_dist << " path:" << cur_dist + << " pos:" << cur_pt; #endif + } if (auto dist_to_goal = distance_l2(cur_pt, to); dist_to_goal < goal_thres) [[unlikely]] { @@ -220,7 +241,7 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o if (auto bb = bbox<float>(bbox_from_pos2(to, cur_pt, own_size)); path_search::is_passable(w, to.chunk3(), bb, own_id, p)) { - goal_idx = id; + goal_idx = cur_idx; max_dist = dist; continue; // path can only get longer } @@ -253,7 +274,7 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o new_idx = (uint32_t)sz; cache.add_index(chunk_idx, tile_idx, new_idx); auto new_node = visited { - .dist = dist, .prev = id, + .dist = dist, .prev = cur_idx, .pt = new_pt, }; nodes.push_back(new_node); @@ -262,7 +283,7 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o { auto& n = nodes[new_idx]; n.dist = dist; - n.prev = id; + n.prev = cur_idx; } #ifndef FM_NO_DEBUG @@ -279,53 +300,54 @@ path_search_result astar::Dijkstra(world& w, const point from, const point to, o //fm_debug_assert(nodes.size() == indexes.size()); path_search_result result; - auto& path = result.path(); - path.clear(); - if (auto i = goal_idx; i != (uint32_t)-1) + if (goal_idx != (uint32_t)-1) { - if (to != nodes[goal_idx].pt) - path.push_back(to); - - do - { - const auto& node = nodes[i]; - path.push_back(node.pt); - i = node.prev; - } - while (i !=(uint32_t)-1); - - std::reverse(path.begin(), path.end()); - result.set_cost(nodes[goal_idx].dist); + result.set_found(true); + result.set_distance(0); + set_result_from_idx(result, 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); } result.set_time(timeline.currentFrameTime()); #ifndef FM_NO_DEBUG - if (debug >= 1) + if (debug >= 1) [[unlikely]] { auto d0_ = Vector2i(Math::abs(from.coord() - to.coord())) * iTILE_SIZE2 + Vector2i(Math::abs(Vector2i(from.offset()) - Vector2i(to.offset()))); auto d0 = (uint32_t)d0_.length(); char buf[128]; - size_t len; + size_t len = 0; const auto time = (uint32_t)Math::ceil(result.time() * 1e3f); if (goal_idx != (uint32_t)-1) { auto d = nodes[goal_idx].dist; - len = snformat(buf, "Dijkstra: found in {} ms len:{} len0:{} ratio:{:.4}\n"_cf, + len = snformat(buf, "Dijkstra: found in {} ms " + "len:{} len0:{} ratio:{:.4}\n"_cf, time, d, d0, d > 0 && d0 > 0 ? (float)d/(float)d0 : 1); } - else + else if (closest_idx != (uint32_t)-1) + { + const auto& closest = nodes[closest_idx]; + fm_assert(closest.dist != 0 && closest.dist != (uint32_t)-1); + len = snformat(buf, "Dijkstra: no path found in {} ms " + "closest:{} len:{} len0:{} ratio:{:.4}\n"_cf, + time, closest_dist, closest.dist, d0, + d0 > 0 ? (float)closest.dist/(float)d0 : 1); + } + if (len) { - len = snformat(buf, "Dijkstra: no path found in {} ms closest:{} len:{} len0:{} ratio:{:.4}\n"_cf, - time, closest_dist, closest_path_len, d0, - closest_path_len > 0 && d0 > 0 ? (float)closest_path_len/(float)d0 : 1); + std::fwrite(buf, len, 1, stdout); + std::fflush(stdout); } - std::fwrite(buf, len, 1, stdout); - std::fflush(stdout); } #endif diff --git a/src/path-search-result.cpp b/src/path-search-result.cpp index ea2cc3db..fe420475 100644 --- a/src/path-search-result.cpp +++ b/src/path-search-result.cpp @@ -7,6 +7,12 @@ namespace floormat { +namespace { + +constexpr size_t min_length = TILE_MAX_DIM*2; + +} // namespace + std::unique_ptr<path_search_result::node> path_search_result::_pool; // NOLINT path_search_result::path_search_result() @@ -55,12 +61,17 @@ path_search_result& path_search_result::operator=(const path_search_result& x) n return *this; } -path_search_result::node::node() noexcept = default; size_t path_search_result::size() const { return _node->vec.size(); } +path_search_result::node::node() noexcept = default; +float path_search_result::time() const { return _time; } + uint32_t path_search_result::cost() const { return _cost; } void path_search_result::set_cost(uint32_t value) { _cost = value; } -float path_search_result::time() const { return _time; } void path_search_result::set_time(float time) { _time = time; } +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; } auto path_search_result::data() const -> const point* { return _node->vec.data(); } path_search_result::operator bool() const { return !_node->vec.empty(); } diff --git a/src/path-search-result.hpp b/src/path-search-result.hpp index 6ae9b5be..89e240b8 100644 --- a/src/path-search-result.hpp +++ b/src/path-search-result.hpp @@ -19,6 +19,10 @@ struct path_search_result final void set_cost(uint32_t value); float time() const; void set_time(float time); + bool is_found() const; + void set_found(bool value); + uint32_t distance() const; + void set_distance(uint32_t dist); std::vector<point>& path(); const std::vector<point>& path() const; @@ -32,8 +36,6 @@ struct path_search_result final ~path_search_result() noexcept; private: - static constexpr size_t min_length = TILE_MAX_DIM*2; - struct node { friend struct path_search_result; @@ -53,7 +55,8 @@ private: std::unique_ptr<node> _node; float _time = 0; - uint32_t _cost = 0; + uint32_t _cost = 0, _distance = (uint32_t)-1; + bool _found : 1 = false; }; } // namespace floormat diff --git a/test/app.hpp b/test/app.hpp index e75c6d32..b3ea87fe 100644 --- a/test/app.hpp +++ b/test/app.hpp @@ -32,6 +32,7 @@ struct test_app final : private FM_APPLICATION static void test_loader(); static void test_bitmask(); static void test_path_search(); + static void test_dijkstra(); static void test_hash(); static void test_path_search_node_pool(); static void test_wall_atlas(); diff --git a/test/dijkstra.cpp b/test/dijkstra.cpp index e69de29b..28f59d4f 100644 --- a/test/dijkstra.cpp +++ b/test/dijkstra.cpp @@ -0,0 +1,66 @@ +#include "app.hpp" +#include "src/path-search.hpp" +#include "loader/loader.hpp" +#include <Magnum/Math/Functions.h> + +namespace floormat { + +void test_app::test_dijkstra() +{ + constexpr bool debug = true; + + auto A = astar{}; + auto w = world(); + + constexpr auto wcx = 1, wcy = 1, wtx = 8, wty = 8, wox = 0, woy = 0; + constexpr auto max_dist = (uint32_t)(Vector2i(Math::abs(wcx)+1, Math::abs(wcy)+1)*TILE_MAX_DIM*iTILE_SIZE2).length(); + constexpr auto wch = chunk_coords_{wcx, wcy, 0}; + constexpr auto wt = local_coords{wtx, wty}; + constexpr auto wpos = global_coords{wch, wt}; + + auto& ch = w[wch]; + auto metal2 = tile_image_proto{loader.tile_atlas("metal2", {2, 2}, pass_mode::blocked), 0}; + + for (int16_t j = wcy - 1; j <= wcy + 1; j++) + for (int16_t i = wcx - 1; i <= wcx + 1; i++) + { + auto &c = w[chunk_coords_{i, j, 0}]; + for (int k : { 3, 4, 5, 6, 11, 12, 13, 14, 15, }) + { + c[{ k, k }].wall_north() = metal2; + c[{ k, k }].wall_west() = metal2; + } + } + + ch[{ wtx, wty }].wall_west() = metal2; + ch[{ wtx, wty }].wall_north() = metal2; + ch[{ wtx+1, wty }].wall_west() = metal2; + ch[{ wtx, wty +1}].wall_north() = metal2; + + for (int16_t j = wcy - 1; j <= wcy + 1; j++) + for (int16_t i = wcx - 1; i <= wcx + 1; i++) + { + auto& c = w[chunk_coords_{i, j, 0}]; + c.mark_passability_modified(); + c.ensure_passability(); + } + + auto result = A.Dijkstra(w, + {{0,0,0}, {11,9}}, // from + {wpos, {wox, woy}}, // to + 0, max_dist, {16,16}, // size + debug ? 1 : 0); + + constexpr auto min = (uint32_t)(TILE_SIZE2*.5f).length() - uint32_t{1}, + max = (uint32_t)(TILE_SIZE2*2.f).length() + uint32_t{1}; + + fm_assert(!result.is_found()); + fm_assert(!result.path().empty()); + fm_assert(result.size() > 4); + fm_assert(result.cost() > 1000); + fm_assert(result.cost() < 3000); + fm_assert(result.distance() > min); + fm_assert(result.distance() < max); +} + +} // namespace floormat diff --git a/test/main.cpp b/test/main.cpp index 165ac932..569d5130 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -36,6 +36,8 @@ int test_app::exec() test_path_search_node_pool(); test_path_search(); + test_dijkstra(); + zzz_test_misc(); return 0; |