diff options
| author | Rémi Verschelde <remi@verschelde.fr> | 2022-04-05 23:55:47 +0200 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2022-04-05 23:55:47 +0200 |
| commit | e4f0fc50f79336cf76beec40e5e8e5164b288714 (patch) | |
| tree | b5bf39537033857249e08b47aca6efc0e3fd3818 | |
| parent | a8ae206a1efd7dcdad2d39aea56d4531a5bb481e (diff) | |
| parent | dcab82ad43e04e9323a45e62a0f16b642ddab8ef (diff) | |
Merge pull request #59930 from Scony/fix-navi-get-simple-path-master
Fix `map_get_path` aka `get_simple_path` behavior in 2D & 3D
| -rw-r--r-- | modules/navigation/nav_map.cpp | 8 |
1 files changed, 6 insertions, 2 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 1151df6390..70d50999f9 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -351,11 +351,15 @@ 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) { - path.push_back(navigation_polys[np_id].entry); + while (np_id != -1 && navigation_polys[np_id].back_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; + path.push_back(point); np_id = navigation_polys[np_id].back_navigation_poly_id; } + path.push_back(begin_point); path.reverse(); } |