From e3f2ee62ca5cfee09d26efab969ba6ad1272c857 Mon Sep 17 00:00:00 2001 From: Relintai Date: Mon, 4 Sep 2023 17:40:28 +0200 Subject: [PATCH] Backported from godot4: Fix pathfinding funnel adding unwanted point Fixes pathfinding funnel adding unwanted point due to precision issues. - smix8 https://github.com/godotengine/godot/commit/c51e2644466b96d414d4e42a9cfe283ce1162264 --- modules/navigation/nav_map.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 17d1df071..e95a98854 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -444,6 +444,17 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p if (p_optimize) { // Set the apex poly/point to the end point gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; + + Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; + const Vector3 back_edge_closest_point = Geometry::get_closest_point_to_segment(end_point, back_pathway); + if (end_point.is_equal_approx(back_edge_closest_point)) { + // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. + // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. + if (apex_poly->back_navigation_poly_id != -1) { + apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id]; + } + } + Vector3 apex_point = end_point; gd::NavigationPoly *left_poly = apex_poly;