From c3ee32f106f9b6c78a24124ee4a56979824f4267 Mon Sep 17 00:00:00 2001 From: Ershn Date: Thu, 7 Dec 2023 10:21:10 +0900 Subject: [PATCH] Use a heap to store traversable polygons for pathfinding --- modules/navigation/nav_map.cpp | 139 ++++++++--------- modules/navigation/nav_utils.h | 176 ++++++++++++++++++++-- tests/servers/test_navigation_server_3d.h | 160 ++++++++++++++++++++ 3 files changed, 391 insertions(+), 84 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 0c91e8dea3e..f89f5b5812e 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -221,27 +221,27 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p // List of all reachable navigation polys. LocalVector navigation_polys; - navigation_polys.reserve(polygons.size() * 0.75); + navigation_polys.resize(polygons.size() + link_polygons.size()); - // Add the start polygon to the reachable navigation polygons. - gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly); - begin_navigation_poly.self_id = 0; + // Initialize the matching navigation polygon. + gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; + begin_navigation_poly.poly = begin_poly; begin_navigation_poly.entry = begin_point; begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; - navigation_polys.push_back(begin_navigation_poly); - // List of polygon IDs to visit. - List to_visit; - to_visit.push_back(0); + // Heap of polygons to travel next. + gd::Heap + traversable_polys; + traversable_polys.reserve(polygons.size() * 0.25); // This is an implementation of the A* algorithm. - int least_cost_id = 0; + int least_cost_id = begin_poly->id; int prev_least_cost_id = -1; bool found_route = false; const gd::Polygon *reachable_end = nullptr; - real_t reachable_d = FLT_MAX; + real_t distance_to_reachable_end = FLT_MAX; bool is_reachable = true; while (true) { @@ -260,51 +260,57 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p real_t poly_enter_cost = 0.0; real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); - if (prev_least_cost_id != -1 && (navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self())) { + if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); } prev_least_cost_id = least_cost_id; Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); - const real_t new_distance = (least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly.traveled_distance; + const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance; - int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon)); + // Check if the neighbor polygon has already been processed. + gd::NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; + if (neighbor_poly.poly != nullptr) { + // If the neighbor polygon hasn't been traversed yet and the new path leading to + // it is shorter, update the polygon. + if (neighbor_poly.traversable_poly_index < traversable_polys.size() && + new_traveled_distance < neighbor_poly.traveled_distance) { + neighbor_poly.back_navigation_poly_id = least_cost_id; + neighbor_poly.back_navigation_edge = connection.edge; + neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start; + neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end; + neighbor_poly.traveled_distance = new_traveled_distance; + neighbor_poly.distance_to_destination = + new_entry.distance_to(end_point) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; - if (already_visited_polygon_index != -1) { - // Polygon already visited, check if we can reduce the travel cost. - gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index]; - if (new_distance < avp.traveled_distance) { - avp.back_navigation_poly_id = least_cost_id; - avp.back_navigation_edge = connection.edge; - avp.back_navigation_edge_pathway_start = connection.pathway_start; - avp.back_navigation_edge_pathway_end = connection.pathway_end; - avp.traveled_distance = new_distance; - avp.entry = new_entry; + // Update the priority of the polygon in the heap. + traversable_polys.shift(neighbor_poly.traversable_poly_index); } } else { - // Add the neighbor polygon to the reachable ones. - gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon); - new_navigation_poly.self_id = navigation_polys.size(); - new_navigation_poly.back_navigation_poly_id = least_cost_id; - new_navigation_poly.back_navigation_edge = connection.edge; - new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start; - new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end; - new_navigation_poly.traveled_distance = new_distance; - new_navigation_poly.entry = new_entry; - navigation_polys.push_back(new_navigation_poly); + // Initialize the matching navigation polygon. + neighbor_poly.poly = connection.polygon; + neighbor_poly.back_navigation_poly_id = least_cost_id; + neighbor_poly.back_navigation_edge = connection.edge; + neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start; + neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end; + neighbor_poly.traveled_distance = new_traveled_distance; + neighbor_poly.distance_to_destination = + new_entry.distance_to(end_point) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; - // Add the neighbor polygon to the polygons to visit. - to_visit.push_back(navigation_polys.size() - 1); + // Add the polygon to the heap of polygons to traverse next. + traversable_polys.push(&neighbor_poly); } } } - // Removes the least cost polygon from the list of polygons to visit so we can advance. - to_visit.erase(least_cost_id); - - // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable - if (to_visit.size() == 0) { + // When the heap of traversable polygons is empty at this point it means the end polygon is + // unreachable. + if (traversable_polys.is_empty()) { // Thus use the further reachable polygon ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); is_reachable = false; @@ -366,13 +372,12 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return path; } - // Reset open and navigation_polys - gd::NavigationPoly np = navigation_polys[0]; - navigation_polys.clear(); - navigation_polys.push_back(np); - to_visit.clear(); - to_visit.push_back(0); - least_cost_id = 0; + for (gd::NavigationPoly &nav_poly : navigation_polys) { + nav_poly.poly = nullptr; + } + navigation_polys[begin_poly->id].poly = begin_poly; + + least_cost_id = begin_poly->id; prev_least_cost_id = -1; reachable_end = nullptr; @@ -380,26 +385,14 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p continue; } - // Find the polygon with the minimum cost from the list of polygons to visit. - least_cost_id = -1; - real_t least_cost = FLT_MAX; - for (List::Element *element = to_visit.front(); element != nullptr; element = element->next()) { - gd::NavigationPoly *np = &navigation_polys[element->get()]; - real_t cost = np->traveled_distance; - cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost()); - if (cost < least_cost) { - least_cost_id = np->self_id; - least_cost = cost; - } - } + // Pop the polygon with the lowest travel cost from the heap of traversable polygons. + least_cost_id = traversable_polys.pop()->poly->id; - ERR_BREAK(least_cost_id == -1); - - // Stores the further reachable end polygon, in case our goal is not reachable. + // Store the farthest reachable end polygon in case our goal is not reachable. if (is_reachable) { - real_t d = navigation_polys[least_cost_id].entry.distance_to(p_destination); - if (reachable_d > d) { - reachable_d = d; + real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_destination); + if (distance_to_reachable_end > distance) { + distance_to_reachable_end = distance; reachable_end = navigation_polys[least_cost_id].poly; } } @@ -943,29 +936,30 @@ void NavMap::sync() { } // Resize the polygon count. - int count = 0; + int polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } - count += region->get_polygons().size(); + polygon_count += region->get_polygons().size(); } - polygons.resize(count); + polygons.resize(polygon_count); // Copy all region polygons in the map. - count = 0; + polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } const LocalVector &polygons_source = region->get_polygons(); for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons[count + n] = polygons_source[n]; + polygons[polygon_count] = polygons_source[n]; + polygons[polygon_count].id = polygon_count; + polygon_count++; } - count += region->get_polygons().size(); } - _new_pm_polygon_count = polygons.size(); + _new_pm_polygon_count = polygon_count; // Group all edges per key. HashMap, gd::EdgeKey> connections; @@ -1136,6 +1130,7 @@ void NavMap::sync() { // If we have both a start and end point, then create a synthetic polygon to route through. if (closest_start_polygon && closest_end_polygon) { gd::Polygon &new_polygon = link_polygons[link_poly_idx++]; + new_polygon.id = polygon_count++; new_polygon.owner = link; new_polygon.edges.clear(); diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h index c3939e9979f..ba4c44b748f 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -98,6 +98,9 @@ struct Edge { }; struct Polygon { + /// Id of the polygon in the map. + uint32_t id = UINT32_MAX; + /// Navigation region or link that contains this polygon. const NavBase *owner = nullptr; @@ -111,9 +114,11 @@ struct Polygon { }; struct NavigationPoly { - uint32_t self_id = 0; /// This poly. - const Polygon *poly; + const Polygon *poly = nullptr; + + /// Index in the heap of traversable polygons. + uint32_t traversable_poly_index = UINT32_MAX; /// Those 4 variables are used to travel the path backwards. int back_navigation_poly_id = -1; @@ -123,20 +128,44 @@ struct NavigationPoly { /// The entry position of this poly. Vector3 entry; - /// The distance to the destination. + /// The distance traveled until now (g cost). real_t traveled_distance = 0.0; + /// The distance to the destination (h cost). + real_t distance_to_destination = 0.0; - NavigationPoly() { poly = nullptr; } - - NavigationPoly(const Polygon *p_poly) : - poly(p_poly) {} - - bool operator==(const NavigationPoly &other) const { - return poly == other.poly; + /// The total travel cost (f cost). + real_t total_travel_cost() const { + return traveled_distance + distance_to_destination; } - bool operator!=(const NavigationPoly &other) const { - return !operator==(other); + bool operator==(const NavigationPoly &p_other) const { + return poly == p_other.poly; + } + + bool operator!=(const NavigationPoly &p_other) const { + return !(*this == p_other); + } +}; + +struct NavPolyTravelCostGreaterThan { + // Returns `true` if the travel cost of `a` is higher than that of `b`. + bool operator()(const NavigationPoly *p_poly_a, const NavigationPoly *p_poly_b) const { + real_t f_cost_a = p_poly_a->total_travel_cost(); + real_t h_cost_a = p_poly_a->distance_to_destination; + real_t f_cost_b = p_poly_b->total_travel_cost(); + real_t h_cost_b = p_poly_b->distance_to_destination; + + if (f_cost_a != f_cost_b) { + return f_cost_a > f_cost_b; + } else { + return h_cost_a > h_cost_b; + } + } +}; + +struct NavPolyHeapIndexer { + void operator()(NavigationPoly *p_poly, uint32_t p_heap_index) const { + p_poly->traversable_poly_index = p_heap_index; } }; @@ -146,6 +175,129 @@ struct ClosestPointQueryResult { RID owner; }; +template +struct NoopIndexer { + void operator()(const T &p_value, uint32_t p_index) {} +}; + +/** + * A max-heap implementation that notifies of element index changes. + */ +template , typename Indexer = NoopIndexer> +class Heap { + LocalVector _buffer; + + LessThan _less_than; + Indexer _indexer; + +public: + void reserve(uint32_t p_size) { + _buffer.reserve(p_size); + } + + uint32_t size() const { + return _buffer.size(); + } + + bool is_empty() const { + return _buffer.is_empty(); + } + + void push(const T &p_element) { + _buffer.push_back(p_element); + _indexer(p_element, _buffer.size() - 1); + _shift_up(_buffer.size() - 1); + } + + T pop() { + ERR_FAIL_COND_V_MSG(_buffer.is_empty(), T(), "Can't pop an empty heap."); + T value = _buffer[0]; + _indexer(value, UINT32_MAX); + if (_buffer.size() > 1) { + _buffer[0] = _buffer[_buffer.size() - 1]; + _indexer(_buffer[0], 0); + _buffer.remove_at(_buffer.size() - 1); + _shift_down(0); + } else { + _buffer.remove_at(_buffer.size() - 1); + } + return value; + } + + /** + * Update the position of the element in the heap if necessary. + */ + void shift(uint32_t p_index) { + ERR_FAIL_UNSIGNED_INDEX_MSG(p_index, _buffer.size(), "Heap element index is out of range."); + if (!_shift_up(p_index)) { + _shift_down(p_index); + } + } + + void clear() { + for (const T &value : _buffer) { + _indexer(value, UINT32_MAX); + } + _buffer.clear(); + } + + Heap() {} + + Heap(const LessThan &p_less_than) : + _less_than(p_less_than) {} + + Heap(const Indexer &p_indexer) : + _indexer(p_indexer) {} + + Heap(const LessThan &p_less_than, const Indexer &p_indexer) : + _less_than(p_less_than), _indexer(p_indexer) {} + +private: + bool _shift_up(uint32_t p_index) { + T value = _buffer[p_index]; + uint32_t current_index = p_index; + uint32_t parent_index = (current_index - 1) / 2; + while (current_index > 0 && _less_than(_buffer[parent_index], value)) { + _buffer[current_index] = _buffer[parent_index]; + _indexer(_buffer[current_index], current_index); + current_index = parent_index; + parent_index = (current_index - 1) / 2; + } + if (current_index != p_index) { + _buffer[current_index] = value; + _indexer(value, current_index); + return true; + } else { + return false; + } + } + + bool _shift_down(uint32_t p_index) { + T value = _buffer[p_index]; + uint32_t current_index = p_index; + uint32_t child_index = 2 * current_index + 1; + while (child_index < _buffer.size()) { + if (child_index + 1 < _buffer.size() && + _less_than(_buffer[child_index], _buffer[child_index + 1])) { + child_index++; + } + if (_less_than(_buffer[child_index], value)) { + break; + } + _buffer[current_index] = _buffer[child_index]; + _indexer(_buffer[current_index], current_index); + current_index = child_index; + child_index = 2 * current_index + 1; + } + if (current_index != p_index) { + _buffer[current_index] = value; + _indexer(value, current_index); + return true; + } else { + return false; + } + } +}; } // namespace gd #endif // NAV_UTILS_H diff --git a/tests/servers/test_navigation_server_3d.h b/tests/servers/test_navigation_server_3d.h index cf6b89c3301..4411b1aae5c 100644 --- a/tests/servers/test_navigation_server_3d.h +++ b/tests/servers/test_navigation_server_3d.h @@ -31,6 +31,7 @@ #ifndef TEST_NAVIGATION_SERVER_3D_H #define TEST_NAVIGATION_SERVER_3D_H +#include "modules/navigation/nav_utils.h" #include "scene/3d/mesh_instance_3d.h" #include "scene/resources/3d/primitive_meshes.h" #include "servers/navigation_server_3d.h" @@ -61,6 +62,32 @@ static inline Array build_array(Variant item, Targs... Fargs) { return a; } +struct GreaterThan { + bool operator()(int p_a, int p_b) const { return p_a > p_b; } +}; + +struct CompareArrayValues { + const int *array; + + CompareArrayValues(const int *p_array) : + array(p_array) {} + + bool operator()(uint32_t p_index_a, uint32_t p_index_b) const { + return array[p_index_a] < array[p_index_b]; + } +}; + +struct RegisterHeapIndexes { + uint32_t *indexes; + + RegisterHeapIndexes(uint32_t *p_indexes) : + indexes(p_indexes) {} + + void operator()(uint32_t p_vector_index, uint32_t p_heap_index) { + indexes[p_vector_index] = p_heap_index; + } +}; + TEST_SUITE("[Navigation]") { TEST_CASE("[NavigationServer3D] Server should be empty when initialized") { NavigationServer3D *navigation_server = NavigationServer3D::get_singleton(); @@ -788,6 +815,139 @@ TEST_SUITE("[Navigation]") { CHECK_EQ(navigation_mesh->get_vertices().size(), 0); } */ + + TEST_CASE("[Heap] size") { + gd::Heap heap; + + CHECK(heap.size() == 0); + + heap.push(0); + CHECK(heap.size() == 1); + + heap.push(1); + CHECK(heap.size() == 2); + + heap.pop(); + CHECK(heap.size() == 1); + + heap.pop(); + CHECK(heap.size() == 0); + } + + TEST_CASE("[Heap] is_empty") { + gd::Heap heap; + + CHECK(heap.is_empty() == true); + + heap.push(0); + CHECK(heap.is_empty() == false); + + heap.pop(); + CHECK(heap.is_empty() == true); + } + + TEST_CASE("[Heap] push/pop") { + SUBCASE("Default comparator") { + gd::Heap heap; + + heap.push(2); + heap.push(7); + heap.push(5); + heap.push(3); + heap.push(4); + + CHECK(heap.pop() == 7); + CHECK(heap.pop() == 5); + CHECK(heap.pop() == 4); + CHECK(heap.pop() == 3); + CHECK(heap.pop() == 2); + } + + SUBCASE("Custom comparator") { + GreaterThan greaterThan; + gd::Heap heap(greaterThan); + + heap.push(2); + heap.push(7); + heap.push(5); + heap.push(3); + heap.push(4); + + CHECK(heap.pop() == 2); + CHECK(heap.pop() == 3); + CHECK(heap.pop() == 4); + CHECK(heap.pop() == 5); + CHECK(heap.pop() == 7); + } + + SUBCASE("Intermediate pops") { + gd::Heap heap; + + heap.push(0); + heap.push(3); + heap.pop(); + heap.push(1); + heap.push(2); + + CHECK(heap.pop() == 2); + CHECK(heap.pop() == 1); + CHECK(heap.pop() == 0); + } + } + + TEST_CASE("[Heap] shift") { + int values[] = { 5, 3, 6, 7, 1 }; + uint32_t heap_indexes[] = { 0, 0, 0, 0, 0 }; + CompareArrayValues comparator(values); + RegisterHeapIndexes indexer(heap_indexes); + gd::Heap heap(comparator, indexer); + + heap.push(0); + heap.push(1); + heap.push(2); + heap.push(3); + heap.push(4); + + // Shift down: 6 -> 2 + values[2] = 2; + heap.shift(heap_indexes[2]); + + // Shift up: 5 -> 8 + values[0] = 8; + heap.shift(heap_indexes[0]); + + CHECK(heap.pop() == 0); + CHECK(heap.pop() == 3); + CHECK(heap.pop() == 1); + CHECK(heap.pop() == 2); + CHECK(heap.pop() == 4); + + CHECK(heap_indexes[0] == UINT32_MAX); + CHECK(heap_indexes[1] == UINT32_MAX); + CHECK(heap_indexes[2] == UINT32_MAX); + CHECK(heap_indexes[3] == UINT32_MAX); + CHECK(heap_indexes[4] == UINT32_MAX); + } + + TEST_CASE("[Heap] clear") { + uint32_t heap_indexes[] = { 0, 0, 0, 0 }; + RegisterHeapIndexes indexer(heap_indexes); + gd::Heap, RegisterHeapIndexes> heap(indexer); + + heap.push(0); + heap.push(2); + heap.push(1); + heap.push(3); + + heap.clear(); + + CHECK(heap.size() == 0); + + CHECK(heap_indexes[0] == UINT32_MAX); + CHECK(heap_indexes[1] == UINT32_MAX); + CHECK(heap_indexes[2] == UINT32_MAX); + CHECK(heap_indexes[3] == UINT32_MAX); + } } } //namespace TestNavigationServer3D