From cbd446ac29957e3b72283526e3d012bd00ab51ae Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sat, 8 Feb 2025 22:08:04 +0100 Subject: [PATCH] Add navigation path query parameter limits Adds navigation path query parameter limits. --- doc/classes/NavigationAgent2D.xml | 19 +++ doc/classes/NavigationAgent3D.xml | 19 +++ .../NavigationPathQueryParameters2D.xml | 13 ++ .../NavigationPathQueryParameters3D.xml | 13 ++ doc/classes/NavigationPathQueryResult2D.xml | 3 + doc/classes/NavigationPathQueryResult3D.xml | 3 + .../navigation_2d/2d/nav_mesh_queries_2d.cpp | 143 ++++++++++++++++++ .../navigation_2d/2d/nav_mesh_queries_2d.h | 11 ++ .../navigation_3d/3d/nav_mesh_queries_3d.cpp | 143 ++++++++++++++++++ .../navigation_3d/3d/nav_mesh_queries_3d.h | 11 ++ scene/2d/navigation/navigation_agent_2d.cpp | 58 +++++++ scene/2d/navigation/navigation_agent_2d.h | 18 +++ scene/3d/navigation/navigation_agent_3d.cpp | 58 +++++++ scene/3d/navigation/navigation_agent_3d.h | 18 +++ servers/navigation/navigation_globals.h | 2 + .../navigation_path_query_parameters_2d.cpp | 48 ++++++ .../navigation_path_query_parameters_2d.h | 19 +++ .../navigation_path_query_parameters_3d.cpp | 48 ++++++ .../navigation_path_query_parameters_3d.h | 19 +++ .../navigation_path_query_result_2d.cpp | 12 ++ .../navigation_path_query_result_2d.h | 4 + .../navigation_path_query_result_3d.cpp | 12 ++ .../navigation_path_query_result_3d.h | 4 + 23 files changed, 698 insertions(+) diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml index a53a363694a..83472b1cc9e 100644 --- a/doc/classes/NavigationAgent2D.xml +++ b/doc/classes/NavigationAgent2D.xml @@ -75,6 +75,12 @@ Returns the next position in global coordinates that can be moved to, making sure that there are no static objects in the way. If the agent does not have a navigation path, it will return the position of the agent's parent. The use of this function once every physics frame is required to update the internal path logic of the NavigationAgent. + + + + Returns the length of the currently calculated path. The returned value is [code]0.0[/code], if the path is still calculating or no calculation has been requested yet. + + @@ -191,6 +197,19 @@ The path postprocessing applied to the raw path corridor found by the [member pathfinding_algorithm]. + + The maximum allowed length of the returned path in world units. A path will be clipped when going over this length. + + + The maximum allowed radius in world units that the returned path can be from the path start. The path will be clipped when going over this radius. Compared to [member path_return_max_length], this allows the agent to go that much further, if they need to walk around a corner. + [b]Note:[/b] This will perform a sphere clip considering only the actual navigation mesh path points with the first path position being the sphere's center. + + + The maximum distance a searched polygon can be away from the start polygon before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + + + The maximum number of polygons that are searched before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + The pathfinding algorithm used in the path query. diff --git a/doc/classes/NavigationAgent3D.xml b/doc/classes/NavigationAgent3D.xml index 5f9f4991d13..3eaecc13082 100644 --- a/doc/classes/NavigationAgent3D.xml +++ b/doc/classes/NavigationAgent3D.xml @@ -75,6 +75,12 @@ Returns the next position in global coordinates that can be moved to, making sure that there are no static objects in the way. If the agent does not have a navigation path, it will return the position of the agent's parent. The use of this function once every physics frame is required to update the internal path logic of the NavigationAgent. + + + + Returns the length of the currently calculated path. The returned value is [code]0.0[/code], if the path is still calculating or no calculation has been requested yet. + + @@ -197,6 +203,19 @@ The path postprocessing applied to the raw path corridor found by the [member pathfinding_algorithm]. + + The maximum allowed length of the returned path in world units. A path will be clipped when going over this length. + + + The maximum allowed radius in world units that the returned path can be from the path start. The path will be clipped when going over this radius. Compared to [member path_return_max_length], this allows the agent to go that much further, if they need to walk around a corner. + [b]Note:[/b] This will perform a sphere clip considering only the actual navigation mesh path points with the first path position being the sphere's center. + + + The maximum distance a searched polygon can be away from the start polygon before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + + + The maximum number of polygons that are searched before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + The pathfinding algorithm used in the path query. diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml index 29a6d835cd4..15b94f0d80a 100644 --- a/doc/classes/NavigationPathQueryParameters2D.xml +++ b/doc/classes/NavigationPathQueryParameters2D.xml @@ -30,6 +30,19 @@ The path postprocessing applied to the raw path corridor found by the [member pathfinding_algorithm]. + + The maximum allowed length of the returned path in world units. A path will be clipped when going over this length. A value of [code]0[/code] or below counts as disabled. + + + The maximum allowed radius in world units that the returned path can be from the path start. The path will be clipped when going over this radius. A value of [code]0[/code] or below counts as disabled. + [b]Note:[/b] This will perform a circle shaped clip operation on the path with the first path position being the circle's center position. + + + The maximum distance a searched polygon can be away from the start polygon before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + + + The maximum number of polygons that are searched before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + The pathfinding algorithm used in the path query. diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml index a9b4794886e..b5fef6e4cb7 100644 --- a/doc/classes/NavigationPathQueryParameters3D.xml +++ b/doc/classes/NavigationPathQueryParameters3D.xml @@ -30,6 +30,19 @@ The path postprocessing applied to the raw path corridor found by the [member pathfinding_algorithm]. + + The maximum allowed length of the returned path in world units. A path will be clipped when going over this length. A value of [code]0[/code] or below counts as disabled. + + + The maximum allowed radius in world units that the returned path can be from the path start. The path will be clipped when going over this radius. A value of [code]0[/code] or below counts as disabled. + [b]Note:[/b] This will perform a sphere shaped clip operation on the path with the first path position being the sphere's center position. + + + The maximum distance a searched polygon can be away from the start polygon before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + + + The maximum number of polygons that are searched before the pathfinding cancels the search for a path to the (possibly unreachable or very far away) target position polygon. In this case the pathfinding resets and builds a path from the start polygon to the polygon that was found closest to the target position so far. A value of [code]0[/code] or below counts as unlimited. In case of unlimited the pathfinding will search all polygons connected with the start polygon until either the target position polygon is found or all available polygon search options are exhausted. + The pathfinding algorithm used in the path query. diff --git a/doc/classes/NavigationPathQueryResult2D.xml b/doc/classes/NavigationPathQueryResult2D.xml index e1ef2ee7de7..94d6b6e3e07 100644 --- a/doc/classes/NavigationPathQueryResult2D.xml +++ b/doc/classes/NavigationPathQueryResult2D.xml @@ -21,6 +21,9 @@ The resulting path array from the navigation query. All path array positions are in global coordinates. Without customized query parameters this is the same path as returned by [method NavigationServer2D.map_get_path]. + + Returns the length of the path. + The [code]ObjectID[/code]s of the [Object]s which manage the regions and links each point of the path goes through. diff --git a/doc/classes/NavigationPathQueryResult3D.xml b/doc/classes/NavigationPathQueryResult3D.xml index 1cda3e64d59..84a9f7d1d66 100644 --- a/doc/classes/NavigationPathQueryResult3D.xml +++ b/doc/classes/NavigationPathQueryResult3D.xml @@ -21,6 +21,9 @@ The resulting path array from the navigation query. All path array positions are in global coordinates. Without customized query parameters this is the same path as returned by [method NavigationServer3D.map_get_path]. + + Returns the length of the path. + The [code]ObjectID[/code]s of the [Object]s which manage the regions and links each point of the path goes through. diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp index e686482cec8..91cba997de9 100644 --- a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp +++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp @@ -211,6 +211,10 @@ void NavMeshQueries2D::map_query_path(NavMap2D *p_map, const Refget_metadata_flags(); query_task.simplify_path = p_query_parameters->get_simplify_path(); query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon(); + query_task.path_return_max_length = p_query_parameters->get_path_return_max_length(); + query_task.path_return_max_radius = p_query_parameters->get_path_return_max_radius(); + query_task.path_search_max_polygons = p_query_parameters->get_path_search_max_polygons(); + query_task.path_search_max_distance = p_query_parameters->get_path_search_max_distance(); query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED; p_map->query_path(query_task); @@ -220,6 +224,7 @@ void NavMeshQueries2D::map_query_path(NavMap2D *p_map, const Refset_path_length(query_task.path_length); if (query_task.callback.is_valid()) { if (emit_callback(query_task.callback)) { @@ -348,11 +353,24 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p const HashMap>> &navbases_polygons_external_connections = p_map_iteration.navbases_polygons_external_connections; + // True if we reached the max polygon search count or distance from the begin position. + bool path_search_max_reached = false; + + const float path_search_max_distance_sqr = p_query_task.path_search_max_distance * p_query_task.path_search_max_distance; + bool has_path_search_max_distance = path_search_max_distance_sqr > 0.0; + + int processed_polygon_count = 0; + bool has_path_search_max_polygons = p_query_task.path_search_max_polygons > 0; + + bool has_path_search_max = p_query_task.path_search_max_polygons > 0 || path_search_max_distance_sqr > 0.0; + while (true) { const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; const NavBaseIteration2D *least_cost_navbase = least_cost_poly.poly->owner; + processed_polygon_count += 1; + const uint32_t navbase_local_polygon_id = least_cost_poly.poly->id; const LocalVector> &navbase_polygons_to_connections = least_cost_poly.poly->owner->get_internal_connections(); @@ -369,6 +387,16 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p _query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point); } + if (has_path_search_max && !path_search_max_reached) { + if (has_path_search_max_polygons && processed_polygon_count >= p_query_task.path_search_max_polygons) { + path_search_max_reached = true; + traversable_polys.clear(); + } else if (has_path_search_max_distance && begin_point.distance_squared_to(least_cost_poly.entry) > path_search_max_distance_sqr) { + path_search_max_reached = true; + traversable_polys.clear(); + } + } + poly_enter_cost = 0; // When the heap of traversable polygons is empty at this point it means the end polygon is // unreachable. @@ -498,6 +526,7 @@ void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D p_query_task.path_clear(); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon); + _query_task_process_path_result_limits(p_query_task); p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; return; } @@ -505,6 +534,7 @@ void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D _query_task_build_path_corridor(p_query_task, p_map_iteration); if (p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FAILED) { + _query_task_process_path_result_limits(p_query_task); return; } @@ -531,6 +561,8 @@ void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D _query_task_simplified_path_points(p_query_task); } + _query_task_process_path_result_limits(p_query_task); + #ifdef DEBUG_ENABLED // Ensure post conditions as path meta arrays if used MUST match in array size with the path points. if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { @@ -549,6 +581,117 @@ void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; } +float NavMeshQueries2D::_calculate_path_length(const LocalVector &p_path, uint32_t p_start_index, uint32_t p_end_index) { + const uint32_t path_size = p_path.size(); + if (path_size < 2) { + return 0.0; + } + + ERR_FAIL_COND_V(p_start_index >= p_end_index, 0.0); + ERR_FAIL_COND_V(p_start_index >= path_size - 1, 0.0); + ERR_FAIL_COND_V(p_end_index >= path_size, 0.0); + + const Vector2 *path_ptr = p_path.ptr(); + + float path_length = 0.0; + + for (uint32_t i = p_start_index; i < p_end_index; i++) { + const Vector2 &vertex1 = path_ptr[i]; + const Vector2 &vertex2 = path_ptr[i + 1]; + float edge_length = vertex1.distance_to(vertex2); + path_length += edge_length; + } + + return path_length; +} + +void NavMeshQueries2D::_query_task_process_path_result_limits(NavMeshPathQueryTask2D &p_query_task) { + if (p_query_task.path_points.size() < 2) { + return; + } + + bool check_max_length = p_query_task.path_return_max_length > 0.0; + bool check_max_radius = p_query_task.path_return_max_radius > 0.0; + + if (!check_max_length && !check_max_radius) { + p_query_task.path_length = _calculate_path_length(p_query_task.path_points, 0, p_query_task.path_points.size() - 1); + return; + } + + LocalVector &path = p_query_task.path_points; + + const float max_length = p_query_task.path_return_max_length; + const float max_radius = p_query_task.path_return_max_radius; + const float max_radius_sqr = max_radius * max_radius; + + const Vector2 &start_pos = path[0]; + + float accumulated_path_length = 0.0; + + Vector2 *path_ptrw = path.ptr(); + + uint32_t path_max_size = path.size(); + bool path_max_reached = false; + + for (uint32_t i = 0; i < path.size() - 1; i++) { + uint32_t next_index = i + 1; + const Vector2 &vertex1 = path_ptrw[i]; + Vector2 &vertex2 = path_ptrw[next_index]; + + float edge_length = (vertex2 - vertex1).length(); + + if (check_max_radius && start_pos.distance_squared_to(vertex2) > max_radius_sqr) { + // Path point segment goes over max radius, clip it. + + real_t intersect_distance = Geometry2D::segment_intersects_circle(vertex2, vertex1, start_pos, max_radius); + if (intersect_distance != -1) { + edge_length = intersect_distance; + Vector2 intersect_positon = vertex1 + (vertex1.direction_to(vertex2) * intersect_distance); + + path_ptrw[next_index] = intersect_positon; + path_max_size = next_index + 1; + path_max_reached = true; + } + } + + if (check_max_length && accumulated_path_length + edge_length > max_length) { + // Path point segment goes over max length, clip it. + edge_length = max_length - accumulated_path_length; + Vector2 edge_direction = vertex1.direction_to(vertex2); + + path_ptrw[next_index] = vertex1 + (edge_direction * edge_length); + path_max_size = next_index + 1; + + p_query_task.path_length = accumulated_path_length + edge_length; + path_max_reached = true; + } + + accumulated_path_length += edge_length; + + if (path_max_reached) { + break; + } + } + + p_query_task.path_length = accumulated_path_length; + + if (path_max_size < path.size()) { + p_query_task.path_points.resize(path_max_size); + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + p_query_task.path_meta_point_types.resize(path_max_size); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + p_query_task.path_meta_point_rids.resize(path_max_size); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + p_query_task.path_meta_point_owners.resize(path_max_size); + } + } +} + void NavMeshQueries2D::_query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task) { if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) { return; diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.h b/modules/navigation_2d/2d/nav_mesh_queries_2d.h index 2bfa4471c07..14ebac1c2cb 100644 --- a/modules/navigation_2d/2d/nav_mesh_queries_2d.h +++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.h @@ -34,6 +34,7 @@ #include "core/templates/a_hash_map.h" +#include "servers/navigation/navigation_globals.h" #include "servers/navigation/navigation_path_query_parameters_2d.h" #include "servers/navigation/navigation_path_query_result_2d.h" #include "servers/navigation/navigation_utilities.h" @@ -71,11 +72,17 @@ 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; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults2D::path_search_max_polygons; + float path_search_max_distance = 0.0; + // Path building. Vector2 begin_position; Vector2 end_position; @@ -92,6 +99,7 @@ public: LocalVector path_meta_point_types; LocalVector path_meta_point_rids; LocalVector path_meta_point_owners; + float path_length = 0.0; Ref query_parameters; Ref query_result; @@ -138,9 +146,12 @@ public: static void _query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const Nav2D::NavigationPoly *p_from_poly, const Vector2 &p_to_point, const Nav2D::NavigationPoly *p_to_poly); static void _query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task); static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner); + static void _query_task_process_path_result_limits(NavMeshPathQueryTask2D &p_query_task); static void _query_task_search_polygon_connections(NavMeshPathQueryTask2D &p_query_task, const Nav2D::Connection &p_connection, uint32_t p_least_cost_id, const Nav2D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector2 &p_end_point); static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices); static LocalVector get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon); + + static float _calculate_path_length(const LocalVector &p_path, uint32_t p_start_index, uint32_t p_end_index); }; diff --git a/modules/navigation_3d/3d/nav_mesh_queries_3d.cpp b/modules/navigation_3d/3d/nav_mesh_queries_3d.cpp index 73f9c010ea1..a0020b3f69d 100644 --- a/modules/navigation_3d/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation_3d/3d/nav_mesh_queries_3d.cpp @@ -34,6 +34,7 @@ #include "../nav_map_3d.h" #include "nav_region_iteration_3d.h" +#include "core/math/geometry_2d.h" #include "core/math/geometry_3d.h" #include "servers/navigation/navigation_utilities.h" @@ -210,6 +211,10 @@ void NavMeshQueries3D::map_query_path(NavMap3D *map, const Refget_metadata_flags(); query_task.simplify_path = p_query_parameters->get_simplify_path(); query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon(); + query_task.path_return_max_length = p_query_parameters->get_path_return_max_length(); + query_task.path_return_max_radius = p_query_parameters->get_path_return_max_radius(); + query_task.path_search_max_polygons = p_query_parameters->get_path_search_max_polygons(); + query_task.path_search_max_distance = p_query_parameters->get_path_search_max_distance(); query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED; map->query_path(query_task); @@ -219,6 +224,7 @@ void NavMeshQueries3D::map_query_path(NavMap3D *map, const Refset_path_length(query_task.path_length); if (query_task.callback.is_valid()) { if (emit_callback(query_task.callback)) { @@ -347,11 +353,24 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p const HashMap>> &navbases_polygons_external_connections = p_map_iteration.navbases_polygons_external_connections; + // True if we reached the max polygon search count or distance from the begin position. + bool path_search_max_reached = false; + + const float path_search_max_distance_sqr = p_query_task.path_search_max_distance * p_query_task.path_search_max_distance; + bool has_path_search_max_distance = path_search_max_distance_sqr > 0.0; + + int processed_polygon_count = 0; + bool has_path_search_max_polygons = p_query_task.path_search_max_polygons > 0; + + bool has_path_search_max = p_query_task.path_search_max_polygons > 0 || path_search_max_distance_sqr > 0.0; + while (true) { const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; const NavBaseIteration3D *least_cost_navbase = least_cost_poly.poly->owner; + processed_polygon_count += 1; + const uint32_t navbase_local_polygon_id = least_cost_poly.poly->id; const LocalVector> &navbase_polygons_to_connections = least_cost_poly.poly->owner->get_internal_connections(); @@ -368,6 +387,16 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p _query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point); } + if (has_path_search_max && !path_search_max_reached) { + if (has_path_search_max_polygons && processed_polygon_count >= p_query_task.path_search_max_polygons) { + path_search_max_reached = true; + traversable_polys.clear(); + } else if (has_path_search_max_distance && begin_point.distance_squared_to(least_cost_poly.entry) > path_search_max_distance_sqr) { + path_search_max_reached = true; + traversable_polys.clear(); + } + } + poly_enter_cost = 0; // When the heap of traversable polygons is empty at this point it means the end polygon is // unreachable. @@ -497,6 +526,7 @@ void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D p_query_task.path_clear(); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon); + _query_task_process_path_result_limits(p_query_task); p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; return; } @@ -504,6 +534,7 @@ void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D _query_task_build_path_corridor(p_query_task, p_map_iteration); if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) { + _query_task_process_path_result_limits(p_query_task); return; } @@ -530,6 +561,8 @@ void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D _query_task_simplified_path_points(p_query_task); } + _query_task_process_path_result_limits(p_query_task); + #ifdef DEBUG_ENABLED // Ensure post conditions as path meta arrays if used MUST match in array size with the path points. if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { @@ -548,6 +581,116 @@ void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; } +float NavMeshQueries3D::_calculate_path_length(const LocalVector &p_path, uint32_t p_start_index, uint32_t p_end_index) { + const uint32_t path_size = p_path.size(); + if (path_size < 2) { + return 0.0; + } + + ERR_FAIL_COND_V(p_start_index >= p_end_index, 0.0); + ERR_FAIL_COND_V(p_start_index >= path_size - 1, 0.0); + ERR_FAIL_COND_V(p_end_index >= path_size, 0.0); + + const Vector3 *path_ptr = p_path.ptr(); + + float path_length = 0.0; + + for (uint32_t i = p_start_index; i < p_end_index; i++) { + const Vector3 &vertex1 = path_ptr[i]; + const Vector3 &vertex2 = path_ptr[i + 1]; + float edge_length = vertex1.distance_to(vertex2); + path_length += edge_length; + } + + return path_length; +} + +void NavMeshQueries3D::_query_task_process_path_result_limits(NavMeshPathQueryTask3D &p_query_task) { + if (p_query_task.path_points.size() < 2) { + return; + } + + bool check_max_length = p_query_task.path_return_max_length > 0.0; + bool check_max_radius = p_query_task.path_return_max_radius > 0.0; + + if (!check_max_length && !check_max_radius) { + p_query_task.path_length = _calculate_path_length(p_query_task.path_points, 0, p_query_task.path_points.size() - 1); + return; + } + + LocalVector &path = p_query_task.path_points; + + const float max_length = p_query_task.path_return_max_length; + const float max_radius = p_query_task.path_return_max_radius; + const float max_radius_sqr = max_radius * max_radius; + + const Vector3 &start_pos = path[0]; + + float accumulated_path_length = 0.0; + + Vector3 *path_ptrw = path.ptr(); + + uint32_t path_max_size = path.size(); + bool path_max_reached = false; + + for (uint32_t i = 0; i < path.size() - 1; i++) { + uint32_t next_index = i + 1; + const Vector3 &vertex1 = path_ptrw[i]; + Vector3 &vertex2 = path_ptrw[next_index]; + + float edge_length = (vertex2 - vertex1).length(); + + if (check_max_radius && start_pos.distance_squared_to(vertex2) > max_radius_sqr) { + // Path point segment goes over max radius, clip it. + Vector3 intersect_positon, intersect_normal; + bool intersected = Geometry3D::segment_intersects_sphere(vertex2, vertex1, start_pos, max_radius, &intersect_positon, &intersect_normal); + if (intersected) { + edge_length = (intersect_positon - vertex1).length(); + + path_ptrw[next_index] = intersect_positon; + path_max_size = next_index + 1; + path_max_reached = true; + } + } + + if (check_max_length && accumulated_path_length + edge_length > max_length) { + // Path point segment goes over max length, clip it. + edge_length = max_length - accumulated_path_length; + Vector3 edge_direction = vertex1.direction_to(vertex2); + + path_ptrw[next_index] = vertex1 + (edge_direction * edge_length); + path_max_size = next_index + 1; + + p_query_task.path_length = accumulated_path_length + edge_length; + path_max_reached = true; + } + + accumulated_path_length += edge_length; + + if (path_max_reached) { + break; + } + } + + p_query_task.path_length = accumulated_path_length; + + if (path_max_size < path.size()) { + p_query_task.path_points.resize(path_max_size); + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + p_query_task.path_meta_point_types.resize(path_max_size); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + p_query_task.path_meta_point_rids.resize(path_max_size); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + p_query_task.path_meta_point_owners.resize(path_max_size); + } + } +} + void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) { if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) { return; diff --git a/modules/navigation_3d/3d/nav_mesh_queries_3d.h b/modules/navigation_3d/3d/nav_mesh_queries_3d.h index cbcdb9e0808..31889e3613d 100644 --- a/modules/navigation_3d/3d/nav_mesh_queries_3d.h +++ b/modules/navigation_3d/3d/nav_mesh_queries_3d.h @@ -34,6 +34,7 @@ #include "core/templates/a_hash_map.h" +#include "servers/navigation/navigation_globals.h" #include "servers/navigation/navigation_path_query_parameters_3d.h" #include "servers/navigation/navigation_path_query_result_3d.h" #include "servers/navigation/navigation_utilities.h" @@ -71,11 +72,17 @@ 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; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults3D::path_search_max_polygons; + float path_search_max_distance = 0.0; + // Path building. Vector3 begin_position; Vector3 end_position; @@ -93,6 +100,7 @@ public: LocalVector path_meta_point_types; LocalVector path_meta_point_rids; LocalVector path_meta_point_owners; + float path_length = 0.0; Ref query_parameters; Ref query_result; @@ -143,9 +151,12 @@ public: static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const Nav3D::NavigationPoly *from_poly, const Vector3 &p_to_point, const Nav3D::NavigationPoly *p_to_poly); static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task); static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner); + static void _query_task_process_path_result_limits(NavMeshPathQueryTask3D &p_query_task); static void _query_task_search_polygon_connections(NavMeshPathQueryTask3D &p_query_task, const Nav3D::Connection &p_connection, uint32_t p_least_cost_id, const Nav3D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector3 &p_end_point); static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices); static LocalVector get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon); + + static float _calculate_path_length(const LocalVector &p_path, uint32_t p_start_index, uint32_t p_end_index); }; diff --git a/scene/2d/navigation/navigation_agent_2d.cpp b/scene/2d/navigation/navigation_agent_2d.cpp index 5b540e0c68a..8fd09e90d79 100644 --- a/scene/2d/navigation/navigation_agent_2d.cpp +++ b/scene/2d/navigation/navigation_agent_2d.cpp @@ -95,6 +95,20 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationAgent2D::set_simplify_epsilon); ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationAgent2D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("set_path_return_max_length", "length"), &NavigationAgent2D::set_path_return_max_length); + ClassDB::bind_method(D_METHOD("get_path_return_max_length"), &NavigationAgent2D::get_path_return_max_length); + + ClassDB::bind_method(D_METHOD("set_path_return_max_radius", "radius"), &NavigationAgent2D::set_path_return_max_radius); + ClassDB::bind_method(D_METHOD("get_path_return_max_radius"), &NavigationAgent2D::get_path_return_max_radius); + + ClassDB::bind_method(D_METHOD("set_path_search_max_polygons", "max_polygons"), &NavigationAgent2D::set_path_search_max_polygons); + ClassDB::bind_method(D_METHOD("get_path_search_max_polygons"), &NavigationAgent2D::get_path_search_max_polygons); + + ClassDB::bind_method(D_METHOD("set_path_search_max_distance", "distance"), &NavigationAgent2D::set_path_search_max_distance); + ClassDB::bind_method(D_METHOD("get_path_search_max_distance"), &NavigationAgent2D::get_path_search_max_distance); + + ClassDB::bind_method(D_METHOD("get_path_length"), &NavigationAgent2D::get_path_length); + ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent2D::get_next_path_position); ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent2D::set_velocity_forced); @@ -137,6 +151,10 @@ void NavigationAgent2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:px"), "set_simplify_epsilon", "get_simplify_epsilon"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_length", PROPERTY_HINT_RANGE, "0.0,10240.0,0.001,or_greater,suffix:px"), "set_path_return_max_length", "get_path_return_max_length"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_radius", PROPERTY_HINT_RANGE, "0.0,10240.0,0.001,or_greater,suffix:px"), "set_path_return_max_radius", "get_path_return_max_radius"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_search_max_polygons", PROPERTY_HINT_RANGE, "0,4096,1,or_greater"), "set_path_search_max_polygons", "get_path_search_max_polygons"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_search_max_distance"), "set_path_search_max_distance", "get_path_search_max_distance"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -461,6 +479,46 @@ real_t NavigationAgent2D::get_simplify_epsilon() const { return simplify_epsilon; } +void NavigationAgent2D::set_path_return_max_length(float p_length) { + path_return_max_length = MAX(0.0, p_length); + navigation_query->set_path_return_max_length(path_return_max_length); +} + +float NavigationAgent2D::get_path_return_max_length() const { + return path_return_max_length; +} + +void NavigationAgent2D::set_path_return_max_radius(float p_radius) { + path_return_max_radius = MAX(0.0, p_radius); + navigation_query->set_path_return_max_radius(path_return_max_radius); +} + +float NavigationAgent2D::get_path_return_max_radius() const { + return path_return_max_radius; +} + +void NavigationAgent2D::set_path_search_max_polygons(int p_max_polygons) { + path_search_max_polygons = p_max_polygons; + navigation_query->set_path_search_max_polygons(path_search_max_polygons); +} + +int NavigationAgent2D::get_path_search_max_polygons() const { + return path_search_max_polygons; +} + +void NavigationAgent2D::set_path_search_max_distance(float p_distance) { + path_search_max_distance = MAX(0.0, p_distance); + navigation_query->set_path_search_max_distance(path_search_max_distance); +} + +float NavigationAgent2D::get_path_search_max_distance() const { + return path_search_max_distance; +} + +float NavigationAgent2D::get_path_length() const { + return navigation_result->get_path_length(); +} + void NavigationAgent2D::set_path_metadata_flags(BitField p_path_metadata_flags) { if (path_metadata_flags == p_path_metadata_flags) { return; diff --git a/scene/2d/navigation/navigation_agent_2d.h b/scene/2d/navigation/navigation_agent_2d.h index 2e442783075..2e35c29c05b 100644 --- a/scene/2d/navigation/navigation_agent_2d.h +++ b/scene/2d/navigation/navigation_agent_2d.h @@ -65,6 +65,10 @@ class NavigationAgent2D : public Node { real_t path_max_distance = 100.0; bool simplify_path = false; real_t simplify_epsilon = 0.0; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults2D::path_search_max_polygons; + float path_search_max_distance = 0.0; Vector2 target_position; @@ -185,6 +189,20 @@ public: void set_simplify_epsilon(real_t p_epsilon); real_t get_simplify_epsilon() const; + void set_path_return_max_length(float p_length); + float get_path_return_max_length() const; + + void set_path_return_max_radius(float p_radius); + float get_path_return_max_radius() const; + + void set_path_search_max_polygons(int p_max_polygons); + int get_path_search_max_polygons() const; + + void set_path_search_max_distance(float p_distance); + float get_path_search_max_distance() const; + + float get_path_length() const; + Vector2 get_next_path_position(); Ref get_current_navigation_result() const { return navigation_result; } diff --git a/scene/3d/navigation/navigation_agent_3d.cpp b/scene/3d/navigation/navigation_agent_3d.cpp index b90c9e47016..3b4c55ca38b 100644 --- a/scene/3d/navigation/navigation_agent_3d.cpp +++ b/scene/3d/navigation/navigation_agent_3d.cpp @@ -105,6 +105,20 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationAgent3D::set_simplify_epsilon); ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationAgent3D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("set_path_return_max_length", "length"), &NavigationAgent3D::set_path_return_max_length); + ClassDB::bind_method(D_METHOD("get_path_return_max_length"), &NavigationAgent3D::get_path_return_max_length); + + ClassDB::bind_method(D_METHOD("set_path_return_max_radius", "radius"), &NavigationAgent3D::set_path_return_max_radius); + ClassDB::bind_method(D_METHOD("get_path_return_max_radius"), &NavigationAgent3D::get_path_return_max_radius); + + ClassDB::bind_method(D_METHOD("set_path_search_max_polygons", "max_polygons"), &NavigationAgent3D::set_path_search_max_polygons); + ClassDB::bind_method(D_METHOD("get_path_search_max_polygons"), &NavigationAgent3D::get_path_search_max_polygons); + + ClassDB::bind_method(D_METHOD("set_path_search_max_distance", "distance"), &NavigationAgent3D::set_path_search_max_distance); + ClassDB::bind_method(D_METHOD("get_path_search_max_distance"), &NavigationAgent3D::get_path_search_max_distance); + + ClassDB::bind_method(D_METHOD("get_path_length"), &NavigationAgent3D::get_path_length); + ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent3D::get_next_path_position); ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent3D::set_velocity_forced); @@ -148,6 +162,10 @@ void NavigationAgent3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:m"), "set_simplify_epsilon", "get_simplify_epsilon"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_length", PROPERTY_HINT_RANGE, "0.0,100.0,0.001,or_greater,suffix:m"), "set_path_return_max_length", "get_path_return_max_length"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_radius", PROPERTY_HINT_RANGE, "0.0,100.0,0.001,or_greater,suffix:m"), "set_path_return_max_radius", "get_path_return_max_radius"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_search_max_polygons", PROPERTY_HINT_RANGE, "0,4096,1,or_greater"), "set_path_search_max_polygons", "get_path_search_max_polygons"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_search_max_distance"), "set_path_search_max_distance", "get_path_search_max_distance"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -498,6 +516,46 @@ real_t NavigationAgent3D::get_simplify_epsilon() const { return simplify_epsilon; } +void NavigationAgent3D::set_path_return_max_length(float p_length) { + path_return_max_length = MAX(0.0, p_length); + navigation_query->set_path_return_max_length(path_return_max_length); +} + +float NavigationAgent3D::get_path_return_max_length() const { + return path_return_max_length; +} + +void NavigationAgent3D::set_path_return_max_radius(float p_radius) { + path_return_max_radius = MAX(0.0, p_radius); + navigation_query->set_path_return_max_radius(path_return_max_radius); +} + +float NavigationAgent3D::get_path_return_max_radius() const { + return path_return_max_radius; +} + +void NavigationAgent3D::set_path_search_max_polygons(int p_max_polygons) { + path_search_max_polygons = p_max_polygons; + navigation_query->set_path_search_max_polygons(path_search_max_polygons); +} + +int NavigationAgent3D::get_path_search_max_polygons() const { + return path_search_max_polygons; +} + +void NavigationAgent3D::set_path_search_max_distance(float p_distance) { + path_search_max_distance = MAX(0.0, p_distance); + navigation_query->set_path_search_max_distance(path_search_max_distance); +} + +float NavigationAgent3D::get_path_search_max_distance() const { + return path_search_max_distance; +} + +float NavigationAgent3D::get_path_length() const { + return navigation_result->get_path_length(); +} + void NavigationAgent3D::set_path_metadata_flags(BitField p_path_metadata_flags) { if (path_metadata_flags == p_path_metadata_flags) { return; diff --git a/scene/3d/navigation/navigation_agent_3d.h b/scene/3d/navigation/navigation_agent_3d.h index edee7c9c2ec..a21fa84ef3b 100644 --- a/scene/3d/navigation/navigation_agent_3d.h +++ b/scene/3d/navigation/navigation_agent_3d.h @@ -68,6 +68,10 @@ class NavigationAgent3D : public Node { real_t path_max_distance = 5.0; bool simplify_path = false; real_t simplify_epsilon = 0.0; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults3D::path_search_max_polygons; + float path_search_max_distance = 0.0; Vector3 target_position; @@ -206,6 +210,20 @@ public: void set_simplify_epsilon(real_t p_epsilon); real_t get_simplify_epsilon() const; + void set_path_return_max_length(float p_length); + float get_path_return_max_length() const; + + void set_path_return_max_radius(float p_radius); + float get_path_return_max_radius() const; + + void set_path_search_max_polygons(int p_max_polygons); + int get_path_search_max_polygons() const; + + void set_path_search_max_distance(float p_distance); + float get_path_search_max_distance() const; + + float get_path_length() const; + Vector3 get_next_path_position(); Ref get_current_navigation_result() const { return navigation_result; } diff --git a/servers/navigation/navigation_globals.h b/servers/navigation/navigation_globals.h index 1fd5d0ae76c..ea4240f34fe 100644 --- a/servers/navigation/navigation_globals.h +++ b/servers/navigation/navigation_globals.h @@ -45,6 +45,7 @@ constexpr const char *const NAV_MESH_CELL_SIZE_HINT = "0.001,100,0.001,or_greate constexpr float EDGE_CONNECTION_MARGIN = 0.25f; constexpr float LINK_CONNECTION_RADIUS = 1.0f; +constexpr int path_search_max_polygons = 4096; // Agent. @@ -71,6 +72,7 @@ constexpr const char *const NAV_MESH_CELL_SIZE_HINT = "0.001,100,0.001,or_greate constexpr float EDGE_CONNECTION_MARGIN = 1.0f; constexpr float LINK_CONNECTION_RADIUS = 4.0f; +constexpr int path_search_max_polygons = 4096; // Agent. diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp index 2e57602dc69..943a0912634 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.cpp +++ b/servers/navigation/navigation_path_query_parameters_2d.cpp @@ -134,6 +134,38 @@ TypedArray NavigationPathQueryParameters2D::get_excluded_regions() const { return r_regions; } +void NavigationPathQueryParameters2D::set_path_return_max_length(float p_length) { + path_return_max_length = MAX(0.0, p_length); +} + +float NavigationPathQueryParameters2D::get_path_return_max_length() const { + return path_return_max_length; +} + +void NavigationPathQueryParameters2D::set_path_return_max_radius(float p_radius) { + path_return_max_radius = MAX(0.0, p_radius); +} + +float NavigationPathQueryParameters2D::get_path_return_max_radius() const { + return path_return_max_radius; +} + +void NavigationPathQueryParameters2D::set_path_search_max_polygons(int p_max_polygons) { + path_search_max_polygons = p_max_polygons; +} + +int NavigationPathQueryParameters2D::get_path_search_max_polygons() const { + return path_search_max_polygons; +} + +void NavigationPathQueryParameters2D::set_path_search_max_distance(float p_distance) { + path_search_max_distance = MAX(0.0, p_distance); +} + +float NavigationPathQueryParameters2D::get_path_search_max_distance() const { + return path_search_max_distance; +} + 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); @@ -168,6 +200,18 @@ void NavigationPathQueryParameters2D::_bind_methods() { 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); + ClassDB::bind_method(D_METHOD("set_path_return_max_length", "length"), &NavigationPathQueryParameters2D::set_path_return_max_length); + ClassDB::bind_method(D_METHOD("get_path_return_max_length"), &NavigationPathQueryParameters2D::get_path_return_max_length); + + ClassDB::bind_method(D_METHOD("set_path_return_max_radius", "radius"), &NavigationPathQueryParameters2D::set_path_return_max_radius); + ClassDB::bind_method(D_METHOD("get_path_return_max_radius"), &NavigationPathQueryParameters2D::get_path_return_max_radius); + + ClassDB::bind_method(D_METHOD("set_path_search_max_polygons", "max_polygons"), &NavigationPathQueryParameters2D::set_path_search_max_polygons); + ClassDB::bind_method(D_METHOD("get_path_search_max_polygons"), &NavigationPathQueryParameters2D::get_path_search_max_polygons); + + ClassDB::bind_method(D_METHOD("set_path_search_max_distance", "distance"), &NavigationPathQueryParameters2D::set_path_search_max_distance); + ClassDB::bind_method(D_METHOD("get_path_search_max_distance"), &NavigationPathQueryParameters2D::get_path_search_max_distance); + 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"); @@ -179,6 +223,10 @@ void NavigationPathQueryParameters2D::_bind_methods() { 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"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_length"), "set_path_return_max_length", "get_path_return_max_length"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_radius"), "set_path_return_max_radius", "get_path_return_max_radius"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_search_max_polygons"), "set_path_search_max_polygons", "get_path_search_max_polygons"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_search_max_distance"), "set_path_search_max_distance", "get_path_search_max_distance"); 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 3a910e22cd8..d794e819eb9 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.h +++ b/servers/navigation/navigation_path_query_parameters_2d.h @@ -31,6 +31,7 @@ #pragma once #include "core/object/ref_counted.h" +#include "servers/navigation/navigation_globals.h" #include "servers/navigation/navigation_utilities.h" class NavigationPathQueryParameters2D : public RefCounted { @@ -68,9 +69,15 @@ private: BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; bool simplify_path = false; real_t simplify_epsilon = 0.0; + LocalVector _excluded_regions; LocalVector _included_regions; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults2D::path_search_max_polygons; + float path_search_max_distance = 0.0; + public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); PathfindingAlgorithm get_pathfinding_algorithm() const; @@ -104,6 +111,18 @@ public: void set_included_regions(const TypedArray &p_regions); TypedArray get_included_regions() const; + + void set_path_return_max_length(float p_length); + float get_path_return_max_length() const; + + void set_path_return_max_radius(float p_radius); + float get_path_return_max_radius() const; + + void set_path_search_max_polygons(int p_max_polygons); + int get_path_search_max_polygons() const; + + void set_path_search_max_distance(float p_distance); + float get_path_search_max_distance() 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 39c41efeb3d..756407ff4f7 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.cpp +++ b/servers/navigation/navigation_path_query_parameters_3d.cpp @@ -134,6 +134,38 @@ TypedArray NavigationPathQueryParameters3D::get_excluded_regions() const { return r_regions; } +void NavigationPathQueryParameters3D::set_path_return_max_length(float p_length) { + path_return_max_length = MAX(0.0, p_length); +} + +float NavigationPathQueryParameters3D::get_path_return_max_length() const { + return path_return_max_length; +} + +void NavigationPathQueryParameters3D::set_path_return_max_radius(float p_radius) { + path_return_max_radius = MAX(0.0, p_radius); +} + +float NavigationPathQueryParameters3D::get_path_return_max_radius() const { + return path_return_max_radius; +} + +void NavigationPathQueryParameters3D::set_path_search_max_polygons(int p_max_polygons) { + path_search_max_polygons = p_max_polygons; +} + +int NavigationPathQueryParameters3D::get_path_search_max_polygons() const { + return path_search_max_polygons; +} + +void NavigationPathQueryParameters3D::set_path_search_max_distance(float p_distance) { + path_search_max_distance = MAX(0.0, p_distance); +} + +float NavigationPathQueryParameters3D::get_path_search_max_distance() const { + return path_search_max_distance; +} + 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); @@ -168,6 +200,18 @@ void NavigationPathQueryParameters3D::_bind_methods() { 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); + ClassDB::bind_method(D_METHOD("set_path_return_max_length", "length"), &NavigationPathQueryParameters3D::set_path_return_max_length); + ClassDB::bind_method(D_METHOD("get_path_return_max_length"), &NavigationPathQueryParameters3D::get_path_return_max_length); + + ClassDB::bind_method(D_METHOD("set_path_return_max_radius", "radius"), &NavigationPathQueryParameters3D::set_path_return_max_radius); + ClassDB::bind_method(D_METHOD("get_path_return_max_radius"), &NavigationPathQueryParameters3D::get_path_return_max_radius); + + ClassDB::bind_method(D_METHOD("set_path_search_max_polygons", "max_polygons"), &NavigationPathQueryParameters3D::set_path_search_max_polygons); + ClassDB::bind_method(D_METHOD("get_path_search_max_polygons"), &NavigationPathQueryParameters3D::get_path_search_max_polygons); + + ClassDB::bind_method(D_METHOD("set_path_search_max_distance", "distance"), &NavigationPathQueryParameters3D::set_path_search_max_distance); + ClassDB::bind_method(D_METHOD("get_path_search_max_distance"), &NavigationPathQueryParameters3D::get_path_search_max_distance); + 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"); @@ -179,6 +223,10 @@ void NavigationPathQueryParameters3D::_bind_methods() { 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"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_length"), "set_path_return_max_length", "get_path_return_max_length"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_return_max_radius"), "set_path_return_max_radius", "get_path_return_max_radius"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_search_max_polygons"), "set_path_search_max_polygons", "get_path_search_max_polygons"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_search_max_distance"), "set_path_search_max_distance", "get_path_search_max_distance"); 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 38b743e313f..94d551c366f 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.h +++ b/servers/navigation/navigation_path_query_parameters_3d.h @@ -31,6 +31,7 @@ #pragma once #include "core/object/ref_counted.h" +#include "servers/navigation/navigation_globals.h" #include "servers/navigation/navigation_utilities.h" class NavigationPathQueryParameters3D : public RefCounted { @@ -68,9 +69,15 @@ private: BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; bool simplify_path = false; real_t simplify_epsilon = 0.0; + LocalVector _excluded_regions; LocalVector _included_regions; + float path_return_max_length = 0.0; + float path_return_max_radius = 0.0; + int path_search_max_polygons = NavigationDefaults3D::path_search_max_polygons; + float path_search_max_distance = 0.0; + public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); PathfindingAlgorithm get_pathfinding_algorithm() const; @@ -104,6 +111,18 @@ public: void set_included_regions(const TypedArray &p_regions); TypedArray get_included_regions() const; + + void set_path_return_max_length(float p_length); + float get_path_return_max_length() const; + + void set_path_return_max_radius(float p_radius); + float get_path_return_max_radius() const; + + void set_path_search_max_polygons(int p_max_polygons); + int get_path_search_max_polygons() const; + + void set_path_search_max_distance(float p_distance); + float get_path_search_max_distance() const; }; VARIANT_ENUM_CAST(NavigationPathQueryParameters3D::PathfindingAlgorithm); diff --git a/servers/navigation/navigation_path_query_result_2d.cpp b/servers/navigation/navigation_path_query_result_2d.cpp index 4623f0e2f58..f41c2e85e99 100644 --- a/servers/navigation/navigation_path_query_result_2d.cpp +++ b/servers/navigation/navigation_path_query_result_2d.cpp @@ -110,6 +110,14 @@ void NavigationPathQueryResult2D::set_data(const LocalVector &p_path, c } } +void NavigationPathQueryResult2D::set_path_length(float p_length) { + path_length = p_length; +} + +float NavigationPathQueryResult2D::get_path_length() const { + return path_length; +} + void NavigationPathQueryResult2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path", "path"), &NavigationPathQueryResult2D::set_path); ClassDB::bind_method(D_METHOD("get_path"), &NavigationPathQueryResult2D::get_path); @@ -123,12 +131,16 @@ void NavigationPathQueryResult2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult2D::set_path_owner_ids); ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult2D::get_path_owner_ids); + ClassDB::bind_method(D_METHOD("set_path_length", "length"), &NavigationPathQueryResult2D::set_path_length); + ClassDB::bind_method(D_METHOD("get_path_length"), &NavigationPathQueryResult2D::get_path_length); + ClassDB::bind_method(D_METHOD("reset"), &NavigationPathQueryResult2D::reset); ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR2_ARRAY, "path"), "set_path", "get_path"); ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT32_ARRAY, "path_types"), "set_path_types", "get_path_types"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "path_rids", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_path_rids", "get_path_rids"); ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT64_ARRAY, "path_owner_ids"), "set_path_owner_ids", "get_path_owner_ids"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_length"), "set_path_length", "get_path_length"); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_REGION); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_LINK); diff --git a/servers/navigation/navigation_path_query_result_2d.h b/servers/navigation/navigation_path_query_result_2d.h index a0519eb00a7..52b534cb69e 100644 --- a/servers/navigation/navigation_path_query_result_2d.h +++ b/servers/navigation/navigation_path_query_result_2d.h @@ -40,6 +40,7 @@ class NavigationPathQueryResult2D : public RefCounted { Vector path_types; TypedArray path_rids; Vector path_owner_ids; + float path_length = 0.0; protected: static void _bind_methods(); @@ -62,6 +63,9 @@ public: void set_path_owner_ids(const Vector &p_path_owner_ids); const Vector &get_path_owner_ids() const; + void set_path_length(float p_length); + float get_path_length() const; + void reset(); void set_data(const LocalVector &p_path, const LocalVector &p_path_types, const LocalVector &p_path_rids, const LocalVector &p_path_owner_ids); diff --git a/servers/navigation/navigation_path_query_result_3d.cpp b/servers/navigation/navigation_path_query_result_3d.cpp index a205dec7e3b..dad81795610 100644 --- a/servers/navigation/navigation_path_query_result_3d.cpp +++ b/servers/navigation/navigation_path_query_result_3d.cpp @@ -110,6 +110,14 @@ void NavigationPathQueryResult3D::set_data(const LocalVector &p_path, c } } +void NavigationPathQueryResult3D::set_path_length(float p_length) { + path_length = p_length; +} + +float NavigationPathQueryResult3D::get_path_length() const { + return path_length; +} + void NavigationPathQueryResult3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path", "path"), &NavigationPathQueryResult3D::set_path); ClassDB::bind_method(D_METHOD("get_path"), &NavigationPathQueryResult3D::get_path); @@ -123,12 +131,16 @@ void NavigationPathQueryResult3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult3D::set_path_owner_ids); ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult3D::get_path_owner_ids); + ClassDB::bind_method(D_METHOD("set_path_length", "length"), &NavigationPathQueryResult3D::set_path_length); + ClassDB::bind_method(D_METHOD("get_path_length"), &NavigationPathQueryResult3D::get_path_length); + ClassDB::bind_method(D_METHOD("reset"), &NavigationPathQueryResult3D::reset); ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "path"), "set_path", "get_path"); ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT32_ARRAY, "path_types"), "set_path_types", "get_path_types"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "path_rids", PROPERTY_HINT_ARRAY_TYPE, "RID"), "set_path_rids", "get_path_rids"); ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT64_ARRAY, "path_owner_ids"), "set_path_owner_ids", "get_path_owner_ids"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_length"), "set_path_length", "get_path_length"); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_REGION); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_LINK); diff --git a/servers/navigation/navigation_path_query_result_3d.h b/servers/navigation/navigation_path_query_result_3d.h index 88ec25cdf3d..5c5b310abb4 100644 --- a/servers/navigation/navigation_path_query_result_3d.h +++ b/servers/navigation/navigation_path_query_result_3d.h @@ -41,6 +41,7 @@ class NavigationPathQueryResult3D : public RefCounted { Vector path_types; TypedArray path_rids; Vector path_owner_ids; + float path_length = 0.0; protected: static void _bind_methods(); @@ -63,6 +64,9 @@ public: void set_path_owner_ids(const Vector &p_path_owner_ids); const Vector &get_path_owner_ids() const; + void set_path_length(float p_length); + float get_path_length() const; + void reset(); void set_data(const LocalVector &p_path, const LocalVector &p_path_types, const LocalVector &p_path_rids, const LocalVector &p_path_owner_ids);