Add navigation region point and segment queries

Adds point and segment queries for regions, e.g. closet point, point normal, or segment intersection.

6 files changed, 51 insertions, 0 deletions

diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp
index bf69adc14c..2af125d434 100644
--- 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
index ea77fa5e6e..1579ca2907 100644
--- a/modules/navigation/2d/godot_navigation_server_2d.h
+++ b/modules/navigation/2d/godot_navigation_server_2d.h
@@ -101,6 +101,7 @@ public:
 	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
index 11a5de608b..5dfc39f6f5 100644
--- 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, 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
index 12a1132f07..eae6ea2860 100644
--- a/modules/navigation/3d/godot_navigation_server_3d.h
+++ b/modules/navigation/3d/godot_navigation_server_3d.h
@@ -178,6 +178,9 @@ public:
 	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, 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
index 7a44adecbc..2c91b80af2 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -105,7 +105,22 @@ 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, bool p_use_collision) const {
+	RWLockRead read_lock(region_rwlock);
+
+	return NavMeshQueries3D::polygons_get_closest_point_to_segment(
+			get_polygons(), p_from, p_to, p_use_collision);
+}
+
+gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
+	RWLockRead read_lock(region_rwlock);
+
+	return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
+}
+
 Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(region_rwlock);
+
 	if (!get_enabled()) {
 		return Vector3();
 	}
@@ -114,6 +129,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
index 662a32c47a..c015802b92 100644
--- 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 @@ public:
 		return polygons;
 	}
 
+	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, 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; };