From b8fc6b4979ebcdee6ece1fe8b05ca4d82b9a6951 Mon Sep 17 00:00:00 2001 From: Artem Date: Mon, 24 Jun 2024 13:57:46 +0400 Subject: [PATCH] NavigationServer3D.map_get_closest_point_to_segment - add an additional shortest distance check For a case when shortest distance is between some point located on a face's edge and some point located on a line segment. --- modules/navigation/nav_map.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 8a7da64eb5e..f917c988eaa 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -644,6 +644,26 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector } } } + // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment. + if (!use_collision) { + for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { + Vector3 a, b; + + Geometry3D::get_closest_points_between_segments( + p_from, + p_to, + p.points[point_id].pos, + p.points[(point_id + 1) % p.points.size()].pos, + a, + b); + + const real_t d = a.distance_to(b); + if (d < closest_point_d) { + closest_point_d = d; + closest_point = b; + } + } + } } return closest_point;