From 5253b93373dc90e0788829ff58982491141c5f0f Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 25 Feb 2024 05:29:59 +0100 Subject: rename more search stuff --- src/chunk-region.hpp | 2 +- src/dijkstra.cpp | 501 ----------------------------------------------- src/search-astar.cpp | 372 +++++++++++++++++++++++++++++++++++ src/search-astar.hpp | 49 +---- src/search-cache.cpp | 145 ++++++++++++++ src/search-cache.hpp | 36 ++++ src/search-constants.hpp | 11 ++ src/search.cpp | 1 + src/search.hpp | 18 +- 9 files changed, 575 insertions(+), 560 deletions(-) delete mode 100644 src/dijkstra.cpp create mode 100644 src/search-astar.cpp create mode 100644 src/search-cache.cpp create mode 100644 src/search-cache.hpp create mode 100644 src/search-constants.hpp (limited to 'src') diff --git a/src/chunk-region.hpp b/src/chunk-region.hpp index 579ab1ef..a7281c50 100644 --- a/src/chunk-region.hpp +++ b/src/chunk-region.hpp @@ -1,6 +1,6 @@ #pragma once #include "chunk.hpp" -#include "search.hpp" +#include "search-constants.hpp" #include namespace floormat { diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp deleted file mode 100644 index 15232f90..00000000 --- a/src/dijkstra.cpp +++ /dev/null @@ -1,501 +0,0 @@ -#include "search-astar.hpp" -#include "search-bbox.hpp" -#include "compat/format.hpp" -#include "compat/vector-wrapper.hpp" -#include "compat/heap.hpp" -#include "object.hpp" -#include "world.hpp" -#include "point.hpp" -#include -#include -#include // todo remove -#include -#include -#include - -namespace floormat { - -using visited = astar::visited; -using Search::bbox; -using Search::div_size; -using Search::div_factor; -using Search::min_size; - -namespace { - -constexpr bbox bbox_from_pos1(point pt, Vector2ui size) -{ - 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 }; -} - -constexpr bbox bbox_from_pos2(point pt, point from, Vector2ui size) -{ - 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; }; - constexpr auto len1 = div_size; - constexpr auto len2 = (uint32_t)(len1.length() + 0.5f); // NOLINT - std::array array = {{ - { { -1, -1 }, len2 }, - { { 1, 1 }, len2 }, - { { -1, 1 }, len2 }, - { { 1, -1 }, len2 }, - { { -1, 0 }, len1.x() }, - { { 0, -1 }, len1.y() }, - { { 1, 0 }, len1.x() }, - { { 0, 1 }, len1.y() }, - }}; - for (auto& [vec, len] : array) - { - vec *= div_size; - vec += Vector2i(1); - } -#if 0 - for (auto i = 0uz; i < array.size(); i++) - for (auto j = 0uz; j < i; j++) - fm_assert(array[i].dir != array[j].dir); -#endif - return array; -}(); - -struct heap_comparator -{ - const Array& nodes; // NOLINT - inline heap_comparator(const Array& nodes) : nodes{nodes} {} - inline bool operator()(uint32_t a, uint32_t b) const { return nodes[b].dist < nodes[a].dist; } -}; - -inline uint32_t distance(point a, point b) -{ - Vector2i dist; - dist += (a.coord() - b.coord())*iTILE_SIZE2; - dist += Vector2i(a.offset()) - Vector2i(b.offset()); - return (uint32_t)Math::ceil(Math::sqrt(Vector2(dist).dot())); -} - -inline uint32_t distance_l2(point a, point b) -{ - Vector2i dist; - dist += (a.coord() - b.coord())*iTILE_SIZE2; - dist += Vector2i(a.offset()) - Vector2i(b.offset()); - return (uint32_t)Math::abs(dist).sum(); -} - -void set_result_from_idx(path_search_result& result, const Array& nodes, - point to, const uint32_t idx) -{ - fm_debug_assert(idx != (uint32_t)-1); - - auto& path = result.raw_path().vec; - path.clear(); - - const auto& to_node = nodes[idx]; - if (result.is_found() && 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() -{ - reserve(initial_capacity); -} - -void astar::reserve(size_t capacity) -{ - arrayReserve(nodes, capacity); - arrayReserve(Q, capacity); -} - -void astar::clear() -{ - arrayResize(nodes, 0); - arrayResize(Q, 0); -} - -void astar::add_to_heap(uint32_t id) -{ - arrayAppend(Q, id); - Heap::push_heap(Q.begin(), Q.end(), heap_comparator{nodes}); -} - -uint32_t astar::pop_from_heap() -{ - Heap::pop_heap(Q.begin(), Q.end(), heap_comparator{nodes}); - const auto id = Q.back(); - arrayRemoveSuffix(Q); - return id; -} - -path_search_result astar::Dijkstra(world& w, const point from, const point to, - object_id own_id, uint32_t max_dist, Vector2ub own_size_, - int debug, const pred& p) -{ -#ifdef FM_NO_DEBUG - (void)debug; -#endif - - Timeline timeline; timeline.start(); - - clear(); - cache.allocate(from, max_dist); - - const auto own_size = Math::max(Vector2ui(own_size_), min_size); - constexpr auto goal_thres = (uint32_t)(div_size.length() + 1.5f); - - if (from.coord().z() != to.coord().z()) [[unlikely]] - return {}; - - // todo try removing this eventually - if (from.coord().z() != 0) [[unlikely]] - return {}; - - if (!path_search::is_passable(w, cache, from.coord(), from.offset(), own_size, own_id, p)) - return {}; - - if (!path_search::is_passable(w, cache, to.coord(), to.offset(), own_size, own_id, p)) - return {}; - - 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++) - { - constexpr auto min_dist = (uint32_t)((TILE_SIZE2*2.f).length() + 1.f); - auto off = Vector2i(x, y) * div_size; - auto pt = object::normalize_coords({from.coord(), {}}, off); - auto bb = bbox(bbox_from_pos2(from, pt, own_size)); - auto dist = distance(from, pt) + min_dist; - - if (path_search::is_passable(w, cache, from.chunk3(), bb, own_id, p)) - { - auto idx = (uint32_t)nodes.size(); - cache.add_index(pt, idx); - arrayAppend(nodes, {.dist = dist, .prev = (uint32_t)-1, .pt = pt, }); - add_to_heap(idx); - } - } - - auto closest_dist = (uint32_t)-1; - uint32_t closest_idx = (uint32_t)-1; - auto goal_idx = (uint32_t)-1; - - while (!Q.isEmpty()) - { - const auto cur_idx = pop_from_heap(); - point cur_pt; - uint32_t cur_dist; - { auto& n = nodes[cur_idx]; - cur_pt = n.pt; - cur_dist = n.dist; - } - - if (cur_dist >= max_dist) [[unlikely]] - continue; - - if (auto d = distance(cur_pt, to); d < closest_dist) [[unlikely]] - { - closest_dist = d; - closest_idx = cur_idx; - -#ifndef FM_NO_DEBUG - 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]] - { - auto dist = cur_dist + dist_to_goal; - if (auto bb = bbox(bbox_from_pos2(to, cur_pt, own_size)); - path_search::is_passable(w, cache, to.chunk3(), bb, own_id, p)) - { - goal_idx = cur_idx; - max_dist = dist; - continue; // path can only get longer - } - } - - for (auto [vec, len] : directions) - { - const auto dist = cur_dist + len; - if (dist >= max_dist) - continue; - - const auto new_pt = object::normalize_coords(cur_pt, vec); - auto chunk_idx = cache.get_chunk_index(Vector2i(new_pt.chunk())); - 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) - { - if (nodes[new_idx].dist <= dist) - continue; - } - - if (auto bb = bbox(bbox_from_pos2(new_pt, cur_pt, own_size)); - !path_search::is_passable(w, cache, new_pt.chunk3(), bb, own_id, p)) - continue; - - if (new_idx == (uint32_t)-1) - { - const auto sz = nodes.size(); - new_idx = (uint32_t)sz; - cache.add_index(chunk_idx, tile_idx, new_idx); - auto new_node = visited { - .dist = dist, .prev = cur_idx, - .pt = new_pt, - }; - arrayAppend(nodes, new_node); - } - else - { - auto& n = nodes[new_idx]; - n.dist = dist; - n.prev = cur_idx; - } - -#ifndef FM_NO_DEBUG - if (debug >= 3) [[unlikely]] - DBG_nospace << " path:" << dist - << " pos:" << Vector3i(new_pt.coord()) - << ";" << new_pt.offset(); -#endif - - add_to_heap(new_idx); - } - } - - //fm_debug_assert(nodes.size() == indexes.size()); - - path_search_result result; - - if (goal_idx != (uint32_t)-1) - { - 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) [[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 = 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, - time, d, d0, - d > 0 && d0 > 0 ? (float)d/(float)d0 : 1); - } - 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 = Math::min(len, std::size(buf)-1); - std::fwrite(buf, len, 1, stdout); - std::fflush(stdout); - } - } -#endif - - return result; -} - -} // namespace floormat - -namespace floormat::Search { - -struct chunk_cache -{ - static constexpr size_t dimensions[] = { - TILE_COUNT, - (size_t)div_factor * (size_t)div_factor, - }; - static constexpr size_t size = []() constexpr -> size_t { - size_t x = 1; - for (auto i : dimensions) - x *= i; - return x; - }(); - static constexpr size_t rank = arraySize(dimensions); - - struct index { uint32_t value = 0; }; - class chunk* chunk = nullptr; - std::array indexes = {}; - std::bitset exists{false}; -}; - -cache::cache() = default; - -Vector2ui cache::get_size_to_allocate(uint32_t max_dist) -{ - constexpr auto chunk_size = Vector2ui(iTILE_SIZE2) * TILE_MAX_DIM; - constexpr auto rounding = chunk_size - Vector2ui(1); - auto nchunks = (Vector2ui(max_dist) + rounding) / chunk_size; - return nchunks + Vector2ui(3); -} - -void cache::allocate(point from, uint32_t max_dist) -{ - auto off = get_size_to_allocate(max_dist); - start = Vector2i(from.chunk()) - Vector2i(off); - size = off * 2u + Vector2ui(1); - auto len = size.product(); - if (len > array.size()) - array = Array{ValueInit, len}; - else - for (auto i = 0uz; i < len; i++) - { - array[i].chunk = {}; - array[i].exists = {}; - } -} - -size_t cache::get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord) -{ - auto off = Vector2ui(coord - start); - fm_assert(off < size); - auto index = off.y() * size.x() + off.x(); - fm_debug_assert(index < size.product()); - return index; -} - -size_t cache::get_chunk_index(Vector2i chunk) const { return get_chunk_index(start, size, chunk); } - -size_t cache::get_tile_index(Vector2i pos, Vector2b offset_) -{ - Vector2i offset{offset_}; - constexpr auto tile_start = div_size * div_factor/-2; - offset -= tile_start; - fm_debug_assert(offset >= Vector2i{0, 0} && offset < div_size * div_factor); - auto nth_div = Vector2ui(offset) / Vector2ui(div_size); - const size_t idx[] = { - (size_t)pos.y() * TILE_MAX_DIM + (size_t)pos.x(), - (size_t)nth_div.y() * div_factor + (size_t)nth_div.x(), - }; - size_t index = 0; - for (auto i = 0uz; i < chunk_cache::rank; i++) - { - size_t k = idx[i]; - for (auto j = 0uz; j < i; j++) - k *= chunk_cache::dimensions[j]; - index += k; - } - fm_debug_assert(index < chunk_cache::size); - return index; -} - -void cache::add_index(size_t chunk_index, size_t tile_index, uint32_t index) -{ - fm_debug_assert(index != (uint32_t)-1); - auto& c = array[chunk_index]; - fm_debug_assert(!c.exists[tile_index]); - c.exists[tile_index] = true; - c.indexes[tile_index] = {index}; -} - -void cache::add_index(point pt, uint32_t index) -{ - auto ch = get_chunk_index(Vector2i(pt.chunk())); - auto tile = get_tile_index(Vector2i(pt.local()), pt.offset()); - fm_debug_assert(!array[ch].exists[tile]); - array[ch].exists[tile] = true; - array[ch].indexes[tile] = {index}; -} - -uint32_t cache::lookup_index(size_t chunk_index, size_t tile_index) -{ - auto& c = array[chunk_index]; - if (c.exists[tile_index]) - return c.indexes[tile_index].value; - else - return (uint32_t)-1; -} - -chunk* cache::try_get_chunk(world& w, floormat::chunk_coords_ ch) -{ - auto idx = get_chunk_index({ch.x, ch.y}); - auto& page = array[idx]; - if (page.chunk == (chunk*)-1) - return nullptr; - else if (!page.chunk) - { - page.chunk = w.at(ch); - if (!page.chunk) - { - page.chunk = (chunk*)-1; - return nullptr; - } - return page.chunk; - } - else - return page.chunk; -} - -std::array cache::get_neighbors(world& w, chunk_coords_ ch0) -{ - fm_debug_assert(!size.isZero()); - std::array neighbors; - for (auto i = 0u; i < 8; i++) - neighbors[i] = try_get_chunk(w, ch0 + world::neighbor_offsets[i]); - return neighbors; -} - -} // namespace floormat::Search diff --git a/src/search-astar.cpp b/src/search-astar.cpp new file mode 100644 index 00000000..eb2812a2 --- /dev/null +++ b/src/search-astar.cpp @@ -0,0 +1,372 @@ +#include "search-astar.hpp" +#include "search-bbox.hpp" +#include "search-constants.hpp" +#include "search-cache.hpp" +#include "compat/format.hpp" +#include "compat/vector-wrapper.hpp" +#include "compat/heap.hpp" +#include "object.hpp" +#include "world.hpp" +#include "point.hpp" +#include +#include +#include // todo remove +#include +#include +#include + +namespace floormat { + +struct astar::visited +{ + uint32_t dist = (uint32_t)-1; + uint32_t prev = (uint32_t)-1; + point pt; +}; + +using visited = astar::visited; +using Search::bbox; +using Search::div_size; +using Search::div_factor; +using Search::min_size; + +namespace { + +constexpr bbox bbox_from_pos1(point pt, Vector2ui size) +{ + 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 }; +} + +constexpr bbox bbox_from_pos2(point pt, point from, Vector2ui size) +{ + 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; }; + constexpr auto len1 = div_size; + constexpr auto len2 = (uint32_t)(len1.length() + 0.5f); // NOLINT + std::array array = {{ + { { -1, -1 }, len2 }, + { { 1, 1 }, len2 }, + { { -1, 1 }, len2 }, + { { 1, -1 }, len2 }, + { { -1, 0 }, len1.x() }, + { { 0, -1 }, len1.y() }, + { { 1, 0 }, len1.x() }, + { { 0, 1 }, len1.y() }, + }}; + for (auto& [vec, len] : array) + { + vec *= div_size; + vec += Vector2i(1); + } +#if 0 + for (auto i = 0uz; i < array.size(); i++) + for (auto j = 0uz; j < i; j++) + fm_assert(array[i].dir != array[j].dir); +#endif + return array; +}(); + +struct heap_comparator +{ + const Array& nodes; // NOLINT + inline heap_comparator(const Array& nodes) : nodes{nodes} {} + inline bool operator()(uint32_t a, uint32_t b) const { return nodes[b].dist < nodes[a].dist; } +}; + +inline uint32_t distance(point a, point b) +{ + Vector2i dist; + dist += (a.coord() - b.coord())*iTILE_SIZE2; + dist += Vector2i(a.offset()) - Vector2i(b.offset()); + return (uint32_t)Math::ceil(Math::sqrt(Vector2(dist).dot())); +} + +inline uint32_t distance_l2(point a, point b) +{ + Vector2i dist; + dist += (a.coord() - b.coord())*iTILE_SIZE2; + dist += Vector2i(a.offset()) - Vector2i(b.offset()); + return (uint32_t)Math::abs(dist).sum(); +} + +void set_result_from_idx(path_search_result& result, const Array& nodes, + point to, const uint32_t idx) +{ + fm_debug_assert(idx != (uint32_t)-1); + + auto& path = result.raw_path().vec; + path.clear(); + + const auto& to_node = nodes[idx]; + if (result.is_found() && 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() +{ + reserve(initial_capacity); +} + +astar::~astar() noexcept = default; + +void astar::reserve(size_t capacity) +{ + arrayReserve(nodes, capacity); + arrayReserve(Q, capacity); +} + +void astar::clear() +{ + arrayResize(nodes, 0); + arrayResize(Q, 0); +} + +void astar::add_to_heap(uint32_t id) +{ + arrayAppend(Q, id); + Heap::push_heap(Q.begin(), Q.end(), heap_comparator{nodes}); +} + +uint32_t astar::pop_from_heap() +{ + Heap::pop_heap(Q.begin(), Q.end(), heap_comparator{nodes}); + const auto id = Q.back(); + arrayRemoveSuffix(Q); + return id; +} + +path_search_result astar::Dijkstra(world& w, const point from, const point to, + object_id own_id, uint32_t max_dist, Vector2ub own_size_, + int debug, const pred& p) +{ +#ifdef FM_NO_DEBUG + (void)debug; +#endif + + Timeline timeline; timeline.start(); + + clear(); + _cache->allocate(from, max_dist); + + const auto own_size = Math::max(Vector2ui(own_size_), min_size); + constexpr auto goal_thres = (uint32_t)(div_size.length() + 1.5f); + + if (from.coord().z() != to.coord().z()) [[unlikely]] + return {}; + + // todo try removing this eventually + if (from.coord().z() != 0) [[unlikely]] + return {}; + + if (!path_search::is_passable(w, *_cache, from.coord(), from.offset(), own_size, own_id, p)) + return {}; + + if (!path_search::is_passable(w, *_cache, to.coord(), to.offset(), own_size, own_id, p)) + return {}; + + 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++) + { + constexpr auto min_dist = (uint32_t)((TILE_SIZE2*2.f).length() + 1.f); + auto off = Vector2i(x, y) * div_size; + auto pt = object::normalize_coords({from.coord(), {}}, off); + auto bb = bbox(bbox_from_pos2(from, pt, own_size)); + auto dist = distance(from, pt) + min_dist; + + if (path_search::is_passable(w, *_cache, from.chunk3(), bb, own_id, p)) + { + auto idx = (uint32_t)nodes.size(); + _cache->add_index(pt, idx); + arrayAppend(nodes, {.dist = dist, .prev = (uint32_t)-1, .pt = pt, }); + add_to_heap(idx); + } + } + + auto closest_dist = (uint32_t)-1; + uint32_t closest_idx = (uint32_t)-1; + auto goal_idx = (uint32_t)-1; + + while (!Q.isEmpty()) + { + const auto cur_idx = pop_from_heap(); + point cur_pt; + uint32_t cur_dist; + { auto& n = nodes[cur_idx]; + cur_pt = n.pt; + cur_dist = n.dist; + } + + if (cur_dist >= max_dist) [[unlikely]] + continue; + + if (auto d = distance(cur_pt, to); d < closest_dist) [[unlikely]] + { + closest_dist = d; + closest_idx = cur_idx; + +#ifndef FM_NO_DEBUG + 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]] + { + auto dist = cur_dist + dist_to_goal; + if (auto bb = bbox(bbox_from_pos2(to, cur_pt, own_size)); + path_search::is_passable(w, *_cache, to.chunk3(), bb, own_id, p)) + { + goal_idx = cur_idx; + max_dist = dist; + continue; // path can only get longer + } + } + + for (auto [vec, len] : directions) + { + const auto dist = cur_dist + len; + if (dist >= max_dist) + continue; + + const auto new_pt = object::normalize_coords(cur_pt, vec); + auto chunk_idx = _cache->get_chunk_index(Vector2i(new_pt.chunk())); + 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) + { + if (nodes[new_idx].dist <= dist) + continue; + } + + if (auto bb = bbox(bbox_from_pos2(new_pt, cur_pt, own_size)); + !path_search::is_passable(w, *_cache, new_pt.chunk3(), bb, own_id, p)) + continue; + + if (new_idx == (uint32_t)-1) + { + const auto sz = nodes.size(); + new_idx = (uint32_t)sz; + _cache->add_index(chunk_idx, tile_idx, new_idx); + auto new_node = visited { + .dist = dist, .prev = cur_idx, + .pt = new_pt, + }; + arrayAppend(nodes, new_node); + } + else + { + auto& n = nodes[new_idx]; + n.dist = dist; + n.prev = cur_idx; + } + +#ifndef FM_NO_DEBUG + if (debug >= 3) [[unlikely]] + DBG_nospace << " path:" << dist + << " pos:" << Vector3i(new_pt.coord()) + << ";" << new_pt.offset(); +#endif + + add_to_heap(new_idx); + } + } + + //fm_debug_assert(nodes.size() == indexes.size()); + + path_search_result result; + + if (goal_idx != (uint32_t)-1) + { + 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) [[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 = 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, + time, d, d0, + d > 0 && d0 > 0 ? (float)d/(float)d0 : 1); + } + 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 = Math::min(len, std::size(buf)-1); + std::fwrite(buf, len, 1, stdout); + std::fflush(stdout); + } + } +#endif + + return result; +} + +} // namespace floormat diff --git a/src/search-astar.hpp b/src/search-astar.hpp index 7c48c5db..a56b7f53 100644 --- a/src/search-astar.hpp +++ b/src/search-astar.hpp @@ -1,56 +1,21 @@ #pragma once +#include "compat/safe-ptr.hpp" #include "search.hpp" -#include "point.hpp" -#include +#include "search-constants.hpp" #include -namespace floormat::Search { - -struct cache; -struct chunk_cache; - -struct cache -{ - Vector2ui size; - Vector2i start{(int)((1u << 31) - 1)}; - Array array; - - cache(); - fm_DECLARE_DELETED_COPY_ASSIGNMENT(cache); - - size_t get_chunk_index(Vector2i chunk) const; - static size_t get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord); - static size_t get_tile_index(Vector2i pos, Vector2b offset); - static Vector2ui get_size_to_allocate(uint32_t max_dist); - - void allocate(point from, uint32_t max_dist); - void add_index(size_t chunk_index, size_t tile_index, uint32_t index); - void add_index(point pt, uint32_t index); - uint32_t lookup_index(size_t chunk_index, size_t tile_index); - chunk* try_get_chunk(world& w, chunk_coords_ ch); - - std::array get_neighbors(world& w, chunk_coords_ ch0); -}; - -} // namespace floormat::Search +namespace floormat::Search { struct cache; } namespace floormat { class astar { public: - struct visited - { - uint32_t dist = (uint32_t)-1; - uint32_t prev = (uint32_t)-1; - point pt; - }; - + struct visited; using pred = Search::pred; - fm_DECLARE_DELETED_COPY_ASSIGNMENT(astar); - astar(); + ~astar() noexcept; void reserve(size_t capacity); void clear(); @@ -62,12 +27,10 @@ public: private: static constexpr auto initial_capacity = TILE_COUNT * 16 * Search::div_factor*Search::div_factor; - struct chunk_cache; - void add_to_heap(uint32_t id); uint32_t pop_from_heap(); - struct Search::cache cache; + safe_ptr _cache; Array nodes; Array Q; }; diff --git a/src/search-cache.cpp b/src/search-cache.cpp new file mode 100644 index 00000000..fae54107 --- /dev/null +++ b/src/search-cache.cpp @@ -0,0 +1,145 @@ +#include "search-cache.hpp" +#include "search-constants.hpp" +#include "world.hpp" +#include +#include + +namespace floormat::Search { + +struct cache::chunk_cache +{ + static constexpr size_t dimensions[] = { + TILE_COUNT, + (size_t)div_factor * (size_t)div_factor, + }; + static constexpr size_t size = []() constexpr -> size_t { + size_t x = 1; + for (auto i : dimensions) + x *= i; + return x; + }(); + static constexpr size_t rank = arraySize(dimensions); + + struct index { uint32_t value = 0; }; + class chunk* chunk = nullptr; + std::array indexes = {}; + std::bitset exists{false}; +}; + +cache::cache() = default; + +Vector2ui cache::get_size_to_allocate(uint32_t max_dist) +{ + constexpr auto chunk_size = Vector2ui(iTILE_SIZE2) * TILE_MAX_DIM; + constexpr auto rounding = chunk_size - Vector2ui(1); + auto nchunks = (Vector2ui(max_dist) + rounding) / chunk_size; + return nchunks + Vector2ui(3); +} + +void cache::allocate(point from, uint32_t max_dist) +{ + auto off = get_size_to_allocate(max_dist); + start = Vector2i(from.chunk()) - Vector2i(off); + size = off * 2u + Vector2ui(1); + auto len = size.product(); + if (len > array.size()) + array = Array{ValueInit, len}; + else + for (auto i = 0uz; i < len; i++) + { + array[i].chunk = {}; + array[i].exists = {}; + } +} + +size_t cache::get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord) +{ + auto off = Vector2ui(coord - start); + fm_assert(off < size); + auto index = off.y() * size.x() + off.x(); + fm_debug_assert(index < size.product()); + return index; +} + +size_t cache::get_chunk_index(Vector2i chunk) const { return get_chunk_index(start, size, chunk); } + +size_t cache::get_tile_index(Vector2i pos, Vector2b offset_) +{ + Vector2i offset{offset_}; + constexpr auto tile_start = div_size * div_factor/-2; + offset -= tile_start; + fm_debug_assert(offset >= Vector2i{0, 0} && offset < div_size * div_factor); + auto nth_div = Vector2ui(offset) / Vector2ui(div_size); + const size_t idx[] = { + (size_t)pos.y() * TILE_MAX_DIM + (size_t)pos.x(), + (size_t)nth_div.y() * div_factor + (size_t)nth_div.x(), + }; + size_t index = 0; + for (auto i = 0uz; i < chunk_cache::rank; i++) + { + size_t k = idx[i]; + for (auto j = 0uz; j < i; j++) + k *= chunk_cache::dimensions[j]; + index += k; + } + fm_debug_assert(index < chunk_cache::size); + return index; +} + +void cache::add_index(size_t chunk_index, size_t tile_index, uint32_t index) +{ + fm_debug_assert(index != (uint32_t)-1); + auto& c = array[chunk_index]; + fm_debug_assert(!c.exists[tile_index]); + c.exists[tile_index] = true; + c.indexes[tile_index] = {index}; +} + +void cache::add_index(point pt, uint32_t index) +{ + auto ch = get_chunk_index(Vector2i(pt.chunk())); + auto tile = get_tile_index(Vector2i(pt.local()), pt.offset()); + fm_debug_assert(!array[ch].exists[tile]); + array[ch].exists[tile] = true; + array[ch].indexes[tile] = {index}; +} + +uint32_t cache::lookup_index(size_t chunk_index, size_t tile_index) +{ + auto& c = array[chunk_index]; + if (c.exists[tile_index]) + return c.indexes[tile_index].value; + else + return (uint32_t)-1; +} + +chunk* cache::try_get_chunk(world& w, floormat::chunk_coords_ ch) +{ + auto idx = get_chunk_index({ch.x, ch.y}); + auto& page = array[idx]; + if (page.chunk == (chunk*)-1) + return nullptr; + else if (!page.chunk) + { + page.chunk = w.at(ch); + if (!page.chunk) + { + page.chunk = (chunk*)-1; + return nullptr; + } + return page.chunk; + } + else + return page.chunk; +} + +std::array cache::get_neighbors(world& w, chunk_coords_ ch0) +{ + fm_debug_assert(!size.isZero()); + std::array neighbors; + for (auto i = 0u; i < 8; i++) + neighbors[i] = try_get_chunk(w, ch0 + world::neighbor_offsets[i]); + return neighbors; +} + +} // namespace floormat::Search diff --git a/src/search-cache.hpp b/src/search-cache.hpp new file mode 100644 index 00000000..c205e36f --- /dev/null +++ b/src/search-cache.hpp @@ -0,0 +1,36 @@ +#pragma once +#include "compat/defs.hpp" +#include "point.hpp" +#include +#include + +namespace floormat { class world; class chunk; } + +namespace floormat::Search { + +struct cache +{ + struct chunk_cache; + + Vector2ui size; + Vector2i start{(int)((1u << 31) - 1)}; + Array array; + + cache(); + fm_DECLARE_DELETED_COPY_ASSIGNMENT(cache); + + size_t get_chunk_index(Vector2i chunk) const; + static size_t get_chunk_index(Vector2i start, Vector2ui size, Vector2i coord); + static size_t get_tile_index(Vector2i pos, Vector2b offset); + static Vector2ui get_size_to_allocate(uint32_t max_dist); + + void allocate(point from, uint32_t max_dist); + void add_index(size_t chunk_index, size_t tile_index, uint32_t index); + void add_index(point pt, uint32_t index); + uint32_t lookup_index(size_t chunk_index, size_t tile_index); + chunk* try_get_chunk(world& w, chunk_coords_ ch); + + std::array get_neighbors(world& w, chunk_coords_ ch0); +}; + +} // namespace floormat::Search diff --git a/src/search-constants.hpp b/src/search-constants.hpp new file mode 100644 index 00000000..6696e255 --- /dev/null +++ b/src/search-constants.hpp @@ -0,0 +1,11 @@ +#pragma once +#include "tile-constants.hpp" + +namespace floormat::Search { + +constexpr inline int div_factor = 4; +constexpr inline auto div_size = iTILE_SIZE2 / div_factor; +constexpr inline auto min_size = Vector2ui(div_size * 2); +constexpr inline auto div_count = iTILE_SIZE2 * TILE_MAX_DIM / div_size; + +} // namespace floormat::Search diff --git a/src/search.cpp b/src/search.cpp index f879260b..af32e7a3 100644 --- a/src/search.cpp +++ b/src/search.cpp @@ -1,6 +1,7 @@ #include "search.hpp" #include "search-bbox.hpp" #include "search-astar.hpp" +#include "search-cache.hpp" #include "global-coords.hpp" #include "world.hpp" #include "pass-mode.hpp" diff --git a/src/search.hpp b/src/search.hpp index 024c8613..b70dc049 100644 --- a/src/search.hpp +++ b/src/search.hpp @@ -6,28 +6,16 @@ #include "search-pred.hpp" #include -namespace floormat { -class world; -struct object; -class chunk; -} // namespace floormat - -// todo add pathfinding sub-namespace - namespace floormat::Search { - template struct bbox; struct cache; -struct chunk_cache; -constexpr inline int div_factor = 4; -constexpr inline auto div_size = iTILE_SIZE2 / div_factor; -constexpr inline auto min_size = Vector2ui(div_size * 2); -constexpr inline auto div_count = iTILE_SIZE2 * TILE_MAX_DIM / Search::div_size; - } // namespace floormat::Search namespace floormat { +class world; +struct object; +class chunk; struct path_search_result; class path_search final -- cgit v1.2.3