asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #66490 from timothyqiu/navmap-realloc

Browse source 
get_path()`
This commit is contained in:
Rémi Verschelde 2022-09-28 10:10:48 +02:00
commit f7b21e88d0
1 changed files with 9 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
modules/navigation/nav_map.cpp
View file

@ -140,20 +140,17 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// This is an implementation of the A* algorithm. // This is an implementation of the A* algorithm.
int least_cost_id = 0; int least_cost_id = 0;
int prev_least_cost_id = -1;
bool found_route = false; bool found_route = false;
const gd::Polygon *reachable_end = nullptr; const gd::Polygon *reachable_end = nullptr;
float reachable_d = 1e30; float reachable_d = 1e30;
bool is_reachable = true; bool is_reachable = true;
gd::NavigationPoly *prev_least_cost_poly = nullptr;
while (true) { while (true) {
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) { for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; const gd::Edge &edge = navigation_polys[least_cost_id].poly->edges[i];
const gd::Edge &edge = least_cost_poly->poly->edges[i];
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
@ -164,17 +161,18 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
continue; continue;
} }
const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
float poly_enter_cost = 0.0; float poly_enter_cost = 0.0;
float poly_travel_cost = least_cost_poly->poly->owner->get_travel_cost(); float poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
if (prev_least_cost_poly != nullptr && (prev_least_cost_poly->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(); poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
} }
prev_least_cost_poly = least_cost_poly; prev_least_cost_id = least_cost_id;
Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; 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 Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly->traveled_distance; const float 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)); int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));