Skip to content

Commit

Permalink
Merge pull request #59930 from Scony/fix-navi-get-simple-path-master
Browse files Browse the repository at this point in the history
Fix `map_get_path` aka `get_simple_path` behavior in 2D & 3D
  • Loading branch information
akien-mga authored Apr 5, 2022
2 parents a8ae206 + dcab82a commit e4f0fc5
Showing 1 changed file with 6 additions and 2 deletions.
8 changes: 6 additions & 2 deletions modules/navigation/nav_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down

0 comments on commit e4f0fc5

Please sign in to comment.