From 4ed747e2553e0759e269db6b8fc750997db4983c Mon Sep 17 00:00:00 2001 From: Artem Date: Sun, 16 Jun 2024 16:23:27 +0400 Subject: [PATCH] Fixed closest edge and face check in NavigationServer3D.map_get_closest_point_to_segment --- modules/navigation/nav_map.cpp | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 1779be2cc2e..f0184a155bf 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -627,24 +627,20 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector closest_point_d = d; } } - } + // If segment does not itersect face, check the distance from segment's endpoints. + else if (!use_collision) { + const Vector3 p_from_closest = f.get_closest_point_to(p_from); + const real_t d_p_from = p_from.distance_to(p_from_closest); + if (closest_point_d > d_p_from) { + closest_point = p_from_closest; + closest_point_d = d_p_from; + } - if (use_collision == false) { - 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; + const Vector3 p_to_closest = f.get_closest_point_to(p_to); + const real_t d_p_to = p_to.distance_to(p_to_closest); + if (closest_point_d > d_p_to) { + closest_point = p_to_closest; + closest_point_d = d_p_to; } } }