From 476479419be1acdc272e74255a1586e697fde53b Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Fri, 29 Nov 2024 18:22:26 +0100 Subject: [PATCH] Despaghettify NavigationServer path queries Despaghettify NavigationServer path queries. --- .../NavigationPathQueryParameters2D.xml | 3 + .../NavigationPathQueryParameters3D.xml | 3 + doc/classes/NavigationServer2D.xml | 7 +- doc/classes/NavigationServer3D.xml | 7 +- doc/classes/ProjectSettings.xml | 3 + .../4.3-stable.expected | 13 + .../2d/godot_navigation_server_2d.cpp | 54 +- .../2d/godot_navigation_server_2d.h | 4 +- .../3d/godot_navigation_server_3d.cpp | 196 +---- .../3d/godot_navigation_server_3d.h | 10 +- modules/navigation/3d/nav_mesh_queries_3d.cpp | 777 +++++++++++------- modules/navigation/3d/nav_mesh_queries_3d.h | 77 +- modules/navigation/nav_map.cpp | 67 +- modules/navigation/nav_map.h | 9 +- modules/navigation/nav_utils.h | 9 + scene/2d/navigation_agent_2d.cpp | 2 +- scene/3d/navigation_agent_3d.cpp | 2 +- .../navigation_path_query_parameters_2d.cpp | 80 +- .../navigation_path_query_parameters_2d.h | 25 +- .../navigation_path_query_parameters_3d.cpp | 84 +- .../navigation_path_query_parameters_3d.h | 33 +- .../navigation_path_query_result_2d.h | 4 +- .../navigation_path_query_result_3d.h | 4 +- servers/navigation/navigation_utilities.h | 20 +- servers/navigation_server_2d.compat.inc | 46 ++ servers/navigation_server_2d.cpp | 3 +- servers/navigation_server_2d.h | 12 +- servers/navigation_server_2d_dummy.h | 4 +- servers/navigation_server_3d.compat.inc | 46 ++ servers/navigation_server_3d.cpp | 17 +- servers/navigation_server_3d.h | 13 +- servers/navigation_server_3d_dummy.h | 5 +- 32 files changed, 984 insertions(+), 655 deletions(-) create mode 100644 servers/navigation_server_2d.compat.inc create mode 100644 servers/navigation_server_3d.compat.inc diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml index ce0a00bc83b..1f9c064f930 100644 --- a/doc/classes/NavigationPathQueryParameters2D.xml +++ b/doc/classes/NavigationPathQueryParameters2D.xml @@ -49,6 +49,9 @@ Centers every path position in the middle of the traveled navigation mesh polygon edge. This creates better paths for tile- or gridbased layouts that restrict the movement to the cells center. + + Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm. + Don't include any additional metadata about the returned path. diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml index fcd913f73b5..a4c622d080d 100644 --- a/doc/classes/NavigationPathQueryParameters3D.xml +++ b/doc/classes/NavigationPathQueryParameters3D.xml @@ -49,6 +49,9 @@ Centers every path position in the middle of the traveled navigation mesh polygon edge. This creates better paths for tile- or gridbased layouts that restrict the movement to the cells center. + + Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm. + Don't include any additional metadata about the returned path. diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index 5c19a6b355d..d1fac97b93e 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -533,7 +533,7 @@ Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map]. - + @@ -754,12 +754,13 @@ [b]Performance:[/b] While convenient, reading data arrays from [Mesh] resources can affect the frame rate negatively. The data needs to be received from the GPU, stalling the [RenderingServer] in the process. For performance prefer the use of e.g. collision shapes or creating the data arrays entirely in code. - + + - Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters2D]. Updates the provided [NavigationPathQueryResult2D] result object with the path among other results requested by the query. + Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters2D]. Updates the provided [NavigationPathQueryResult2D] result object with the path among other results requested by the query. After the process is finished the optional [param callback] will be called. diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index 66a286758bc..ab65f3f7437 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -605,7 +605,7 @@ Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map]. - + @@ -887,12 +887,13 @@ [b]Performance:[/b] While convenient, reading data arrays from [Mesh] resources can affect the frame rate negatively. The data needs to be received from the GPU, stalling the [RenderingServer] in the process. For performance prefer the use of e.g. collision shapes or creating the data arrays entirely in code. - + + - Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters3D]. Updates the provided [NavigationPathQueryResult3D] result object with the path among other results requested by the query. + Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters3D]. Updates the provided [NavigationPathQueryResult3D] result object with the path among other results requested by the query. After the process is finished the optional [param callback] will be called. diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index 0a16b159316..ef3a96cb53c 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -2162,6 +2162,9 @@ If enabled, and baking would potentially lead to an engine crash, the baking will be interrupted and an error message with explanation will be raised. + + Maximum number of threads that can run pathfinding queries simultaneously on the same pathfinding graph, for example the same navigation map. Additional threads increase memory consumption and synchronization time due to the need for extra data copies prepared for each thread. A value of [code]-1[/code] means unlimited and the maximum available OS processor count is used. Defaults to [code]1[/code] when the OS does not support threads. + Maximum number of characters allowed to send as output from the debugger. Over this value, content is dropped. This helps not to stall the debugger connection. diff --git a/misc/extension_api_validation/4.3-stable.expected b/misc/extension_api_validation/4.3-stable.expected index 1bbe720eb94..8cade432e64 100644 --- a/misc/extension_api_validation/4.3-stable.expected +++ b/misc/extension_api_validation/4.3-stable.expected @@ -215,3 +215,16 @@ Validate extension JSON: Error: Field 'classes/Control/properties/offset_right': Validate extension JSON: Error: Field 'classes/Control/properties/offset_top': type changed value in new API, from "int" to "float". Property type changed to float to match the actual internal API and documentation. + + +GH-100129 +--------- +Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/query_path': is_const changed value in new API, from true to false. +Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/query_path': is_const changed value in new API, from true to false. +Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/query_path/arguments': size changed value in new API, from 2 to 3. +Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/query_path/arguments': size changed value in new API, from 2 to 3. +Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/map_get_path': is_const changed value in new API, from true to false. +Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/map_get_path': is_const changed value in new API, from true to false. + +`query_path` and `map_get_path` methods changed to be non const due to internal compatibility and server changes. +Added optional callback parameters to `query_path` functions. Compatibility methods registered. diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index 2af125d4348..de78ea5826a 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -81,12 +81,6 @@ return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \ } -#define FORWARD_5_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, T_4, D_4, CONV_0, CONV_1, CONV_2, CONV_3, CONV_4) \ - GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3, T_4 D_4) \ - const { \ - return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3), CONV_4(D_4))); \ - } - static RID rid_to_rid(const RID d) { return d; } @@ -277,7 +271,9 @@ real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid); void FORWARD_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius, rid_to_rid, real_to_real); real_t FORWARD_1_C(map_get_link_connection_radius, RID, p_map, rid_to_rid); -Vector FORWARD_5_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, uint32_t, p_layers, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool, uint32_to_uint32); +Vector GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) { + return vector_v3_to_v2(NavigationServer3D::get_singleton()->map_get_path(p_map, v2_to_v3(p_origin), v2_to_v3(p_destination), p_optimize, p_navigation_layers)); +} Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3); RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3); @@ -456,16 +452,48 @@ Vector GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) c return vector_v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_vertices(p_obstacle)); } -void GodotNavigationServer2D::query_path(const Ref &p_query_parameters, Ref p_query_result) const { +void GodotNavigationServer2D::query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) { ERR_FAIL_COND(!p_query_parameters.is_valid()); ERR_FAIL_COND(!p_query_result.is_valid()); - const NavigationUtilities::PathQueryResult _query_result = NavigationServer3D::get_singleton()->_query_path(p_query_parameters->get_parameters()); + Ref query_parameters; + query_parameters.instantiate(); - p_query_result->set_path(vector_v3_to_v2(_query_result.path)); - p_query_result->set_path_types(_query_result.path_types); - p_query_result->set_path_rids(_query_result.path_rids); - p_query_result->set_path_owner_ids(_query_result.path_owner_ids); + query_parameters->set_map(p_query_parameters->get_map()); + query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position())); + query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position())); + query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers()); + query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR); + + switch (p_query_parameters->get_path_postprocessing()) { + case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL); + } break; + case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED); + } break; + case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: { + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE); + } break; + default: { + WARN_PRINT("No match for used PathPostProcessing - fallback to default"); + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL); + } break; + } + + query_parameters->set_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()); + + Ref query_result; + query_result.instantiate(); + + NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback); + + p_query_result->set_path(vector_v3_to_v2(query_result->get_path())); + p_query_result->set_path_types(query_result->get_path_types()); + p_query_result->set_path_rids(query_result->get_path_rids()); + p_query_result->set_path_owner_ids(query_result->get_path_owner_ids()); } RID GodotNavigationServer2D::source_geometry_parser_create() { diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h index 1579ca29074..80fdea5ba9d 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.h +++ b/modules/navigation/2d/godot_navigation_server_2d.h @@ -68,7 +68,7 @@ public: virtual real_t map_get_edge_connection_margin(RID p_map) const override; virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override; virtual real_t map_get_link_connection_radius(RID p_map) const override; - virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override; + virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override; virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override; virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override; virtual TypedArray map_get_links(RID p_map) const override; @@ -242,7 +242,7 @@ public: virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override; virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override; - virtual void query_path(const Ref &p_query_parameters, Ref p_query_result) const override; + virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) override; virtual void init() override; virtual void sync() override; diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 5dfc39f6f5e..80e87a4b2e8 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -237,11 +237,29 @@ real_t GodotNavigationServer3D::map_get_link_connection_radius(RID p_map) const return map->get_link_connection_radius(); } -Vector GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const { +Vector GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) { const NavMap *map = map_owner.get_or_null(p_map); ERR_FAIL_NULL_V(map, Vector()); - return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers, nullptr, nullptr, nullptr); + Ref query_parameters; + query_parameters.instantiate(); + + query_parameters->set_map(p_map); + query_parameters->set_start_position(p_origin); + query_parameters->set_target_position(p_destination); + query_parameters->set_navigation_layers(p_navigation_layers); + query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR); + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL); + if (!p_optimize) { + query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PATH_POSTPROCESSING_EDGECENTERED); + } + + Ref query_result; + query_result.instantiate(); + + query_path(query_parameters, query_result); + + return query_result->get_path(); } Vector3 GodotNavigationServer3D::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -1384,86 +1402,14 @@ void GodotNavigationServer3D::finish() { #endif // _3D_DISABLED } -PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters &p_parameters) const { - PathQueryResult r_query_result; +void GodotNavigationServer3D::query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) { + ERR_FAIL_COND(p_query_parameters.is_null()); + ERR_FAIL_COND(p_query_result.is_null()); - const NavMap *map = map_owner.get_or_null(p_parameters.map); - ERR_FAIL_NULL_V(map, r_query_result); + NavMap *map = map_owner.get_or_null(p_query_parameters->get_map()); + ERR_FAIL_NULL(map); - // run the pathfinding - - if (p_parameters.pathfinding_algorithm == PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) { - // while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing - if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) { - r_query_result.path = map->get_path( - p_parameters.start_position, - p_parameters.target_position, - true, - p_parameters.navigation_layers, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr); - } else if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) { - r_query_result.path = map->get_path( - p_parameters.start_position, - p_parameters.target_position, - false, - p_parameters.navigation_layers, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr, - p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr); - } - } else { - return r_query_result; - } - - // add path postprocessing - - if (r_query_result.path.size() > 2 && p_parameters.simplify_path) { - const LocalVector &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon); - - uint32_t indices_count = simplified_path_indices.size(); - - { - Vector3 *w = r_query_result.path.ptrw(); - const Vector3 *r = r_query_result.path.ptr(); - for (uint32_t i = 0; i < indices_count; i++) { - w[i] = r[simplified_path_indices[i]]; - } - r_query_result.path.resize(indices_count); - } - - if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { - int32_t *w = r_query_result.path_types.ptrw(); - const int32_t *r = r_query_result.path_types.ptr(); - for (uint32_t i = 0; i < indices_count; i++) { - w[i] = r[simplified_path_indices[i]]; - } - r_query_result.path_types.resize(indices_count); - } - - if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { - TypedArray simplified_path_rids; - simplified_path_rids.resize(indices_count); - for (uint32_t i = 0; i < indices_count; i++) { - simplified_path_rids[i] = r_query_result.path_rids[i]; - } - r_query_result.path_rids = simplified_path_rids; - } - - if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { - int64_t *w = r_query_result.path_owner_ids.ptrw(); - const int64_t *r = r_query_result.path_owner_ids.ptr(); - for (uint32_t i = 0; i < indices_count; i++) { - w[i] = r[simplified_path_indices[i]]; - } - r_query_result.path_owner_ids.resize(indices_count); - } - } - - // add path stats - - return r_query_result; + NavMeshQueries3D::map_query_path(map, p_query_parameters, p_query_result, p_callback); } RID GodotNavigationServer3D::source_geometry_parser_create() { @@ -1490,86 +1436,32 @@ Vector GodotNavigationServer3D::simplify_path(const Vector &p_ p_epsilon = MAX(0.0, p_epsilon); - LocalVector simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon); + LocalVector source_path; + { + source_path.resize(p_path.size()); + const Vector3 *r = p_path.ptr(); + for (uint32_t i = 0; i < p_path.size(); i++) { + source_path[i] = r[i]; + } + } - uint32_t indices_count = simplified_path_indices.size(); + LocalVector simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(source_path, p_epsilon); + + uint32_t index_count = simplified_path_indices.size(); Vector simplified_path; - simplified_path.resize(indices_count); - - Vector3 *w = simplified_path.ptrw(); - const Vector3 *r = p_path.ptr(); - for (uint32_t i = 0; i < indices_count; i++) { - w[i] = r[simplified_path_indices[i]]; + { + simplified_path.resize(index_count); + Vector3 *w = simplified_path.ptrw(); + const Vector3 *r = source_path.ptr(); + for (uint32_t i = 0; i < index_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } } return simplified_path; } -LocalVector GodotNavigationServer3D::get_simplified_path_indices(const Vector &p_path, real_t p_epsilon) { - p_epsilon = MAX(0.0, p_epsilon); - real_t squared_epsilon = p_epsilon * p_epsilon; - - LocalVector valid_points; - valid_points.resize(p_path.size()); - for (uint32_t i = 0; i < valid_points.size(); i++) { - valid_points[i] = false; - } - - simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points); - - int valid_point_index = 0; - - for (bool valid : valid_points) { - if (valid) { - valid_point_index += 1; - } - } - - LocalVector simplified_path_indices; - simplified_path_indices.resize(valid_point_index); - valid_point_index = 0; - - for (uint32_t i = 0; i < valid_points.size(); i++) { - if (valid_points[i]) { - simplified_path_indices[valid_point_index] = i; - valid_point_index += 1; - } - } - - return simplified_path_indices; -} - -void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector &p_points, real_t p_epsilon, LocalVector &r_valid_points) { - r_valid_points[p_start_inx] = true; - r_valid_points[p_end_inx] = true; - - const Vector3 &start_point = p_points[p_start_inx]; - const Vector3 &end_point = p_points[p_end_inx]; - - Vector3 path_segment[2] = { start_point, end_point }; - - real_t point_max_distance = 0.0; - int point_max_index = 0; - - for (int i = p_start_inx; i < p_end_inx; i++) { - const Vector3 &checked_point = p_points[i]; - - const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment); - real_t distance_squared = closest_point.distance_squared_to(checked_point); - - if (distance_squared > point_max_distance) { - point_max_index = i; - point_max_distance = distance_squared; - } - } - - if (point_max_distance > p_epsilon) { - simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points); - simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points); - } -} - int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const { switch (p_info) { case INFO_ACTIVE_MAPS: { diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h index eae6ea28608..ce8d333535f 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.h +++ b/modules/navigation/3d/godot_navigation_server_3d.h @@ -40,6 +40,8 @@ #include "core/templates/local_vector.h" #include "core/templates/rid.h" #include "core/templates/rid_owner.h" +#include "servers/navigation/navigation_path_query_parameters_3d.h" +#include "servers/navigation/navigation_path_query_result_3d.h" #include "servers/navigation_server_3d.h" /// The commands are functions executed during the `sync` phase. @@ -130,7 +132,7 @@ public: COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius); virtual real_t map_get_link_connection_radius(RID p_map) const override; - virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override; + virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override; virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override; virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override; @@ -273,10 +275,6 @@ public: virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) override; -private: - static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector &p_points, real_t p_epsilon, LocalVector &r_valid_points); - static LocalVector get_simplified_path_indices(const Vector &p_path, real_t p_epsilon); - public: COMMAND_1(free, RID, p_object); @@ -288,7 +286,7 @@ public: virtual void sync() override; virtual void finish() override; - virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override; + virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) override; int get_process_info(ProcessInfo p_info) const override; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp index fa7ccbeb701..b60d8d073dc 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -33,21 +33,22 @@ #include "nav_mesh_queries_3d.h" #include "../nav_base.h" +#include "../nav_map.h" #include "core/math/geometry_3d.h" +#include "servers/navigation/navigation_utilities.h" #define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) -#define APPEND_METADATA(poly) \ - if (r_path_types) { \ - r_path_types->push_back(poly->owner->get_type()); \ - } \ - if (r_path_rids) { \ - r_path_rids->push_back(poly->owner->get_self()); \ - } \ - if (r_path_owners) { \ - r_path_owners->push_back(poly->owner->get_owner_id()); \ - } +bool NavMeshQueries3D::emit_callback(const Callable &p_callback) { + ERR_FAIL_COND_V(!p_callback.is_valid(), false); + + Callable::CallError ce; + Variant result; + p_callback.callp(nullptr, 0, result, ce); + + return ce.error == Callable::CallError::CALL_OK; +} Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) { const LocalVector ®ion_polygons = p_polygons; @@ -127,87 +128,225 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector NavMeshQueries3D::polygons_get_path(const LocalVector &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size) { - // Clear metadata outputs. - if (r_path_types) { - r_path_types->clear(); - } - if (r_path_rids) { - r_path_rids->clear(); - } - if (r_path_owners) { - r_path_owners->clear(); +void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) { + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + p_query_task.path_meta_point_types.resize(2); + p_query_task.path_meta_point_types[0] = begin_poly->owner->get_type(); + p_query_task.path_meta_point_types[1] = end_poly->owner->get_type(); } - // Find the start poly and the end poly on this map. + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + p_query_task.path_meta_point_rids.resize(2); + p_query_task.path_meta_point_rids[0] = begin_poly->owner->get_self(); + p_query_task.path_meta_point_rids[1] = end_poly->owner->get_self(); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + p_query_task.path_meta_point_owners.resize(2); + p_query_task.path_meta_point_owners[0] = begin_poly->owner->get_owner_id(); + p_query_task.path_meta_point_owners[1] = end_poly->owner->get_owner_id(); + } + + p_query_task.path_points.resize(2); + p_query_task.path_points[0] = begin_point; + p_query_task.path_points[1] = end_point; +} + +void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) { + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type()); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self()); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id()); + } + + p_query_task.path_points.push_back(p_point); +} + +void NavMeshQueries3D::map_query_path(NavMap *map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) { + ERR_FAIL_NULL(map); + ERR_FAIL_COND(p_query_parameters.is_null()); + ERR_FAIL_COND(p_query_result.is_null()); + + using namespace NavigationUtilities; + + NavMeshQueries3D::NavMeshPathQueryTask3D query_task; + query_task.start_position = p_query_parameters->get_start_position(); + query_task.target_position = p_query_parameters->get_target_position(); + query_task.navigation_layers = p_query_parameters->get_navigation_layers(); + query_task.callback = p_callback; + + switch (p_query_parameters->get_pathfinding_algorithm()) { + case NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: { + query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; + } break; + default: { + WARN_PRINT("No match for used PathfindingAlgorithm - fallback to default"); + query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; + } break; + } + + switch (p_query_parameters->get_path_postprocessing()) { + case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { + query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; + } break; + case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { + query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED; + } break; + case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE: { + query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_NONE; + } break; + default: { + WARN_PRINT("No match for used PathPostProcessing - fallback to default"); + query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; + } break; + } + + query_task.metadata_flags = (int64_t)p_query_parameters->get_metadata_flags(); + query_task.simplify_path = p_query_parameters->get_simplify_path(); + query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon(); + query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED; + + map->query_path(query_task); + + const uint32_t path_point_size = query_task.path_points.size(); + + Vector path_points; + Vector path_meta_point_types; + TypedArray path_meta_point_rids; + Vector path_meta_point_owners; + + { + path_points.resize(path_point_size); + Vector3 *w = path_points.ptrw(); + const Vector3 *r = query_task.path_points.ptr(); + for (uint32_t i = 0; i < path_point_size; i++) { + w[i] = r[i]; + } + } + + if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + path_meta_point_types.resize(path_point_size); + int32_t *w = path_meta_point_types.ptrw(); + const int32_t *r = query_task.path_meta_point_types.ptr(); + for (uint32_t i = 0; i < path_point_size; i++) { + w[i] = r[i]; + } + } + if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + path_meta_point_rids.resize(path_point_size); + for (uint32_t i = 0; i < path_point_size; i++) { + path_meta_point_rids[i] = query_task.path_meta_point_rids[i]; + } + } + if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + path_meta_point_owners.resize(path_point_size); + int64_t *w = path_meta_point_owners.ptrw(); + const int64_t *r = query_task.path_meta_point_owners.ptr(); + for (uint32_t i = 0; i < path_point_size; i++) { + w[i] = r[i]; + } + } + + p_query_result->set_path(path_points); + p_query_result->set_path_types(path_meta_point_types); + p_query_result->set_path_rids(path_meta_point_rids); + p_query_result->set_path_owner_ids(path_meta_point_owners); + + if (query_task.callback.is_valid()) { + if (emit_callback(query_task.callback)) { + query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_DISPATCHED; + } else { + query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_FAILED; + } + } +} + +void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) { + p_query_task.path_points.clear(); + p_query_task.path_meta_point_types.clear(); + p_query_task.path_meta_point_rids.clear(); + p_query_task.path_meta_point_owners.clear(); + + // Find begin polyon and begin position closest to start position and + // end polyon and end position closest to target position on the map. const gd::Polygon *begin_poly = nullptr; const gd::Polygon *end_poly = nullptr; Vector3 begin_point; Vector3 end_point; - real_t begin_d = FLT_MAX; - real_t end_d = FLT_MAX; - // Find the initial poly and the end poly on this map. - for (const gd::Polygon &p : p_polygons) { - // Only consider the polygon if it in a region with compatible layers. - if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) { - continue; - } - // For each face check the distance between the origin/destination - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - - Vector3 point = face.get_closest_point_to(p_origin); - real_t distance_to_point = point.distance_to(p_origin); - if (distance_to_point < begin_d) { - begin_d = distance_to_point; - begin_poly = &p; - begin_point = point; - } - - point = face.get_closest_point_to(p_destination); - distance_to_point = point.distance_to(p_destination); - if (distance_to_point < end_d) { - end_d = distance_to_point; - end_poly = &p; - end_point = point; - } - } - } + _query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point); // Check for trivial cases if (!begin_poly || !end_poly) { - return Vector(); + p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED; + return; } + if (begin_poly == end_poly) { - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = end_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = end_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = end_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; + _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point); + return; } + _query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point); + + // Post-Process path. + switch (p_query_task.path_postprocessing) { + case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { + _path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up); + } break; + case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { + _path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point); + } break; + case PathPostProcessing::PATH_POSTPROCESSING_NONE: { + _path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point); + } break; + default: { + WARN_PRINT("No match for used PathPostProcessing - fallback to default"); + _path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up); + } break; + } + + p_query_task.path_points.invert(); + p_query_task.path_meta_point_types.invert(); + p_query_task.path_meta_point_rids.invert(); + p_query_task.path_meta_point_owners.invert(); + + if (p_query_task.simplify_path) { + _query_task_simplified_path_points(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)) { + DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size()); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size()); + } + + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size()); + } +#endif // DEBUG_ENABLED + + p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; +} + +void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) { // List of all reachable navigation polys. - LocalVector navigation_polys; - navigation_polys.resize(p_polygons.size() + p_link_polygons_size); + LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; + for (gd::NavigationPoly &polygon : navigation_polys) { + polygon.reset(); + } + + DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_link_polygons_size); // Initialize the matching navigation polygon. gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; @@ -218,11 +357,12 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVector - traversable_polys; + &traversable_polys = p_query_task.path_query_slot->traversable_polys; + traversable_polys.clear(); traversable_polys.reserve(p_polygons.size() * 0.25); // This is an implementation of the A* algorithm. - int least_cost_id = begin_poly->id; + p_query_task.least_cost_id = begin_poly->id; int prev_least_cost_id = -1; bool found_route = false; @@ -232,24 +372,24 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectoredges) { + for (const gd::Edge &edge : navigation_polys[p_query_task.least_cost_id].poly->edges) { // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) { const gd::Edge::Connection &connection = edge.connections[connection_index]; // Only consider the connection to another polygon if this polygon is in a region with compatible layers. - if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { + if ((p_query_task.navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { continue; } - const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; + const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id]; real_t poly_enter_cost = 0.0; real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); } - prev_least_cost_id = least_cost_id; + prev_least_cost_id = p_query_task.least_cost_id; Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); @@ -262,7 +402,7 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVector NavMeshQueries3D::polygons_get_path(const LocalVector NavMeshQueries3D::polygons_get_path(const LocalVectorpoints.size(); point_id++) { Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); + Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); + real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; @@ -322,8 +462,8 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectorpoints.size(); point_id++) { Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); + Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); + real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; @@ -332,30 +472,8 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectorresize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; + _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point); + return; } for (gd::NavigationPoly &nav_poly : navigation_polys) { @@ -363,7 +481,7 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectorid].poly = begin_poly; - least_cost_id = begin_poly->id; + p_query_task.least_cost_id = begin_poly->id; prev_least_cost_id = -1; reachable_end = nullptr; @@ -372,19 +490,19 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectorpoly->id; + p_query_task.least_cost_id = traversable_polys.pop()->poly->id; // Store the farthest reachable end polygon in case our goal is not reachable. if (is_reachable) { - real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_destination); + real_t distance = navigation_polys[p_query_task.least_cost_id].entry.distance_to(p_query_task.target_position); if (distance_to_reachable_end > distance) { distance_to_reachable_end = distance; - reachable_end = navigation_polys[least_cost_id].poly; + reachable_end = navigation_polys[p_query_task.least_cost_id].poly; } } // Check if we reached the end - if (navigation_polys[least_cost_id].poly == end_poly) { + if (navigation_polys[p_query_task.least_cost_id].poly == end_poly) { found_route = true; break; } @@ -393,190 +511,227 @@ Vector NavMeshQueries3D::polygons_get_path(const LocalVectorpoints.size(); point_id++) { Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); + Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); + real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; } } + _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point); + return; + } +} - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; +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; } - Vector path; - // Optimize the path. - if (p_optimize) { - // Set the apex poly/point to the end point - gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; + const LocalVector &simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon); - Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; - const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway); - if (end_point.is_equal_approx(back_edge_closest_point)) { - // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. - // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. - if (apex_poly->back_navigation_poly_id != -1) { - apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id]; - } + uint32_t index_count = simplified_path_indices.size(); + + { + Vector3 *points_ptr = p_query_task.path_points.ptr(); + for (uint32_t i = 0; i < index_count; i++) { + points_ptr[i] = points_ptr[simplified_path_indices[i]]; } + p_query_task.path_points.resize(index_count); + } - Vector3 apex_point = end_point; - - gd::NavigationPoly *left_poly = apex_poly; - Vector3 left_portal = apex_point; - gd::NavigationPoly *right_poly = apex_poly; - Vector3 right_portal = apex_point; - - gd::NavigationPoly *p = apex_poly; - - path.push_back(end_point); - APPEND_METADATA(end_poly); - - while (p) { - // Set left and right points of the pathway between polygons. - Vector3 left = p->back_navigation_edge_pathway_start; - Vector3 right = p->back_navigation_edge_pathway_end; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) { - SWAP(left, right); - } - - bool skip = false; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) { - //process - if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) { - left_poly = p; - left_portal = left; - } else { - clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); - - apex_point = right_portal; - p = right_poly; - left_poly = p; - apex_poly = p; - left_portal = apex_point; - right_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - skip = true; - } - } - - if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) { - //process - if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) { - right_poly = p; - right_portal = right; - } else { - clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); - - apex_point = left_portal; - p = left_poly; - right_poly = p; - apex_poly = p; - right_portal = apex_point; - left_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - } - } - - // Go to the previous polygon. - if (p->back_navigation_poly_id != -1) { - p = &navigation_polys[p->back_navigation_poly_id]; - } else { - // The end - p = nullptr; - } + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + int32_t *types_ptr = p_query_task.path_meta_point_types.ptr(); + for (uint32_t i = 0; i < index_count; i++) { + types_ptr[i] = types_ptr[simplified_path_indices[i]]; } + p_query_task.path_meta_point_types.resize(index_count); + } - // If the last point is not the begin point, add it to the list. - if (path[path.size() - 1] != begin_point) { - path.push_back(begin_point); - APPEND_METADATA(begin_poly); + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + RID *rids_ptr = p_query_task.path_meta_point_rids.ptr(); + for (uint32_t i = 0; i < index_count; i++) { + rids_ptr[i] = rids_ptr[simplified_path_indices[i]]; } + p_query_task.path_meta_point_rids.resize(index_count); + } - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); + if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr(); + for (uint32_t i = 0; i < index_count; i++) { + owners_ptr[i] = owners_ptr[simplified_path_indices[i]]; } + p_query_task.path_meta_point_owners.resize(index_count); + } +} - } else { - path.push_back(end_point); - APPEND_METADATA(end_poly); +void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) { + LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; - // Add mid points - int np_id = least_cost_id; - while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { - if (navigation_polys[np_id].back_navigation_edge != -1) { - int prev = navigation_polys[np_id].back_navigation_edge; - int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); - Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; + // Set the apex poly/point to the end point + gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id]; - path.push_back(point); - APPEND_METADATA(navigation_polys[np_id].poly); - } else { - path.push_back(navigation_polys[np_id].entry); - APPEND_METADATA(navigation_polys[np_id].poly); - } - - np_id = navigation_polys[np_id].back_navigation_poly_id; - } - - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); + Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; + const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway); + if (p_end_point.is_equal_approx(back_edge_closest_point)) { + // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. + // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. + if (apex_poly->back_navigation_poly_id != -1) { + apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id]; } } - // Ensure post conditions (path arrays MUST match in size). - CRASH_COND(r_path_types && path.size() != r_path_types->size()); - CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); - CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); + Vector3 apex_point = p_end_point; - return path; + gd::NavigationPoly *left_poly = apex_poly; + Vector3 left_portal = apex_point; + gd::NavigationPoly *right_poly = apex_poly; + Vector3 right_portal = apex_point; + + gd::NavigationPoly *p = apex_poly; + + _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + + while (p) { + // Set left and right points of the pathway between polygons. + Vector3 left = p->back_navigation_edge_pathway_start; + Vector3 right = p->back_navigation_edge_pathway_end; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) { + SWAP(left, right); + } + + bool skip = false; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) { + //process + if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) { + left_poly = p; + left_portal = left; + } else { + clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up); + + apex_point = right_portal; + p = right_poly; + left_poly = p; + apex_poly = p; + left_portal = apex_point; + right_portal = apex_point; + + _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly); + + skip = true; + } + } + + if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) { + //process + if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) { + right_poly = p; + right_portal = right; + } else { + clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up); + + apex_point = left_portal; + p = left_poly; + right_poly = p; + apex_poly = p; + right_portal = apex_point; + left_portal = apex_point; + + _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly); + } + } + + // Go to the previous polygon. + if (p->back_navigation_poly_id != -1) { + p = &p_path_corridor[p->back_navigation_poly_id]; + } else { + // The end + p = nullptr; + } + } + + // If the last point is not the begin point, add it to the list. + if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) { + _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); + } +} + +void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) { + LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; + + _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + + // Add mid points. + int np_id = p_least_cost_id; + while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) { + if (p_path_corridor[np_id].back_navigation_edge != -1) { + int prev = p_path_corridor[np_id].back_navigation_edge; + int prev_n = (p_path_corridor[np_id].back_navigation_edge + 1) % p_path_corridor[np_id].poly->points.size(); + Vector3 point = (p_path_corridor[np_id].poly->points[prev].pos + p_path_corridor[np_id].poly->points[prev_n].pos) * 0.5; + + _query_task_push_back_point_with_metadata(p_query_task, point, p_path_corridor[np_id].poly); + } else { + _query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly); + } + + np_id = p_path_corridor[np_id].back_navigation_poly_id; + } + + _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); +} + +void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) { + LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; + + _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + + // Add mid points. + int np_id = p_least_cost_id; + while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) { + _query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly); + + np_id = p_path_corridor[np_id].back_navigation_poly_id; + } + + _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); +} + +void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point) { + real_t begin_d = FLT_MAX; + real_t end_d = FLT_MAX; + + // Find the initial poly and the end poly on this map. + for (const gd::Polygon &p : p_polygons) { + // Only consider the polygon if it in a region with compatible layers. + if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) { + continue; + } + + // For each face check the distance between the origin/destination. + for (size_t point_id = 2; point_id < p.points.size(); point_id++) { + const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + + Vector3 point = face.get_closest_point_to(p_query_task.start_position); + real_t distance_to_point = point.distance_to(p_query_task.start_position); + if (distance_to_point < begin_d) { + begin_d = distance_to_point; + *r_begin_poly = &p; + r_begin_point = point; + } + + point = face.get_closest_point_to(p_query_task.target_position); + distance_to_point = point.distance_to(p_query_task.target_position); + if (distance_to_point < end_d) { + end_d = distance_to_point; + *r_end_poly = &p; + r_end_point = point; + } + } + } } Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) { @@ -728,8 +883,8 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up) { - Vector3 from = path[path.size() - 1]; +void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up) { + Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1]; if (from.is_equal_approx(p_to_point)) { return; @@ -753,13 +908,73 @@ void NavMeshQueries3D::clip_path(const LocalVector &p_naviga if (!pathway_start.is_equal_approx(pathway_end)) { Vector3 inters; if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { - if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { - path.push_back(inters); - APPEND_METADATA(from_poly->poly); + if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(p_query_task.path_points[p_query_task.path_points.size() - 1])) { + _query_task_push_back_point_with_metadata(p_query_task, inters, from_poly->poly); } } } } } +LocalVector NavMeshQueries3D::get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon) { + p_epsilon = MAX(0.0, p_epsilon); + real_t squared_epsilon = p_epsilon * p_epsilon; + + LocalVector valid_points; + valid_points.resize(p_path.size()); + for (uint32_t i = 0; i < valid_points.size(); i++) { + valid_points[i] = false; + } + + simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points); + + int valid_point_index = 0; + + for (bool valid : valid_points) { + if (valid) { + valid_point_index += 1; + } + } + + LocalVector simplified_path_indices; + simplified_path_indices.resize(valid_point_index); + valid_point_index = 0; + + for (uint32_t i = 0; i < valid_points.size(); i++) { + if (valid_points[i]) { + simplified_path_indices[valid_point_index] = i; + valid_point_index += 1; + } + } + + return simplified_path_indices; +} + +void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_valid_points) { + r_valid_points[p_start_inx] = true; + r_valid_points[p_end_inx] = true; + + Vector3 path_segment[2] = { p_points[p_start_inx], p_points[p_end_inx] }; + + real_t point_max_distance = 0.0; + int point_max_index = 0; + + for (int i = p_start_inx; i < p_end_inx; i++) { + const Vector3 &checked_point = p_points[i]; + + const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment); + real_t distance_squared = closest_point.distance_squared_to(checked_point); + + if (distance_squared > point_max_distance) { + point_max_index = i; + point_max_distance = distance_squared; + } + } + + if (point_max_distance > p_epsilon) { + simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points); + simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points); + } +} + #endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h index 109bb2f9712..a92520f9444 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -33,20 +33,91 @@ #ifndef _3D_DISABLED -#include "../nav_map.h" +#include "../nav_utils.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" + +using namespace NavigationUtilities; + +class NavMap; class NavMeshQueries3D { public: + struct PathQuerySlot { + LocalVector path_corridor; + gd::Heap traversable_polys; + bool in_use = false; + uint32_t slot_index = 0; + }; + + struct NavMeshPathQueryTask3D { + enum TaskStatus { + QUERY_STARTED, + QUERY_FINISHED, + QUERY_FAILED, + CALLBACK_DISPATCHED, + CALLBACK_FAILED, + }; + + // Parameters. + Vector3 start_position; + Vector3 target_position; + uint32_t navigation_layers; + BitField metadata_flags = PathMetadataFlags::PATH_INCLUDE_ALL; + PathfindingAlgorithm pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; + PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; + + // Path building. + Vector3 begin_position; + Vector3 end_position; + uint32_t least_cost_id = 0; + Vector3 map_up; + NavMap *map = nullptr; + PathQuerySlot *path_query_slot = nullptr; + + // Path points. + LocalVector path_points; + LocalVector path_meta_point_types; + LocalVector path_meta_point_rids; + LocalVector path_meta_point_owners; + + Ref query_parameters; + Ref query_result; + Callable callback; + NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED; + }; + + static bool emit_callback(const Callable &p_callback); + static Vector3 polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly); - static Vector polygons_get_path(const LocalVector &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size); static Vector3 polygons_get_closest_point_to_segment(const LocalVector &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision); static Vector3 polygons_get_closest_point(const LocalVector &p_polygons, const Vector3 &p_point); static Vector3 polygons_get_closest_point_normal(const LocalVector &p_polygons, const Vector3 &p_point); static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector3 &p_point); static RID polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector3 &p_point); - static void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up); + static void map_query_path(NavMap *map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback); + + static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size); + + static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point); + static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon); + static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point); + static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_polygon, Vector3 end_point); + static void _path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up); + static void _path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point); + static void _path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point); + + static void clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up); + static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task); + + static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_valid_points); + static LocalVector get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon); }; #endif // _3D_DISABLED diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 739b031d061..07ee96dc321 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -35,8 +35,6 @@ #include "nav_obstacle.h" #include "nav_region.h" -#include "3d/nav_mesh_queries_3d.h" - #include "core/config/project_settings.h" #include "core/object/worker_thread_pool.h" @@ -123,16 +121,40 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { return p; } -Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners) const { +void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) { RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { - NAVMAP_ITERATION_ZERO_ERROR_MSG(); - return Vector(); + return; } - return NavMeshQueries3D::polygons_get_path( - polygons, p_origin, p_destination, p_optimize, p_navigation_layers, - r_path_types, r_path_rids, r_path_owners, up, link_polygons.size()); + path_query_slots_semaphore.wait(); + + path_query_slots_mutex.lock(); + for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) { + if (!p_path_query_slot.in_use) { + p_path_query_slot.in_use = true; + p_query_task.path_query_slot = &p_path_query_slot; + break; + } + } + path_query_slots_mutex.unlock(); + + if (p_query_task.path_query_slot == nullptr) { + path_query_slots_semaphore.post(); + ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(."); + } + + p_query_task.map_up = get_up(); + + NavMeshQueries3D::query_task_polygons_get_path(p_query_task, polygons, up, link_polygons.size()); + + path_query_slots_mutex.lock(); + uint32_t used_slot_index = p_query_task.path_query_slot->slot_index; + path_query_slots[used_slot_index].in_use = false; + p_query_task.path_query_slot = nullptr; + path_query_slots_mutex.unlock(); + + path_query_slots_semaphore.post(); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -639,6 +661,15 @@ void NavMap::sync() { // Some code treats 0 as a failure case, so we avoid returning 0 and modulo wrap UINT32_MAX manually. iteration_id = iteration_id % UINT32_MAX + 1; + + path_query_slots_mutex.lock(); + for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) { + p_path_query_slot.path_corridor.clear(); + p_path_query_slot.path_corridor.resize(polygons.size() + link_polygons.size()); + p_path_query_slot.traversable_polys.clear(); + p_path_query_slot.traversable_polys.reserve(polygons.size() * 0.25); + } + path_query_slots_mutex.unlock(); } map_settings_dirty = false; @@ -969,6 +1000,26 @@ void NavMap::_sync_dirty_avoidance_update_requests() { NavMap::NavMap() { avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads"); avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads"); + + path_query_slots_max = GLOBAL_GET("navigation/pathfinding/max_threads"); + + int processor_count = OS::get_singleton()->get_processor_count(); + if (path_query_slots_max < 0) { + path_query_slots_max = processor_count; + } + if (processor_count < path_query_slots_max) { + path_query_slots_max = processor_count; + } + if (path_query_slots_max < 1) { + path_query_slots_max = 1; + } + + path_query_slots.resize(path_query_slots_max); + for (uint32_t i = 0; i < path_query_slots.size(); i++) { + path_query_slots[i].slot_index = i; + } + + path_query_slots_semaphore.post(path_query_slots_max); } NavMap::~NavMap() { diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index feeb2cc5483..ce247b83c16 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -31,6 +31,7 @@ #ifndef NAV_MAP_H #define NAV_MAP_H +#include "3d/nav_mesh_queries_3d.h" #include "nav_rid.h" #include "nav_utils.h" @@ -135,6 +136,11 @@ class NavMap : public NavRid { SelfList::List obstacles; } sync_dirty_requests; + LocalVector path_query_slots; + int path_query_slots_max = 4; + Mutex path_query_slots_mutex; + Semaphore path_query_slots_semaphore; + public: NavMap(); ~NavMap(); @@ -176,7 +182,8 @@ public: gd::PointKey get_point_key(const Vector3 &p_pos) const; - Vector get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners) const; + void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task); + Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const; Vector3 get_closest_point(const Vector3 &p_point) const; Vector3 get_closest_point_normal(const Vector3 &p_point) const; diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h index 993a97638dd..ea22fb0bab3 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -145,6 +145,15 @@ struct NavigationPoly { bool operator!=(const NavigationPoly &p_other) const { return !(*this == p_other); } + + void reset() { + poly = nullptr; + traversable_poly_index = UINT32_MAX; + back_navigation_poly_id = -1; + back_navigation_edge = -1; + traveled_distance = 0.0; + distance_to_destination = 0.0; + } }; struct NavPolyTravelCostGreaterThan { diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp index f030473c4b7..786598f01a3 100644 --- a/scene/2d/navigation_agent_2d.cpp +++ b/scene/2d/navigation_agent_2d.cpp @@ -133,7 +133,7 @@ void NavigationAgent2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_max_distance", PROPERTY_HINT_RANGE, "10,1000,1,or_greater,suffix:px"), "set_path_max_distance", "get_path_max_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing"); 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"); diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp index faf138896a5..d5e162d4169 100644 --- a/scene/3d/navigation_agent_3d.cpp +++ b/scene/3d/navigation_agent_3d.cpp @@ -144,7 +144,7 @@ void NavigationAgent3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1,or_greater,suffix:m"), "set_path_max_distance", "get_path_max_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing"); 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"); diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp index 6c1f88e3490..74aaf64b4e0 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.cpp +++ b/servers/navigation/navigation_path_query_parameters_2d.cpp @@ -31,108 +31,75 @@ #include "navigation_path_query_parameters_2d.h" void NavigationPathQueryParameters2D::set_pathfinding_algorithm(const NavigationPathQueryParameters2D::PathfindingAlgorithm p_pathfinding_algorithm) { - switch (p_pathfinding_algorithm) { - case PATHFINDING_ALGORITHM_ASTAR: { - parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; - } break; - default: { - WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default"); - parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; - } break; - } + pathfinding_algorithm = p_pathfinding_algorithm; } NavigationPathQueryParameters2D::PathfindingAlgorithm NavigationPathQueryParameters2D::get_pathfinding_algorithm() const { - switch (parameters.pathfinding_algorithm) { - case NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: - return PATHFINDING_ALGORITHM_ASTAR; - default: - WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default"); - return PATHFINDING_ALGORITHM_ASTAR; - } + return pathfinding_algorithm; } void NavigationPathQueryParameters2D::set_path_postprocessing(const NavigationPathQueryParameters2D::PathPostProcessing p_path_postprocessing) { - switch (p_path_postprocessing) { - case PATH_POSTPROCESSING_CORRIDORFUNNEL: { - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; - } break; - case PATH_POSTPROCESSING_EDGECENTERED: { - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED; - } break; - default: { - WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default"); - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; - } break; - } + path_postprocessing = p_path_postprocessing; } NavigationPathQueryParameters2D::PathPostProcessing NavigationPathQueryParameters2D::get_path_postprocessing() const { - switch (parameters.path_postprocessing) { - case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: - return PATH_POSTPROCESSING_CORRIDORFUNNEL; - case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: - return PATH_POSTPROCESSING_EDGECENTERED; - default: - WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default"); - return PATH_POSTPROCESSING_CORRIDORFUNNEL; - } + return path_postprocessing; } -void NavigationPathQueryParameters2D::set_map(const RID &p_map) { - parameters.map = p_map; +void NavigationPathQueryParameters2D::set_map(RID p_map) { + map = p_map; } -const RID &NavigationPathQueryParameters2D::get_map() const { - return parameters.map; +RID NavigationPathQueryParameters2D::get_map() const { + return map; } -void NavigationPathQueryParameters2D::set_start_position(const Vector2 p_start_position) { - parameters.start_position = Vector3(p_start_position.x, 0.0, p_start_position.y); +void NavigationPathQueryParameters2D::set_start_position(Vector2 p_start_position) { + start_position = p_start_position; } Vector2 NavigationPathQueryParameters2D::get_start_position() const { - return Vector2(parameters.start_position.x, parameters.start_position.z); + return start_position; } -void NavigationPathQueryParameters2D::set_target_position(const Vector2 p_target_position) { - parameters.target_position = Vector3(p_target_position.x, 0.0, p_target_position.y); +void NavigationPathQueryParameters2D::set_target_position(Vector2 p_target_position) { + target_position = p_target_position; } Vector2 NavigationPathQueryParameters2D::get_target_position() const { - return Vector2(parameters.target_position.x, parameters.target_position.z); + return target_position; } void NavigationPathQueryParameters2D::set_navigation_layers(uint32_t p_navigation_layers) { - parameters.navigation_layers = p_navigation_layers; + navigation_layers = p_navigation_layers; } uint32_t NavigationPathQueryParameters2D::get_navigation_layers() const { - return parameters.navigation_layers; + return navigation_layers; } void NavigationPathQueryParameters2D::set_metadata_flags(BitField p_flags) { - parameters.metadata_flags = (int64_t)p_flags; + metadata_flags = (int64_t)p_flags; } BitField NavigationPathQueryParameters2D::get_metadata_flags() const { - return (int64_t)parameters.metadata_flags; + return (int64_t)metadata_flags; } void NavigationPathQueryParameters2D::set_simplify_path(bool p_enabled) { - parameters.simplify_path = p_enabled; + simplify_path = p_enabled; } bool NavigationPathQueryParameters2D::get_simplify_path() const { - return parameters.simplify_path; + return simplify_path; } void NavigationPathQueryParameters2D::set_simplify_epsilon(real_t p_epsilon) { - parameters.simplify_epsilon = MAX(0.0, p_epsilon); + simplify_epsilon = MAX(0.0, p_epsilon); } real_t NavigationPathQueryParameters2D::get_simplify_epsilon() const { - return parameters.simplify_epsilon; + return simplify_epsilon; } void NavigationPathQueryParameters2D::_bind_methods() { @@ -168,7 +135,7 @@ void NavigationPathQueryParameters2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "target_position"), "set_target_position", "get_target_position"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing"); 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"); @@ -177,6 +144,7 @@ void NavigationPathQueryParameters2D::_bind_methods() { BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_CORRIDORFUNNEL); BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_EDGECENTERED); + BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_NONE); BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_NONE); BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_TYPES); diff --git a/servers/navigation/navigation_path_query_parameters_2d.h b/servers/navigation/navigation_path_query_parameters_2d.h index a1d5f2d109a..91031bfc180 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.h +++ b/servers/navigation/navigation_path_query_parameters_2d.h @@ -37,19 +37,18 @@ class NavigationPathQueryParameters2D : public RefCounted { GDCLASS(NavigationPathQueryParameters2D, RefCounted); - NavigationUtilities::PathQueryParameters parameters; - protected: static void _bind_methods(); public: enum PathfindingAlgorithm { - PATHFINDING_ALGORITHM_ASTAR = 0, + PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR, }; enum PathPostProcessing { - PATH_POSTPROCESSING_CORRIDORFUNNEL = 0, - PATH_POSTPROCESSING_EDGECENTERED, + PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL, + PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED, + PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE, }; enum PathMetadataFlags { @@ -60,16 +59,26 @@ public: PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL }; - const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; } +private: + PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR; + PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL; + RID map; + Vector2 start_position; + Vector2 target_position; + uint32_t navigation_layers = 1; + BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; +public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); PathfindingAlgorithm get_pathfinding_algorithm() const; void set_path_postprocessing(const PathPostProcessing p_path_postprocessing); PathPostProcessing get_path_postprocessing() const; - void set_map(const RID &p_map); - const RID &get_map() const; + void set_map(RID p_map); + RID get_map() const; void set_start_position(const Vector2 p_start_position); Vector2 get_start_position() const; diff --git a/servers/navigation/navigation_path_query_parameters_3d.cpp b/servers/navigation/navigation_path_query_parameters_3d.cpp index b0a5b0ad82b..99c5318bed2 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.cpp +++ b/servers/navigation/navigation_path_query_parameters_3d.cpp @@ -31,108 +31,75 @@ #include "navigation_path_query_parameters_3d.h" void NavigationPathQueryParameters3D::set_pathfinding_algorithm(const NavigationPathQueryParameters3D::PathfindingAlgorithm p_pathfinding_algorithm) { - switch (p_pathfinding_algorithm) { - case PATHFINDING_ALGORITHM_ASTAR: { - parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; - } break; - default: { - WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default"); - parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; - } break; - } + pathfinding_algorithm = p_pathfinding_algorithm; } NavigationPathQueryParameters3D::PathfindingAlgorithm NavigationPathQueryParameters3D::get_pathfinding_algorithm() const { - switch (parameters.pathfinding_algorithm) { - case NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: - return PATHFINDING_ALGORITHM_ASTAR; - default: - WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default"); - return PATHFINDING_ALGORITHM_ASTAR; - } + return pathfinding_algorithm; } void NavigationPathQueryParameters3D::set_path_postprocessing(const NavigationPathQueryParameters3D::PathPostProcessing p_path_postprocessing) { - switch (p_path_postprocessing) { - case PATH_POSTPROCESSING_CORRIDORFUNNEL: { - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; - } break; - case PATH_POSTPROCESSING_EDGECENTERED: { - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED; - } break; - default: { - WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default"); - parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; - } break; - } + path_postprocessing = p_path_postprocessing; } NavigationPathQueryParameters3D::PathPostProcessing NavigationPathQueryParameters3D::get_path_postprocessing() const { - switch (parameters.path_postprocessing) { - case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: - return PATH_POSTPROCESSING_CORRIDORFUNNEL; - case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: - return PATH_POSTPROCESSING_EDGECENTERED; - default: - WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default"); - return PATH_POSTPROCESSING_CORRIDORFUNNEL; - } + return path_postprocessing; } -void NavigationPathQueryParameters3D::set_map(const RID &p_map) { - parameters.map = p_map; +void NavigationPathQueryParameters3D::set_map(RID p_map) { + map = p_map; } -const RID &NavigationPathQueryParameters3D::get_map() const { - return parameters.map; +RID NavigationPathQueryParameters3D::get_map() const { + return map; } -void NavigationPathQueryParameters3D::set_start_position(const Vector3 &p_start_position) { - parameters.start_position = p_start_position; +void NavigationPathQueryParameters3D::set_start_position(Vector3 p_start_position) { + start_position = p_start_position; } -const Vector3 &NavigationPathQueryParameters3D::get_start_position() const { - return parameters.start_position; +Vector3 NavigationPathQueryParameters3D::get_start_position() const { + return start_position; } -void NavigationPathQueryParameters3D::set_target_position(const Vector3 &p_target_position) { - parameters.target_position = p_target_position; +void NavigationPathQueryParameters3D::set_target_position(Vector3 p_target_position) { + target_position = p_target_position; } -const Vector3 &NavigationPathQueryParameters3D::get_target_position() const { - return parameters.target_position; +Vector3 NavigationPathQueryParameters3D::get_target_position() const { + return target_position; } void NavigationPathQueryParameters3D::set_navigation_layers(uint32_t p_navigation_layers) { - parameters.navigation_layers = p_navigation_layers; + navigation_layers = p_navigation_layers; } uint32_t NavigationPathQueryParameters3D::get_navigation_layers() const { - return parameters.navigation_layers; + return navigation_layers; } void NavigationPathQueryParameters3D::set_metadata_flags(BitField p_flags) { - parameters.metadata_flags = (int64_t)p_flags; + metadata_flags = (int64_t)p_flags; } BitField NavigationPathQueryParameters3D::get_metadata_flags() const { - return (int64_t)parameters.metadata_flags; + return (int64_t)metadata_flags; } void NavigationPathQueryParameters3D::set_simplify_path(bool p_enabled) { - parameters.simplify_path = p_enabled; + simplify_path = p_enabled; } bool NavigationPathQueryParameters3D::get_simplify_path() const { - return parameters.simplify_path; + return simplify_path; } void NavigationPathQueryParameters3D::set_simplify_epsilon(real_t p_epsilon) { - parameters.simplify_epsilon = MAX(0.0, p_epsilon); + simplify_epsilon = MAX(0.0, p_epsilon); } real_t NavigationPathQueryParameters3D::get_simplify_epsilon() const { - return parameters.simplify_epsilon; + return simplify_epsilon; } void NavigationPathQueryParameters3D::_bind_methods() { @@ -168,7 +135,7 @@ void NavigationPathQueryParameters3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position"), "set_target_position", "get_target_position"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing"); 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"); @@ -177,6 +144,7 @@ void NavigationPathQueryParameters3D::_bind_methods() { BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_CORRIDORFUNNEL); BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_EDGECENTERED); + BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_NONE); BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_NONE); BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_TYPES); diff --git a/servers/navigation/navigation_path_query_parameters_3d.h b/servers/navigation/navigation_path_query_parameters_3d.h index 2eb85db7875..66cad8dcd54 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.h +++ b/servers/navigation/navigation_path_query_parameters_3d.h @@ -37,19 +37,18 @@ class NavigationPathQueryParameters3D : public RefCounted { GDCLASS(NavigationPathQueryParameters3D, RefCounted); - NavigationUtilities::PathQueryParameters parameters; - protected: static void _bind_methods(); public: enum PathfindingAlgorithm { - PATHFINDING_ALGORITHM_ASTAR = 0, + PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR, }; enum PathPostProcessing { - PATH_POSTPROCESSING_CORRIDORFUNNEL = 0, - PATH_POSTPROCESSING_EDGECENTERED, + PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL, + PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED, + PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE, }; enum PathMetadataFlags { @@ -60,22 +59,32 @@ public: PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL }; - const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; } +private: + PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR; + PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL; + RID map; + Vector3 start_position; + Vector3 target_position; + uint32_t navigation_layers = 1; + BitField metadata_flags = PATH_METADATA_INCLUDE_ALL; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; +public: void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm); PathfindingAlgorithm get_pathfinding_algorithm() const; void set_path_postprocessing(const PathPostProcessing p_path_postprocessing); PathPostProcessing get_path_postprocessing() const; - void set_map(const RID &p_map); - const RID &get_map() const; + void set_map(RID p_map); + RID get_map() const; - void set_start_position(const Vector3 &p_start_position); - const Vector3 &get_start_position() const; + void set_start_position(Vector3 p_start_position); + Vector3 get_start_position() const; - void set_target_position(const Vector3 &p_target_position); - const Vector3 &get_target_position() const; + void set_target_position(Vector3 p_target_position); + Vector3 get_target_position() const; void set_navigation_layers(uint32_t p_navigation_layers); uint32_t get_navigation_layers() const; diff --git a/servers/navigation/navigation_path_query_result_2d.h b/servers/navigation/navigation_path_query_result_2d.h index 856219f998d..1320d15ea2e 100644 --- a/servers/navigation/navigation_path_query_result_2d.h +++ b/servers/navigation/navigation_path_query_result_2d.h @@ -47,8 +47,8 @@ protected: public: enum PathSegmentType { - PATH_SEGMENT_TYPE_REGION = 0, - PATH_SEGMENT_TYPE_LINK = 1, + PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION, + PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK, }; void set_path(const Vector &p_path); diff --git a/servers/navigation/navigation_path_query_result_3d.h b/servers/navigation/navigation_path_query_result_3d.h index fd8545a0c93..8c4e89b9b01 100644 --- a/servers/navigation/navigation_path_query_result_3d.h +++ b/servers/navigation/navigation_path_query_result_3d.h @@ -48,8 +48,8 @@ protected: public: enum PathSegmentType { - PATH_SEGMENT_TYPE_REGION = 0, - PATH_SEGMENT_TYPE_LINK = 1, + PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION, + PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK, }; void set_path(const Vector &p_path); diff --git a/servers/navigation/navigation_utilities.h b/servers/navigation/navigation_utilities.h index 7ae22b1d3a2..36192c140fc 100644 --- a/servers/navigation/navigation_utilities.h +++ b/servers/navigation/navigation_utilities.h @@ -43,6 +43,7 @@ enum PathfindingAlgorithm { enum PathPostProcessing { PATH_POSTPROCESSING_CORRIDORFUNNEL = 0, PATH_POSTPROCESSING_EDGECENTERED, + PATH_POSTPROCESSING_NONE, }; enum PathSegmentType { @@ -58,25 +59,6 @@ enum PathMetadataFlags { PATH_INCLUDE_ALL = PATH_INCLUDE_TYPES | PATH_INCLUDE_RIDS | PATH_INCLUDE_OWNERS }; -struct PathQueryParameters { - PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR; - PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL; - RID map; - Vector3 start_position; - Vector3 target_position; - uint32_t navigation_layers = 1; - BitField metadata_flags = PATH_INCLUDE_ALL; - bool simplify_path = false; - real_t simplify_epsilon = 0.0; -}; - -struct PathQueryResult { - PackedVector3Array path; - PackedInt32Array path_types; - TypedArray path_rids; - PackedInt64Array path_owner_ids; -}; - } //namespace NavigationUtilities #endif // NAVIGATION_UTILITIES_H diff --git a/servers/navigation_server_2d.compat.inc b/servers/navigation_server_2d.compat.inc new file mode 100644 index 00000000000..14205ef536b --- /dev/null +++ b/servers/navigation_server_2d.compat.inc @@ -0,0 +1,46 @@ +/**************************************************************************/ +/* navigation_server_2d.compat.inc */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef DISABLE_DEPRECATED + +Vector NavigationServer2D::_map_get_path_bind_compat_100129(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) const { + return const_cast(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers); +} + +void NavigationServer2D::_query_path_bind_compat_100129(const Ref &p_query_parameters, Ref p_query_result) const { + return const_cast(this)->query_path(p_query_parameters, p_query_result, Callable()); +} + +void NavigationServer2D::_bind_compatibility_methods() { + ClassDB::bind_compatibility_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize", "navigation_layers"), &NavigationServer2D::_map_get_path_bind_compat_100129, DEFVAL(1)); + ClassDB::bind_compatibility_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer2D::_query_path_bind_compat_100129); +} + +#endif // DISABLE_DEPRECATED diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index ceb5e909a3b..cd0dd7e3a04 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -29,6 +29,7 @@ /**************************************************************************/ #include "navigation_server_2d.h" +#include "navigation_server_2d.compat.inc" #include "servers/navigation_server_3d.h" @@ -62,7 +63,7 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer2D::map_get_random_point); - ClassDB::bind_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer2D::query_path); + ClassDB::bind_method(D_METHOD("query_path", "parameters", "result", "callback"), &NavigationServer2D::query_path, DEFVAL(Callable())); ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer2D::region_create); ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer2D::region_set_enabled); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index 250183300f1..e7c7cf06533 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -91,7 +91,7 @@ public: virtual real_t map_get_link_connection_radius(RID p_map) const = 0; /// Returns the navigation path to reach the destination from the origin. - virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const = 0; + virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0; virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const = 0; virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const = 0; @@ -293,7 +293,7 @@ public: virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const = 0; /// Returns a customized navigation path using a query parameters object - virtual void query_path(const Ref &p_query_parameters, Ref p_query_result) const = 0; + virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) = 0; virtual void init() = 0; virtual void sync() = 0; @@ -318,6 +318,14 @@ public: void set_debug_enabled(bool p_enabled); bool get_debug_enabled() const; +protected: +#ifndef DISABLE_DEPRECATED + Vector _map_get_path_bind_compat_100129(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const; + void _query_path_bind_compat_100129(const Ref &p_query_parameters, Ref p_query_result) const; + static void _bind_compatibility_methods(); +#endif + +public: #ifdef DEBUG_ENABLED void set_debug_navigation_enabled(bool p_enabled); bool get_debug_navigation_enabled() const; diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h index 5bc91830e64..c54e62f80e6 100644 --- a/servers/navigation_server_2d_dummy.h +++ b/servers/navigation_server_2d_dummy.h @@ -50,7 +50,7 @@ public: real_t map_get_edge_connection_margin(RID p_map) const override { return 0; } void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {} real_t map_get_link_connection_radius(RID p_map) const override { return 0; } - Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override { return Vector(); } + Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override { return Vector(); } Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override { return Vector2(); } RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override { return RID(); } TypedArray map_get_links(RID p_map) const override { return TypedArray(); } @@ -158,7 +158,7 @@ public: void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {} uint32_t obstacle_get_avoidance_layers(RID p_agent) const override { return 0; } - void query_path(const Ref &p_query_parameters, Ref p_query_result) const override {} + void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) override {} void init() override {} void sync() override {} diff --git a/servers/navigation_server_3d.compat.inc b/servers/navigation_server_3d.compat.inc new file mode 100644 index 00000000000..633e4d5ddff --- /dev/null +++ b/servers/navigation_server_3d.compat.inc @@ -0,0 +1,46 @@ +/**************************************************************************/ +/* navigation_server_3d.compat.inc */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef DISABLE_DEPRECATED + +Vector NavigationServer3D::_map_get_path_bind_compat_100129(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const { + return const_cast(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers); +} + +void NavigationServer3D::_query_path_bind_compat_100129(const Ref &p_query_parameters, Ref p_query_result) const { + return const_cast(this)->query_path(p_query_parameters, p_query_result, Callable()); +} + +void NavigationServer3D::_bind_compatibility_methods() { + ClassDB::bind_compatibility_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize", "navigation_layers"), &NavigationServer3D::_map_get_path_bind_compat_100129, DEFVAL(1)); + ClassDB::bind_compatibility_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer3D::_query_path_bind_compat_100129); +} + +#endif // DISABLE_DEPRECATED diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index 572309c4296..4c36df83f33 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -29,6 +29,7 @@ /**************************************************************************/ #include "navigation_server_3d.h" +#include "navigation_server_3d.compat.inc" #include "core/config/project_settings.h" #include "scene/main/node.h" @@ -72,7 +73,7 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer3D::map_get_random_point); - ClassDB::bind_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer3D::query_path); + ClassDB::bind_method(D_METHOD("query_path", "parameters", "result", "callback"), &NavigationServer3D::query_path, DEFVAL(Callable())); ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer3D::region_create); ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer3D::region_set_enabled); @@ -247,6 +248,8 @@ NavigationServer3D::NavigationServer3D() { GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true); GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true); + GLOBAL_DEF("navigation/pathfinding/max_threads", 4); + GLOBAL_DEF("navigation/baking/use_crash_prevention_checks", true); GLOBAL_DEF("navigation/baking/thread_model/baking_use_multiple_threads", true); GLOBAL_DEF("navigation/baking/thread_model/baking_use_high_priority_threads", true); @@ -935,18 +938,6 @@ bool NavigationServer3D::get_debug_avoidance_enabled() const { #endif // DEBUG_ENABLED -void NavigationServer3D::query_path(const Ref &p_query_parameters, Ref p_query_result) const { - ERR_FAIL_COND(!p_query_parameters.is_valid()); - ERR_FAIL_COND(!p_query_result.is_valid()); - - const NavigationUtilities::PathQueryResult _query_result = _query_path(p_query_parameters->get_parameters()); - - p_query_result->set_path(_query_result.path); - p_query_result->set_path_types(_query_result.path_types); - p_query_result->set_path_rids(_query_result.path_rids); - p_query_result->set_path_owner_ids(_query_result.path_owner_ids); -} - /////////////////////////////////////////////////////// NavigationServer3DCallback NavigationServer3DManager::create_callback = nullptr; diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index 6dbbd356481..df5f7658bb2 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -103,7 +103,7 @@ public: virtual real_t map_get_link_connection_radius(RID p_map) const = 0; /// Returns the navigation path to reach the destination from the origin. - virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const = 0; + virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0; virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const = 0; virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const = 0; @@ -343,9 +343,7 @@ public: virtual void finish() = 0; /// Returns a customized navigation path using a query parameters object - virtual void query_path(const Ref &p_query_parameters, Ref p_query_result) const; - - virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const = 0; + virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) = 0; #ifndef _3D_DISABLED virtual void parse_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) = 0; @@ -380,6 +378,13 @@ public: void set_debug_enabled(bool p_enabled); bool get_debug_enabled() const; +protected: +#ifndef DISABLE_DEPRECATED + Vector _map_get_path_bind_compat_100129(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const; + void _query_path_bind_compat_100129(const Ref &p_query_parameters, Ref p_query_result) const; + static void _bind_compatibility_methods(); +#endif + private: bool debug_enabled = false; diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h index 210c404365a..418f7b373d2 100644 --- a/servers/navigation_server_3d_dummy.h +++ b/servers/navigation_server_3d_dummy.h @@ -55,7 +55,7 @@ public: real_t map_get_edge_connection_margin(RID p_map) const override { return 0; } void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {} real_t map_get_link_connection_radius(RID p_map) const override { return 0; } - Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const override { return Vector(); } + Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) override { return Vector(); } Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const override { return Vector3(); } Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override { return Vector3(); } Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override { return Vector3(); } @@ -178,6 +178,8 @@ public: void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {} uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override { return 0; } + virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) override {} + #ifndef _3D_DISABLED void parse_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override {} void bake_from_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override {} @@ -197,7 +199,6 @@ public: void sync() override {} void finish() override {} - NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override { return NavigationUtilities::PathQueryResult(); } int get_process_info(ProcessInfo p_info) const override { return 0; } void set_debug_enabled(bool p_enabled) {}