diff options
Diffstat (limited to 'modules')
| -rw-r--r-- | modules/navigation/nav_map.cpp | 20 |
1 files changed, 9 insertions, 11 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 100db9bc82..394c32f20d 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -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. int least_cost_id = 0; + int prev_least_cost_id = -1; bool found_route = false; const gd::Polygon *reachable_end = nullptr; float reachable_d = 1e30; bool is_reachable = true; - gd::NavigationPoly *prev_least_cost_poly = nullptr; - while (true) { // 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++) { - gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; - - const gd::Edge &edge = least_cost_poly->poly->edges[i]; + const gd::Edge &edge = navigation_polys[least_cost_id].poly->edges[i]; // 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++) { @@ -164,17 +161,18 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p continue; } + const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; 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())) { - poly_enter_cost = least_cost_poly->poly->owner->get_enter_cost(); + 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_poly = least_cost_poly; + 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 float new_distance = (least_cost_poly->entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly->traveled_distance; + 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; int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon)); |