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 d8d1d21b6c6..3876d520797 100644 --- a/modules/navigation/3d/nav_base_iteration_3d.h +++ b/modules/navigation/3d/nav_base_iteration_3d.h @@ -30,6 +30,8 @@ #pragma once +#include "../nav_utils.h" + #include "servers/navigation/navigation_utilities.h" struct NavBaseIteration { @@ -42,6 +44,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; } @@ -51,4 +54,5 @@ 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; } }; 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 b296bd0d211..78d4f822700 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -70,6 +70,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 5123c23f226..258248565ab 100644 --- a/modules/navigation/3d/nav_region_iteration_3d.h +++ b/modules/navigation/3d/nav_region_iteration_3d.h @@ -37,12 +37,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 9573f7f1461..bdce6008c1d 100644 --- a/modules/navigation/nav_link.h +++ b/modules/navigation/nav_link.h @@ -38,7 +38,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 a0e56c88edf..3a910e22cd8 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.h +++ b/servers/navigation/navigation_path_query_parameters_2d.h @@ -68,6 +68,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); @@ -96,6 +98,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 97a6fcb3e4f..38b743e313f 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.h +++ b/servers/navigation/navigation_path_query_parameters_3d.h @@ -68,6 +68,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); @@ -96,6 +98,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 4e2758f1148..aa01f227dc4 100644 --- a/tests/servers/test_navigation_server_3d.h +++ b/tests/servers/test_navigation_server_3d.h @@ -793,6 +793,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.