Fixed closest edge and face check in NavigationServer3D.map_get_closest_point_to_segment

1 files changed, 13 insertions, 17 deletions

diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 1779be2cc2..f0184a155b 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 (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);
+			// 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;
+				}
 
-				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;
 				}
 			}
 		}