From 287fdb16d53c38f6766f773bebbea33c5270b8f3 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sun, 25 Aug 2024 22:59:13 +0200 Subject: [PATCH] Add navigation region point and segment queries Adds point and segment queries for regions, e.g. closet point, point normal, or segment intersection. --- doc/classes/NavigationServer2D.xml | 12 +++++-- doc/classes/NavigationServer3D.xml | 36 ++++++++++++++++--- .../2d/godot_navigation_server_2d.cpp | 5 +++ .../2d/godot_navigation_server_2d.h | 1 + .../3d/godot_navigation_server_3d.cpp | 21 +++++++++++ .../3d/godot_navigation_server_3d.h | 3 ++ modules/navigation/nav_region.cpp | 17 +++++++++ modules/navigation/nav_region.h | 4 +++ servers/navigation_server_2d.cpp | 1 + servers/navigation_server_2d.h | 1 + servers/navigation_server_2d_dummy.h | 1 + servers/navigation_server_3d.cpp | 3 ++ servers/navigation_server_3d.h | 3 ++ servers/navigation_server_3d_dummy.h | 3 ++ 14 files changed, 105 insertions(+), 6 deletions(-) diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index a0d03d7a012..7e780062404 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -486,7 +486,7 @@ - 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]. @@ -494,7 +494,7 @@ - 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]. @@ -768,6 +768,14 @@ Creates a new region. + + + + + + Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param region]. + + diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index 42f6235f8b6..7e206046d68 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -532,7 +532,7 @@ - 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]. @@ -540,7 +540,7 @@ - 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]. @@ -548,7 +548,7 @@ - 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]. @@ -558,7 +558,8 @@ - Returns the closest point between the navigation surface and the segment. + Returns the navigation mesh surface point closest to the provided [param start] and [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. @@ -908,6 +909,33 @@ Creates a new region. + + + + + + Returns the navigation mesh surface point closest to the provided [param to_point] on the navigation [param region]. + + + + + + + + Returns the navigation mesh surface normal closest to the provided [param to_point] on the navigation [param region]. + + + + + + + + + + Returns the navigation mesh surface point closest to the provided [param start] and [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. + + diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index bf69adc14c9..2af125d4348 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 ea77fa5e6ec..1579ca29074 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 11a5de608b0..5dfc39f6f5e 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 12a1132f07b..eae6ea28608 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 7a44adecbc3..2c91b80af2a 100644 --- a/modules/navigation/nav_region.cpp +++ b/modules/navigation/nav_region.cpp @@ -105,7 +105,22 @@ void NavRegion::set_navigation_mesh(Ref 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 662a32c47a4..c015802b921 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; }; diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index c5ce82265b7..ceb5e909a3b 100644 --- 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 index a8d9678a6f0..250183300f1 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -149,6 +149,7 @@ public: 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 index 465cfcca988..0664b37ef0c 100644 --- a/servers/navigation_server_2d_dummy.h +++ b/servers/navigation_server_2d_dummy.h @@ -83,6 +83,7 @@ public: 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 index f4ffcf5a3e6..572309c4296 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -99,6 +99,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 index cdacf8e7e45..6dbbd356481 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -168,6 +168,9 @@ public: 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, 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 index 5c9e97d2265..210c404365a 100644 --- a/servers/navigation_server_3d_dummy.h +++ b/servers/navigation_server_3d_dummy.h @@ -93,6 +93,9 @@ public: 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, bool p_use_collision = false) 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(); }