From 7a7a7a79d03902ddab6484ac3566ef3b9f7e8fec Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 22 Oct 2023 20:04:30 +0200 Subject: fix 2 pathfinding bugs 1. bboxes crossing the chunk boundary 2. `max = min + size`, incorrectly was `max = center + size` --- src/dijkstra.cpp | 106 +++++++++++++++++++++++++--------------------------- src/path-search.hpp | 16 +++++++- 2 files changed, 66 insertions(+), 56 deletions(-) diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp index 5b50a8f0..7784da35 100644 --- a/src/dijkstra.cpp +++ b/src/dijkstra.cpp @@ -15,24 +15,35 @@ namespace { constexpr auto div_size = path_search::div_size; -template -requires std::is_arithmetic_v -constexpr auto bbox_from_pos(Math::Vector2 pos, Vector2b offset, Vector2ui size) +constexpr bbox bbox_from_pos1(point pt, Vector2ui size) { - const auto vec = Vector2i(pos) * iTILE_SIZE2 + Vector2i(offset); - const auto min = vec - Vector2i(size / 2); - const auto max = vec + Vector2i(size); - const auto bb = bbox{Vector2(min), Vector2(max)}; - return bb; + auto center = Vector2i(pt.local()) * iTILE_SIZE2 + Vector2i(pt.offset()); + auto top_left = center - Vector2i(size / 2); + auto bottom_right = top_left + Vector2i(size); + return { top_left, bottom_right }; } -template -requires std::is_arithmetic_v -constexpr inline bbox bbox_union(bbox bb0, bbox bb1) +constexpr bbox bbox_from_pos2(point pt, point from, Vector2ui size) { - return { Math::min(bb0.min, bb1.min), Math::max(bb0.max, bb1.max) }; + constexpr auto chunk_size = iTILE_SIZE2 * (int)TILE_MAX_DIM; + auto nchunks = from.chunk() - pt.chunk(); + auto chunk_pixels = nchunks * chunk_size; + + auto bb0_ = bbox_from_pos1(from, size); + auto bb0 = bbox{ bb0_.min + chunk_pixels, bb0_.max + chunk_pixels }; + auto bb = bbox_from_pos1(pt, size); + + auto min = Math::min(bb0.min, bb.min); + auto max = Math::max(bb0.max, bb.max); + + return { min, max }; } +static_assert(bbox_from_pos1({{}, {0, 0}, {15, 35}}, {10, 20}) == bbox{{10, 25}, {20, 45}}); +static_assert(bbox_from_pos2({{{1, 1}, {1, 15}, 0}, {1, -1}}, + {{{1, 2}, {1, 0}, 0}, {1, -1}}, + {256, 256}) == bbox{{-63, 831}, {193, 1151}}); + constexpr auto directions = []() constexpr { struct pair { Vector2i dir; uint32_t len; }; @@ -148,33 +159,29 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, cache.allocate(from_, max_dist); - { const auto bb0 = bbox_from_pos(Vector2(from.local()), from_offset, own_size); - constexpr int8_t div_min = -div_factor*2, div_max = div_factor*2; + constexpr int8_t div_min = -div_factor*2, div_max = div_factor*2; + + for (int8_t y = div_min; y <= div_max; y++) + for (int8_t x = div_min; x <= div_max; x++) + { + auto off = Vector2i(x, y) * div_size; + auto pt = object::normalize_coords({from, {}}, off); + auto bb = bbox(bbox_from_pos2(from_, pt, own_size)); + auto dist = distance(from_, pt); - for (int8_t y = div_min; y <= div_max; y++) - for (int8_t x = div_min; x <= div_max; x++) + if (path_search::is_passable(w, from.chunk3(), bb, own_id, p)) { - auto off = Vector2i(x, y) * div_size; - auto pt = object::normalize_coords({from, {}}, off); - auto bb1 = bbox_from_pos(Vector2(pt.local()), pt.offset(), own_size); - auto bb = bbox_union(bb0, bb1); - auto dist = distance(from_, pt); - - if (path_search::is_passable(w, from.chunk3(), bb, own_id, p)) - { - auto idx = (uint32_t)nodes.size(); - cache.add_index(pt, idx); - nodes.push_back({.dist = dist, .prev = (uint32_t)-1, .pt = pt, }); - add_to_heap(idx); - } + auto idx = (uint32_t)nodes.size(); + cache.add_index(pt, idx); + nodes.push_back({.dist = dist, .prev = (uint32_t)-1, .pt = pt, }); + add_to_heap(idx); } - } + } auto closest_dist = distance(from_, to_); auto closest_pos = from_; uint32_t closest_path_len = 0; auto goal_idx = (uint32_t)-1; - const auto goal_bb = bbox_from_pos(Vector2(to_.local()), to_offset, own_size); while (!Q.empty()) { @@ -205,19 +212,13 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, if (auto dist_to_goal = distance_l2(cur_pt, to_); dist_to_goal < goal_thres) [[unlikely]] { - //if (auto dist = cur_dist + dist_to_goal; dist < max_dist) + auto dist = cur_dist; + if (auto bb = bbox(bbox_from_pos2(to_, cur_pt, own_size)); + path_search::is_passable(w, to_.chunk3(), bb, own_id, p)) { - auto dist = cur_dist; - auto vec = Vector2(cur_pt.coord() - to_.coord()) * TILE_SIZE2 + Vector2(cur_pt.offset() - to_.offset()); - auto bb1 = bbox{goal_bb.min + vec, goal_bb.max + vec}; - auto bb = bbox_union(goal_bb, bb1); - - if (path_search::is_passable(w, to_.chunk3(), bb, own_id, p)) - { - goal_idx = id; - max_dist = dist; - continue; // path can only get longer - } + goal_idx = id; + max_dist = dist; + continue; // path can only get longer } } @@ -228,9 +229,8 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, continue; const auto new_pt = object::normalize_coords(cur_pt, vec); - const auto [new_coord, new_offset] = new_pt; auto chunk_idx = cache.get_chunk_index(Vector2i(new_pt.chunk())); - auto tile_idx = cache.get_tile_index(Vector2i(new_pt.local()), new_offset); + auto tile_idx = cache.get_tile_index(Vector2i(new_pt.local()), new_pt.offset()); auto new_idx = cache.lookup_index(chunk_idx, tile_idx); if (new_idx != (uint32_t)-1) @@ -239,13 +239,9 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, continue; } - { const auto bb0 = bbox_from_pos(Vector2(cur_pt.local()), cur_pt.offset(), own_size); - auto vec_ = Vector2(vec); - auto bb1 = bbox{ bb0.min + vec_, bb0.max + vec_ }; - auto bb = bbox_union(bb0, bb1); - if (!path_search::is_passable(w, new_coord.chunk3(), bb, own_id, p)) - continue; - } + if (auto bb = bbox(bbox_from_pos2(new_pt, cur_pt, own_size)); + !path_search::is_passable(w, new_pt.chunk3(), bb, own_id, p)) + continue; if (new_idx == (uint32_t)-1) { @@ -254,7 +250,7 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, cache.add_index(chunk_idx, tile_idx, new_idx); auto new_node = visited { .dist = dist, .prev = id, - .pt = {new_coord, new_offset}, + .pt = new_pt, }; nodes.push_back(new_node); } @@ -268,8 +264,8 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_, #ifndef FM_NO_DEBUG if (debug >= 3) [[unlikely]] DBG_nospace << " path:" << dist - << " pos:" << Vector3i(new_coord) - << ";" << new_offset; + << " pos:" << Vector3i(new_pt.coord()) + << ";" << new_pt.offset(); #endif add_to_heap(new_idx); diff --git a/src/path-search.hpp b/src/path-search.hpp index 4ca5b7b4..6391181b 100644 --- a/src/path-search.hpp +++ b/src/path-search.hpp @@ -35,7 +35,21 @@ public: static constexpr auto div_size = iTILE_SIZE2 / div_factor; static constexpr auto min_size = Vector2ui(div_size * 2); - template struct bbox { VectorTypeFor<2, T> min, max; }; + template + requires std::is_arithmetic_v + struct bbox + { + VectorTypeFor<2, T> min, max; + + template + requires std::is_arithmetic_v + explicit constexpr operator bbox() const { + using Vec = VectorTypeFor<2, U>; + return bbox{ Vec(min), Vec(max) }; + } + + constexpr bool operator==(const bbox&) const = default; + }; using pred = fu2::function_view; -- cgit v1.2.3