diff options
| author | Pawel Lampe <pawel.lampe@gmail.com> | 2022-04-05 23:24:03 +0200 |
|---|---|---|
| committer | Pawel Lampe <pawel.lampe@gmail.com> | 2022-04-05 23:26:37 +0200 |
| commit | dcab82ad43e04e9323a45e62a0f16b642ddab8ef (patch) | |
| tree | b443fed1ca509aa64791ff45b5823cddc924bd00 | |
| parent | 77843355a04361c4a770f870e9406d4a748fb0ca (diff) | |
Fix 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(); } |