diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 100db9bc824..394c32f20d5 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));