NavMap::get_path Fix not resetting least_cost_id

modules/navigation/nav_map.cpp

@@ -156,10 +156,10 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 
 	while (found_route == false) {
 		{
+			gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+
 			// Takes the current least_cost_poly neighbors and compute the traveled_distance of each
 			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];
 				if (!edge.other_polygon)
 					continue;
@@ -241,6 +241,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			navigation_polys.push_back(np);
 			open_list.clear();
 			open_list.push_back(0);
+			least_cost_id = 0;
 
 			reachable_end = NULL;
 
@@ -265,6 +266,8 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			}
 		}
 
+		ERR_BREAK(least_cost_id == -1);
+
 		// Stores the further reachable end polygon, in case our goal is not reachable.
 		if (is_reachable) {
 			float d = navigation_polys[least_cost_id].entry.distance_to(p_destination);
@@ -274,8 +277,6 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			}
 		}
 
-		ERR_BREAK(least_cost_id == -1);
-
 		// Check if we reached the end
 		if (navigation_polys[least_cost_id].poly == end_poly) {
 			// Yep, done!!