Add navigation region point and segment queries

Adds point and segment queries for regions, e.g. closet point, point normal, or segment intersection.
smix8 · Aug 25, 2024 · 5fc1ea5 · 5fc1ea5
1 parent 28a72fa
commit 5fc1ea5
Show file tree

Hide file tree

Showing 14 changed files with 181 additions and 8 deletions.
diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml
@@ -486,15 +486,15 @@
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="to_point" type="Vector2" />
 			<description>
-				Returns the point closest to the provided [param to_point] on the navigation mesh surface.
+				Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param map].
 			</description>
 		</method>
 		<method name="map_get_closest_point_owner" qualifiers="const">
 			<return type="RID" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="to_point" type="Vector2" />
 			<description>
-				Returns the owner region RID for the point returned by [method map_get_closest_point].
+				Returns the owner region RID for the navigation mesh surface point closest to the provided [param to_point] on the navigation [param map].
 			</description>
 		</method>
 		<method name="map_get_edge_connection_margin" qualifiers="const">
@@ -768,6 +768,14 @@
 				Creates a new region.
 			</description>
 		</method>
+		<method name="region_get_closest_point" qualifiers="const">
+			<return type="Vector2" />
+			<param index="0" name="region" type="RID" />
+			<param index="1" name="to_point" type="Vector2" />
+			<description>
+				Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param region].
+			</description>
+		</method>
 		<method name="region_get_connection_pathway_end" qualifiers="const">
 			<return type="Vector2" />
 			<param index="0" name="region" type="RID" />

diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml
@@ -532,23 +532,23 @@
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="to_point" type="Vector3" />
 			<description>
-				Returns the point closest to the provided [param to_point] on the navigation mesh surface.
+				Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param map].
 			</description>
 		</method>
 		<method name="map_get_closest_point_normal" qualifiers="const">
 			<return type="Vector3" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="to_point" type="Vector3" />
 			<description>
-				Returns the normal for the point returned by [method map_get_closest_point].
+				Returns the navigation mesh surface normal closest to the provided [param to_point] on the navigation [param map].
 			</description>
 		</method>
 		<method name="map_get_closest_point_owner" qualifiers="const">
 			<return type="RID" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="to_point" type="Vector3" />
 			<description>
-				Returns the owner region RID for the point returned by [method map_get_closest_point].
+				Returns the owner region RID for the navigation mesh surface point closest to the provided [param to_point] on the navigation [param map].
 			</description>
 		</method>
 		<method name="map_get_closest_point_to_segment" qualifiers="const">
@@ -558,7 +558,8 @@
 			<param index="2" name="end" type="Vector3" />
 			<param index="3" name="use_collision" type="bool" default="false" />
 			<description>
-				Returns the closest point between the navigation surface and the segment.
+				Returns the navigation mesh surface point closest to the provided [param start] to [param end] segment on the navigation [param map].
+				If [param use_collision] is [code]true[/code] a closest point test is only done when the segment intersects with the navigation mesh surface.
 			</description>
 		</method>
 		<method name="map_get_edge_connection_margin" qualifiers="const">
@@ -908,6 +909,33 @@
 				Creates a new region.
 			</description>
 		</method>
+		<method name="region_get_closest_point" qualifiers="const">
+			<return type="Vector3" />
+			<param index="0" name="region" type="RID" />
+			<param index="1" name="to_point" type="Vector3" />
+			<description>
+				Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param region].
+			</description>
+		</method>
+		<method name="region_get_closest_point_normal" qualifiers="const">
+			<return type="Vector3" />
+			<param index="0" name="region" type="RID" />
+			<param index="1" name="to_point" type="Vector3" />
+			<description>
+				Returns the navigation mesh surface normal closest to the provided [param to_point] on the navigation [param region].
+			</description>
+		</method>
+		<method name="region_get_closest_point_to_segment" qualifiers="const">
+			<return type="Vector3" />
+			<param index="0" name="region" type="RID" />
+			<param index="1" name="start" type="Vector3" />
+			<param index="2" name="end" type="Vector3" />
+			<param index="3" name="use_collision" type="bool" default="false" />
+			<description>
+				Returns the navigation mesh surface point closest to the provided [param start] to [param end] segment on the navigation [param region].
+				If [param use_collision] is [code]true[/code] a closest point test is only done when the segment intersects with the navigation mesh surface.
+			</description>
+		</method>
 		<method name="region_get_connection_pathway_end" qualifiers="const">
 			<return type="Vector3" />
 			<param index="0" name="region" type="RID" />

diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp
@@ -318,6 +318,11 @@ int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid);
 Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
 Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
 
+Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
+	Vector3 result = NavigationServer3D::get_singleton()->region_get_closest_point(p_region, v2_to_v3(p_point));
+	return v3_to_v2(result);
+}
+
 Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
 	Vector3 result = NavigationServer3D::get_singleton()->region_get_random_point(p_region, p_navigation_layers, p_uniformly);
 	return v3_to_v2(result);

diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h
@@ -101,6 +101,7 @@ class GodotNavigationServer2D : public NavigationServer2D {
 	virtual int region_get_connections_count(RID p_region) const override;
 	virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
 	virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
+	virtual Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const override;
 	virtual Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
 
 	virtual RID link_create() override;

diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp
@@ -536,6 +536,27 @@ Vector3 GodotNavigationServer3D::region_get_connection_pathway_end(RID p_region,
 	return Vector3();
 }
 
+Vector3 GodotNavigationServer3D::region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+	const NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector3());
+
+	return region->get_closest_point_to_segment(p_from, p_to, p_use_collision);
+}
+
+Vector3 GodotNavigationServer3D::region_get_closest_point(RID p_region, const Vector3 &p_point) const {
+	const NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector3());
+
+	return region->get_closest_point_info(p_point).point;
+}
+
+Vector3 GodotNavigationServer3D::region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const {
+	const NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector3());
+
+	return region->get_closest_point_info(p_point).normal;
+}
+
 Vector3 GodotNavigationServer3D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
 	const NavRegion *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_NULL_V(region, Vector3());

diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h
@@ -178,6 +178,9 @@ class GodotNavigationServer3D : public NavigationServer3D {
 	virtual int region_get_connections_count(RID p_region) const override;
 	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
 	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
+	virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
+	virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override;
+	virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override;
 	virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
 
 	virtual RID link_create() override;

diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
@@ -103,7 +103,98 @@ void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
 	polygons_dirty = true;
 }
 
+Vector3 NavRegion::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+	RWLockRead read_lock(region_rwlock);
+
+	bool use_collision = p_use_collision;
+	Vector3 closest_point;
+	real_t closest_point_d = FLT_MAX;
+
+	for (const gd::Polygon &p : get_polygons()) {
+		// For each face 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[0].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 = 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 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 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;
+				}
+			}
+		}
+		// 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;
+}
+
+gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
+	RWLockRead read_lock(region_rwlock);
+
+	gd::ClosestPointQueryResult result;
+	real_t closest_point_ds = FLT_MAX;
+
+	for (const gd::Polygon &p : get_polygons()) {
+		// For each face 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[0].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 ds = inters.distance_squared_to(p_point);
+			if (ds < closest_point_ds) {
+				result.point = inters;
+				result.normal = f.get_plane().normal;
+				result.owner = p.owner->get_self();
+				closest_point_ds = ds;
+			}
+		}
+	}
+
+	return result;
+}
+
 Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(region_rwlock);
+
 	if (!get_enabled()) {
 		return Vector3();
 	}
@@ -186,6 +277,8 @@ Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniform
 }
 
 bool NavRegion::sync() {
+	RWLockWrite write_lock(region_rwlock);
+
 	bool something_changed = polygons_dirty /* || something_dirty? */;
 
 	update_polygons();

diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
@@ -38,6 +38,8 @@
 #include "scene/resources/navigation_mesh.h"
 
 class NavRegion : public NavBase {
+	RWLock region_rwlock;
+
 	NavMap *map = nullptr;
 	Transform3D transform;
 	bool enabled = true;
@@ -88,6 +90,8 @@ class NavRegion : public NavBase {
 		return polygons;
 	}
 
+	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
+	gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
 	Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
 
 	real_t get_surface_area() const { return surface_area; };

diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp
@@ -86,6 +86,7 @@ void NavigationServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer2D::region_get_connections_count);
 	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_start);
 	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_end);
+	ClassDB::bind_method(D_METHOD("region_get_closest_point", "region", "to_point"), &NavigationServer2D::region_get_closest_point);
 	ClassDB::bind_method(D_METHOD("region_get_random_point", "region", "navigation_layers", "uniformly"), &NavigationServer2D::region_get_random_point);
 
 	ClassDB::bind_method(D_METHOD("link_create"), &NavigationServer2D::link_create);

diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h
@@ -148,7 +148,7 @@ class NavigationServer2D : public Object {
 	virtual int region_get_connections_count(RID p_region) const = 0;
 	virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const = 0;
 	virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const = 0;
-
+	virtual Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const = 0;
 	virtual Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const = 0;
 
 	/// Creates a new link between positions in the nav map.

diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h
@@ -83,6 +83,7 @@ class NavigationServer2DDummy : public NavigationServer2D {
 	int region_get_connections_count(RID p_region) const override { return 0; }
 	Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override { return Vector2(); }
 	Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override { return Vector2(); }
+	Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const override { return Vector2(); }
 	Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override { return Vector2(); };
 
 	RID link_create() override { return RID(); }

diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp
@@ -98,6 +98,9 @@ void NavigationServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer3D::region_get_connections_count);
 	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_start);
 	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_end);
+	ClassDB::bind_method(D_METHOD("region_get_closest_point_to_segment", "region", "start", "end", "use_collision"), &NavigationServer3D::region_get_closest_point_to_segment, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("region_get_closest_point", "region", "to_point"), &NavigationServer3D::region_get_closest_point);
+	ClassDB::bind_method(D_METHOD("region_get_closest_point_normal", "region", "to_point"), &NavigationServer3D::region_get_closest_point_normal);
 	ClassDB::bind_method(D_METHOD("region_get_random_point", "region", "navigation_layers", "uniformly"), &NavigationServer3D::region_get_random_point);
 
 	ClassDB::bind_method(D_METHOD("link_create"), &NavigationServer3D::link_create);

diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h
@@ -167,7 +167,9 @@ class NavigationServer3D : public Object {
 	virtual int region_get_connections_count(RID p_region) const = 0;
 	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const = 0;
 	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const = 0;
-
+	virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const = 0;
+	virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const = 0;
+	virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const = 0;
 	virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const = 0;
 
 	/// Creates a new link between positions in the nav map.

diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h
@@ -93,6 +93,9 @@ class NavigationServer3DDummy : public NavigationServer3D {
 	int region_get_connections_count(RID p_region) const override { return 0; }
 	Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override { return Vector3(); }
 	Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override { return Vector3(); }
+	Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const override { return Vector3(); }
+	Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override { return Vector3(); }
+	Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override { return Vector3(); }
 	Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override { return Vector3(); }
 
 	RID link_create() override { return RID(); }