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

Backport navigation crash fixes

Browse source 
Co-Authored-By: Sean <sean@geekotron.net>
This commit is contained in:
Haoyu Qiu 2022-11-22 12:46:43 +08:00
parent ab65273087
commit 37ccdb201a
1 changed files with 10 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
modules/navigation/nav_map.cpp
View file

@ -141,20 +141,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++) {
@ -165,17 +162,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 region_enter_cost = 0.0; float region_enter_cost = 0.0;
float region_travel_cost = least_cost_poly->poly->owner->get_travel_cost(); float region_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())) {
region_enter_cost = least_cost_poly->poly->owner->get_enter_cost(); region_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 = Geometry::get_closest_point_to_segment(least_cost_poly->entry, pathway); const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly.entry, pathway);
const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance; const float new_distance = (least_cost_poly.entry.distance_to(new_entry) * region_travel_cost) + region_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));
@ -241,6 +239,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
to_visit.clear(); to_visit.clear();
to_visit.push_back(0); to_visit.push_back(0);
least_cost_id = 0; least_cost_id = 0;
prev_least_cost_id = -1;
reachable_end = nullptr; reachable_end = nullptr;