diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 09dac702c4c..4448ddc2abe 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -376,19 +376,16 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 
 			// Add mid points
 			int np_id = least_cost_id;
-			while (np_id != -1) {
-#ifdef USE_ENTRY_POINT
-				Vector3 point = navigation_polys[np_id].entry;
-#else
+			while (np_id != -1 && navigation_polys[np_id].prev_navigation_poly_id != -1) {
 				int prev = navigation_polys[np_id].back_navigation_edge;
 				int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
 				Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
-#endif
-
 				path.push_back(point);
 				np_id = navigation_polys[np_id].prev_navigation_poly_id;
 			}
 
+			path.push_back(begin_point);
+
 			path.invert();
 		}