From 0909a9adb5080523bf7baaec110b444ff6b44a75 Mon Sep 17 00:00:00 2001 From: Pawel Lampe Date: Wed, 16 Feb 2022 23:22:13 +0100 Subject: [PATCH] Fix `get_simple_path` behavior in 2D, fixes #56852 --- modules/navigation/nav_map.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 9f1f44d26..782b0595c 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -378,19 +378,16 @@ Vector 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(); }