Added utility functions to the new NavigationServer:

- Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision = false);
- Vector3 get_closest_point(const Vector3 &p_point);
- Vector3 get_closest_point_normal(const Vector3 &p_point);
- Object *get_closest_point_owner(const Vector3 &p_point);

4 files changed, 179 insertions, 4 deletions

diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 5bafa5507c..1f1783802d 100644
--- a/modules/gdnavigation/gd_navigation_server.cpp
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -170,7 +170,7 @@ COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) {
 }
 
 Vector3 GdNavigationServer::map_get_up(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, Vector3());
 
 	return map->get_up();
@@ -184,7 +184,7 @@ COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
 }
 
 real_t GdNavigationServer::map_get_cell_size(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, 0);
 
 	return map->get_cell_size();
@@ -198,19 +198,47 @@ COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margi
 }
 
 real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, 0);
 
 	return map->get_edge_connection_margin();
 }
 
 Vector<Vector3> GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, Vector<Vector3>());
 
 	return map->get_path(p_origin, p_destination, p_optimize);
 }
 
+Vector3 GdNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point_to_segment(p_from, p_to, p_use_collision);
+}
+
+Vector3 GdNavigationServer::map_get_closest_point(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point(p_point);
+}
+
+Vector3 GdNavigationServer::map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point_normal(p_point);
+}
+
+RID GdNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, RID());
+
+	return map->get_closest_point_owner(p_point);
+}
+
 RID GdNavigationServer::region_create() const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
 	mut_this->operations_mutex->lock();
diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h
index 564e9870a0..7fa5979c31 100644
--- a/modules/gdnavigation/gd_navigation_server.h
+++ b/modules/gdnavigation/gd_navigation_server.h
@@ -103,6 +103,11 @@ public:
 
 	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
 
+	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const;
+	virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const;
+	virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const;
+	virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const;
+
 	virtual RID region_create() const;
 	COMMAND_2(region_set_map, RID, p_region, RID, p_map);
 	COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform);
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
index d1765f4da9..d3e2f8f388 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -401,6 +401,144 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 	return Vector<Vector3>();
 }
 
+Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+
+	bool use_collision = p_use_collision;
+	Vector3 closest_point;
+	real_t closest_point_d = 1e20;
+
+	// Find the initial poly and the end poly on this map.
+	for (size_t i(0); i < polygons.size(); i++) {
+		const gd::Polygon &p = polygons[i];
+
+		// For each point cast a face and check the distance to the segment
+		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+
+			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			Vector3 inters;
+			if (f.intersects_segment(p_from, p_to, &inters)) {
+				const real_t d = closest_point_d = p_from.distance_to(inters);
+				if (use_collision == false) {
+					closest_point = inters;
+					use_collision = true;
+					closest_point_d = d;
+				} else if (closest_point_d > d) {
+
+					closest_point = inters;
+					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;
+
+				Geometry::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;
+}
+
+Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
+	// TODO this is really not optimal, please redesign the API to directly return all this data
+
+	Vector3 closest_point;
+	real_t closest_point_d = 1e20;
+
+	// Find the initial poly and the end poly on this map.
+	for (size_t i(0); i < polygons.size(); i++) {
+		const gd::Polygon &p = polygons[i];
+
+		// For each point cast a face and check the distance to the point
+		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+
+			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			const Vector3 inters = f.get_closest_point_to(p_point);
+			const real_t d = inters.distance_to(p_point);
+			if (d < closest_point_d) {
+				closest_point = inters;
+				closest_point_d = d;
+			}
+		}
+	}
+
+	return closest_point;
+}
+
+Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
+	// TODO this is really not optimal, please redesign the API to directly return all this data
+
+	Vector3 closest_point;
+	Vector3 closest_point_normal;
+	real_t closest_point_d = 1e20;
+
+	// Find the initial poly and the end poly on this map.
+	for (size_t i(0); i < polygons.size(); i++) {
+		const gd::Polygon &p = polygons[i];
+
+		// For each point cast a face and check the distance to the point
+		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+
+			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			const Vector3 inters = f.get_closest_point_to(p_point);
+			const real_t d = inters.distance_to(p_point);
+			if (d < closest_point_d) {
+				closest_point = inters;
+				closest_point_normal = f.get_plane().normal;
+				closest_point_d = d;
+			}
+		}
+	}
+
+	return closest_point_normal;
+}
+
+RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
+	// TODO this is really not optimal, please redesign the API to directly return all this data
+
+	Vector3 closest_point;
+	RID closest_point_owner;
+	real_t closest_point_d = 1e20;
+
+	// Find the initial poly and the end poly on this map.
+	for (size_t i(0); i < polygons.size(); i++) {
+		const gd::Polygon &p = polygons[i];
+
+		// For each point cast a face and check the distance to the point
+		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+
+			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			const Vector3 inters = f.get_closest_point_to(p_point);
+			const real_t d = inters.distance_to(p_point);
+			if (d < closest_point_d) {
+				closest_point = inters;
+				closest_point_owner = p.owner->get_self();
+				closest_point_d = d;
+			}
+		}
+	}
+
+	return closest_point_owner;
+}
+
 void NavMap::add_region(NavRegion *p_region) {
 	regions.push_back(p_region);
 	regenerate_links = true;
diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h
index 128a82580c..4543f00926 100644
--- a/modules/gdnavigation/nav_map.h
+++ b/modules/gdnavigation/nav_map.h
@@ -104,6 +104,10 @@ public:
 	gd::PointKey get_point_key(const Vector3 &p_pos) const;
 
 	Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
+	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
+	Vector3 get_closest_point(const Vector3 &p_point) const;
+	Vector3 get_closest_point_normal(const Vector3 &p_point) const;
+	RID get_closest_point_owner(const Vector3 &p_point) const;
 
 	void add_region(NavRegion *p_region);
 	void remove_region(NavRegion *p_region);