Merge pull request #85965 from ershn/use_heap_in_astar_path_finding
Improve pathfinding performance by using a heap to store traversable polygons
This commit is contained in:
Rémi Verschelde
commit
e004ae7bbe
|
|
@ -221,27 +221,27 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
|
|||
|
||||
// List of all reachable navigation polys.
|
||||
LocalVector<gd::NavigationPoly> 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<uint32_t> to_visit;
|
||||
to_visit.push_back(0);
|
||||
// Heap of polygons to travel next.
|
||||
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
|
||||
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<Vector3> 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<Vector3> 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<Vector3> 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<uint32_t>::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<gd::Polygon> &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, Vector<gd::Edge::Connection>, 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();
|
||||
|
|
|
|||
|
|
@ -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 <typename T>
|
||||
struct NoopIndexer {
|
||||
void operator()(const T &p_value, uint32_t p_index) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* A max-heap implementation that notifies of element index changes.
|
||||
*/
|
||||
template <typename T, typename LessThan = Comparator<T>, typename Indexer = NoopIndexer<T>>
|
||||
class Heap {
|
||||
LocalVector<T> _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
|
||||
|
|
|
|||
|
|
@ -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<int> 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<int> 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<int> 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<int, GreaterThan> 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<int> 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<uint32_t, CompareArrayValues, RegisterHeapIndexes> 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<uint32_t, Comparator<uint32_t>, 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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue