Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve pathfinding performance by using a heap to store traversable polygons #85965

Merged
merged 1 commit into from
Sep 3, 2024
Merged

Improve pathfinding performance by using a heap to store traversable polygons #85965

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
145 changes: 70 additions & 75 deletions modules/navigation/nav_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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;

int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));

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;
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;

// 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;

// 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);

// Add the neighbor polygon to the polygons to visit.
to_visit.push_back(navigation_polys.size() - 1);
// 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 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;
Expand Down Expand Up @@ -366,40 +372,27 @@ 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;

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;
}
}

ERR_BREAK(least_cost_id == -1);
// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
least_cost_id = traversable_polys.pop()->poly->id;

// 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;
}
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down
Loading
Loading