diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml index 1f9c064f930..29a6d835cd4 100644 --- a/doc/classes/NavigationPathQueryParameters2D.xml +++ b/doc/classes/NavigationPathQueryParameters2D.xml @@ -10,6 +10,14 @@ $DOCS_URL/tutorials/navigation/navigation_using_navigationpathqueryobjects.html + + The list of region [RID]s that will be excluded from the path query. Use [method NavigationRegion2D.get_rid] to get the [RID] associated with a [NavigationRegion2D] node. + [b]Note:[/b] The returned array is copied and any changes to it will not update the original property value. To update the value you need to modify the returned array, and then set it to the property again. + + + The list of region [RID]s that will be included by the path query. Use [method NavigationRegion2D.get_rid] to get the [RID] associated with a [NavigationRegion2D] node. If left empty all regions are included. If a region ends up being both included and excluded at the same time it will be excluded. + [b]Note:[/b] The returned array is copied and any changes to it will not update the original property value. To update the value you need to modify the returned array, and then set it to the property again. + The navigation map [RID] used in the path query. diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml index a4c622d080d..a9b4794886e 100644 --- a/doc/classes/NavigationPathQueryParameters3D.xml +++ b/doc/classes/NavigationPathQueryParameters3D.xml @@ -10,6 +10,14 @@ $DOCS_URL/tutorials/navigation/navigation_using_navigationpathqueryobjects.html + + The list of region [RID]s that will be excluded from the path query. Use [method NavigationRegion3D.get_rid] to get the [RID] associated with a [NavigationRegion3D] node. + [b]Note:[/b] The returned array is copied and any changes to it will not update the original property value. To update the value you need to modify the returned array, and then set it to the property again. + + + The list of region [RID]s that will be included by the path query. Use [method NavigationRegion3D.get_rid] to get the [RID] associated with a [NavigationRegion3D] node. If left empty all regions are included. If a region ends up being both included and excluded at the same time it will be excluded. + [b]Note:[/b] The returned array is copied and any changes to it will not update the original property value. To update the value you need to modify the returned array, and then set it to the property again. + The navigation map [RID] used in the path query. diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index 5a9dccdd374..5fda2993039 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -513,6 +513,8 @@ void GodotNavigationServer2D::query_path(const Refset_metadata_flags((int64_t)p_query_parameters->get_metadata_flags()); query_parameters->set_simplify_path(p_query_parameters->get_simplify_path()); query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon()); + query_parameters->set_excluded_regions(p_query_parameters->get_excluded_regions()); + query_parameters->set_included_regions(p_query_parameters->get_included_regions()); Ref query_result; query_result.instantiate(); diff --git a/modules/navigation/3d/nav_base_iteration_3d.h b/modules/navigation/3d/nav_base_iteration_3d.h index d289c5d533a..d77e4c4811e 100644 --- a/modules/navigation/3d/nav_base_iteration_3d.h +++ b/modules/navigation/3d/nav_base_iteration_3d.h @@ -31,6 +31,8 @@ #ifndef NAV_BASE_ITERATION_3D_H #define NAV_BASE_ITERATION_3D_H +#include "../nav_utils.h" + #include "servers/navigation/navigation_utilities.h" struct NavBaseIteration { @@ -43,6 +45,7 @@ struct NavBaseIteration { ObjectID owner_object_id; RID owner_rid; bool owner_use_edge_connections = false; + LocalVector navmesh_polygons; bool get_enabled() const { return enabled; } NavigationUtilities::PathSegmentType get_type() const { return owner_type; } @@ -52,6 +55,7 @@ struct NavBaseIteration { real_t get_enter_cost() const { return enter_cost; } real_t get_travel_cost() const { return travel_cost; } bool get_use_edge_connections() const { return owner_use_edge_connections; } + const LocalVector &get_navmesh_polygons() const { return navmesh_polygons; } }; #endif // NAV_BASE_ITERATION_3D_H diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp index b4ad057b01f..13f77ef8eee 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -158,6 +158,29 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Refget_navigation_layers(); query_task.callback = p_callback; + const TypedArray &_excluded_regions = p_query_parameters->get_excluded_regions(); + const TypedArray &_included_regions = p_query_parameters->get_included_regions(); + + uint32_t _excluded_region_count = _excluded_regions.size(); + uint32_t _included_region_count = _included_regions.size(); + + query_task.exclude_regions = _excluded_region_count > 0; + query_task.include_regions = _included_region_count > 0; + + if (query_task.exclude_regions) { + query_task.excluded_regions.resize(_excluded_region_count); + for (uint32_t i = 0; i < _excluded_region_count; i++) { + query_task.excluded_regions[i] = _excluded_regions[i]; + } + } + + if (query_task.include_regions) { + query_task.included_regions.resize(_included_region_count); + for (uint32_t i = 0; i < _included_region_count; i++) { + query_task.included_regions[i] = _included_regions[i]; + } + } + switch (p_query_parameters->get_pathfinding_algorithm()) { case NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: { query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; @@ -217,6 +240,13 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask continue; } + if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) { + continue; + } + if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) { + continue; + } + // Find the initial poly and the end poly on this map. for (const gd::Polygon &p : region.get_navmesh_polygons()) { // Only consider the polygon if it in a region with compatible layers. @@ -295,6 +325,41 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p // Only consider the connection to another polygon if this polygon is in a region with compatible layers. const NavBaseIteration *owner = connection.polygon->owner; + bool skip_connection = false; + if (p_query_task.exclude_regions || p_query_task.include_regions) { + switch (owner->get_type()) { + case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: { + if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(owner->get_self())) { + skip_connection = true; + } else if (p_query_task.include_regions && !p_query_task.included_regions.has(owner->get_self())) { + skip_connection = true; + } + } break; + case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: { + const LocalVector &link_polygons = owner->get_navmesh_polygons(); + if (link_polygons.size() != 2) { + // Whatever this is, it is not a valid connected link. + skip_connection = true; + } else { + const RID link_start_region = link_polygons[0].owner->get_self(); + const RID link_end_region = link_polygons[1].owner->get_self(); + if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) { + // At least one region of the link is excluded so skip. + skip_connection = true; + } + if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) { + // Not both regions of the link are included so skip. + skip_connection = true; + } + } + } break; + } + } + + if (skip_connection) { + continue; + } + if ((p_navigation_layers & owner->get_navigation_layers()) != 0) { Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h index 3ef97c87860..0c3494ef654 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -71,6 +71,10 @@ public: PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; bool simplify_path = false; real_t simplify_epsilon = 0.0; + bool exclude_regions = false; + bool include_regions = false; + LocalVector excluded_regions; + LocalVector included_regions; // Path building. Vector3 begin_position; diff --git a/modules/navigation/3d/nav_region_iteration_3d.h b/modules/navigation/3d/nav_region_iteration_3d.h index cee1a2d5775..57d32c18571 100644 --- a/modules/navigation/3d/nav_region_iteration_3d.h +++ b/modules/navigation/3d/nav_region_iteration_3d.h @@ -38,12 +38,10 @@ struct NavRegionIteration : NavBaseIteration { Transform3D transform; - LocalVector navmesh_polygons; real_t surface_area = 0.0; AABB bounds; const Transform3D &get_transform() const { return transform; } - const LocalVector &get_navmesh_polygons() const { return navmesh_polygons; } real_t get_surface_area() const { return surface_area; } AABB get_bounds() const { return bounds; } }; diff --git a/modules/navigation/nav_link.h b/modules/navigation/nav_link.h index e06a10d5bf3..a89881d31ae 100644 --- a/modules/navigation/nav_link.h +++ b/modules/navigation/nav_link.h @@ -39,7 +39,6 @@ struct NavLinkIteration : NavBaseIteration { bool bidirectional = true; Vector3 start_position; Vector3 end_position; - LocalVector navmesh_polygons; Vector3 get_start_position() const { return start_position; } Vector3 get_end_position() const { return end_position; } diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp index 74aaf64b4e0..2e57602dc69 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.cpp +++ b/servers/navigation/navigation_path_query_parameters_2d.cpp @@ -102,6 +102,38 @@ real_t NavigationPathQueryParameters2D::get_simplify_epsilon() const { return simplify_epsilon; } +void NavigationPathQueryParameters2D::set_included_regions(const TypedArray &p_regions) { + _included_regions.resize(p_regions.size()); + for (uint32_t i = 0; i < _included_regions.size(); i++) { + _included_regions[i] = p_regions[i]; + } +} + +TypedArray NavigationPathQueryParameters2D::get_included_regions() const { + TypedArray r_regions; + r_regions.resize(_included_regions.size()); + for (uint32_t i = 0; i < _included_regions.size(); i++) { + r_regions[i] = _included_regions[i]; + } + return r_regions; +} + +void NavigationPathQueryParameters2D::set_excluded_regions(const TypedArray &p_regions) { + _excluded_regions.resize(p_regions.size()); + for (uint32_t i = 0; i < _excluded_regions.size(); i++) { + _excluded_regions[i] = p_regions[i]; + } +} + +TypedArray NavigationPathQueryParameters2D::get_excluded_regions() const { + TypedArray r_regions; + r_regions.resize(_excluded_regions.size()); + for (uint32_t i = 0; i < _excluded_regions.size(); i++) { + r_regions[i] = _excluded_regions[i]; + } + return r_regions; +} + void NavigationPathQueryParameters2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_pathfinding_algorithm", "pathfinding_algorithm"), &NavigationPathQueryParameters2D::set_pathfinding_algorithm); ClassDB::bind_method(D_METHOD("get_pathfinding_algorithm"), &NavigationPathQueryParameters2D::get_pathfinding_algorithm); @@ -130,6 +162,12 @@ void NavigationPathQueryParameters2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationPathQueryParameters2D::set_simplify_epsilon); ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationPathQueryParameters2D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("set_included_regions", "regions"), &NavigationPathQueryParameters2D::set_included_regions); + ClassDB::bind_method(D_METHOD("get_included_regions"), &NavigationPathQueryParameters2D::get_included_regions); + + ClassDB::bind_method(D_METHOD("set_excluded_regions", "regions"), &NavigationPathQueryParameters2D::set_excluded_regions); + ClassDB::bind_method(D_METHOD("get_excluded_regions"), &NavigationPathQueryParameters2D::get_excluded_regions); + ADD_PROPERTY(PropertyInfo(Variant::RID, "map"), "set_map", "get_map"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "start_position"), "set_start_position", "get_start_position"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "target_position"), "set_target_position", "get_target_position"); @@ -139,6 +177,8 @@ void NavigationPathQueryParameters2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_metadata_flags", "get_metadata_flags"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon"), "set_simplify_epsilon", "get_simplify_epsilon"); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "excluded_regions", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_excluded_regions", "get_excluded_regions"); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "included_regions", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_included_regions", "get_included_regions"); BIND_ENUM_CONSTANT(PATHFINDING_ALGORITHM_ASTAR); diff --git a/servers/navigation/navigation_path_query_parameters_2d.h b/servers/navigation/navigation_path_query_parameters_2d.h index 91031bfc180..3a1120bd941 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.h +++ b/servers/navigation/navigation_path_query_parameters_2d.h @@ -69,6 +69,8 @@ private: BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; bool simplify_path = false; real_t simplify_epsilon = 0.0; + LocalVector _excluded_regions; + LocalVector _included_regions; public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); @@ -97,6 +99,12 @@ public: void set_simplify_epsilon(real_t p_epsilon); real_t get_simplify_epsilon() const; + + void set_excluded_regions(const TypedArray &p_regions); + TypedArray get_excluded_regions() const; + + void set_included_regions(const TypedArray &p_regions); + TypedArray get_included_regions() const; }; VARIANT_ENUM_CAST(NavigationPathQueryParameters2D::PathfindingAlgorithm); diff --git a/servers/navigation/navigation_path_query_parameters_3d.cpp b/servers/navigation/navigation_path_query_parameters_3d.cpp index 99c5318bed2..39c41efeb3d 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.cpp +++ b/servers/navigation/navigation_path_query_parameters_3d.cpp @@ -102,6 +102,38 @@ real_t NavigationPathQueryParameters3D::get_simplify_epsilon() const { return simplify_epsilon; } +void NavigationPathQueryParameters3D::set_included_regions(const TypedArray &p_regions) { + _included_regions.resize(p_regions.size()); + for (uint32_t i = 0; i < _included_regions.size(); i++) { + _included_regions[i] = p_regions[i]; + } +} + +TypedArray NavigationPathQueryParameters3D::get_included_regions() const { + TypedArray r_regions; + r_regions.resize(_included_regions.size()); + for (uint32_t i = 0; i < _included_regions.size(); i++) { + r_regions[i] = _included_regions[i]; + } + return r_regions; +} + +void NavigationPathQueryParameters3D::set_excluded_regions(const TypedArray &p_regions) { + _excluded_regions.resize(p_regions.size()); + for (uint32_t i = 0; i < _excluded_regions.size(); i++) { + _excluded_regions[i] = p_regions[i]; + } +} + +TypedArray NavigationPathQueryParameters3D::get_excluded_regions() const { + TypedArray r_regions; + r_regions.resize(_excluded_regions.size()); + for (uint32_t i = 0; i < _excluded_regions.size(); i++) { + r_regions[i] = _excluded_regions[i]; + } + return r_regions; +} + void NavigationPathQueryParameters3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_pathfinding_algorithm", "pathfinding_algorithm"), &NavigationPathQueryParameters3D::set_pathfinding_algorithm); ClassDB::bind_method(D_METHOD("get_pathfinding_algorithm"), &NavigationPathQueryParameters3D::get_pathfinding_algorithm); @@ -130,6 +162,12 @@ void NavigationPathQueryParameters3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationPathQueryParameters3D::set_simplify_epsilon); ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationPathQueryParameters3D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("set_included_regions", "regions"), &NavigationPathQueryParameters3D::set_included_regions); + ClassDB::bind_method(D_METHOD("get_included_regions"), &NavigationPathQueryParameters3D::get_included_regions); + + ClassDB::bind_method(D_METHOD("set_excluded_regions", "regions"), &NavigationPathQueryParameters3D::set_excluded_regions); + ClassDB::bind_method(D_METHOD("get_excluded_regions"), &NavigationPathQueryParameters3D::get_excluded_regions); + ADD_PROPERTY(PropertyInfo(Variant::RID, "map"), "set_map", "get_map"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "start_position"), "set_start_position", "get_start_position"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position"), "set_target_position", "get_target_position"); @@ -139,6 +177,8 @@ void NavigationPathQueryParameters3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_metadata_flags", "get_metadata_flags"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon"), "set_simplify_epsilon", "get_simplify_epsilon"); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "excluded_regions", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_excluded_regions", "get_excluded_regions"); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "included_regions", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_included_regions", "get_included_regions"); BIND_ENUM_CONSTANT(PATHFINDING_ALGORITHM_ASTAR); diff --git a/servers/navigation/navigation_path_query_parameters_3d.h b/servers/navigation/navigation_path_query_parameters_3d.h index 66cad8dcd54..e9ee8048a86 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.h +++ b/servers/navigation/navigation_path_query_parameters_3d.h @@ -69,6 +69,8 @@ private: BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; bool simplify_path = false; real_t simplify_epsilon = 0.0; + LocalVector _excluded_regions; + LocalVector _included_regions; public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); @@ -97,6 +99,12 @@ public: void set_simplify_epsilon(real_t p_epsilon); real_t get_simplify_epsilon() const; + + void set_excluded_regions(const TypedArray &p_regions); + TypedArray get_excluded_regions() const; + + void set_included_regions(const TypedArray &p_regions); + TypedArray get_included_regions() const; }; VARIANT_ENUM_CAST(NavigationPathQueryParameters3D::PathfindingAlgorithm); diff --git a/tests/servers/test_navigation_server_3d.h b/tests/servers/test_navigation_server_3d.h index d45e7558638..d3a30b7ceb9 100644 --- a/tests/servers/test_navigation_server_3d.h +++ b/tests/servers/test_navigation_server_3d.h @@ -794,6 +794,54 @@ TEST_SUITE("[Navigation]") { CHECK_EQ(query_result->get_path_owner_ids().size(), 0); } + SUBCASE("Elaborate query with excluded region should yield empty path") { + Ref query_parameters; + query_parameters.instantiate(); + query_parameters->set_map(map); + query_parameters->set_start_position(Vector3(10, 0, 10)); + query_parameters->set_target_position(Vector3(0, 0, 0)); + Array excluded_regions; + excluded_regions.push_back(region); + query_parameters->set_excluded_regions(excluded_regions); + Ref query_result; + query_result.instantiate(); + navigation_server->query_path(query_parameters, query_result); + CHECK_EQ(query_result->get_path().size(), 0); + } + + SUBCASE("Elaborate query with included region should yield path") { + Ref query_parameters; + query_parameters.instantiate(); + query_parameters->set_map(map); + query_parameters->set_start_position(Vector3(10, 0, 10)); + query_parameters->set_target_position(Vector3(0, 0, 0)); + Array included_regions; + included_regions.push_back(region); + query_parameters->set_included_regions(included_regions); + Ref query_result; + query_result.instantiate(); + navigation_server->query_path(query_parameters, query_result); + CHECK_NE(query_result->get_path().size(), 0); + } + + SUBCASE("Elaborate query with excluded and included region should yield empty path") { + Ref query_parameters; + query_parameters.instantiate(); + query_parameters->set_map(map); + query_parameters->set_start_position(Vector3(10, 0, 10)); + query_parameters->set_target_position(Vector3(0, 0, 0)); + Array excluded_regions; + excluded_regions.push_back(region); + query_parameters->set_excluded_regions(excluded_regions); + Array included_regions; + included_regions.push_back(region); + query_parameters->set_included_regions(included_regions); + Ref query_result; + query_result.instantiate(); + navigation_server->query_path(query_parameters, query_result); + CHECK_EQ(query_result->get_path().size(), 0); + } + navigation_server->free(region); navigation_server->free(map); navigation_server->process(0.0); // Give server some cycles to commit.