diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml
index ce0a00bc83bb..1f9c064f930b 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 fcd913f73b59..a4c622d080d6 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 5c19a6b355db..d1fac97b93e7 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 66a286758bca..ab65f3f74376 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 0a16b159316c..ef3a96cb53cf 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 1bbe720eb944..8cade432e64d 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 2af125d43481..de78ea5826a8 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();
+
+ 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.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);
+ 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 1579ca290741..80fdea5ba9db 100644
--- a/modules/navigation/2d/godot_navigation_server_2d.h
+++ b/modules/navigation/2d/godot_navigation_server_2d.h
@@ -68,7 +68,7 @@ class GodotNavigationServer2D : public NavigationServer2D {
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 @@ class GodotNavigationServer2D : public NavigationServer2D {
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 5dfc39f6f5e0..80e87a4b2e8d 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;
-
- const NavMap *map = map_owner.get_or_null(p_parameters.map);
- ERR_FAIL_NULL_V(map, r_query_result);
-
- // 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);
- }
- }
+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());
- // add path stats
+ NavMap *map = map_owner.get_or_null(p_query_parameters->get_map());
+ ERR_FAIL_NULL(map);
- return r_query_result;
+ NavMeshQueries3D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
}
RID GodotNavigationServer3D::source_geometry_parser_create() {
@@ -1490,84 +1436,30 @@ 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);
-
- uint32_t indices_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]];
- }
-
- 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;
+ 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];
}
}
- 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];
+ LocalVector simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(source_path, p_epsilon);
- Vector3 path_segment[2] = { start_point, end_point };
+ uint32_t index_count = simplified_path_indices.size();
- 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;
+ Vector simplified_path;
+ {
+ 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]];
}
}
- 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);
- }
+ return simplified_path;
}
int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const {
diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h
index eae6ea28608a..ce8d333535f2 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 @@ class GodotNavigationServer3D : public NavigationServer3D {
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 @@ class GodotNavigationServer3D : public NavigationServer3D {
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 @@ class GodotNavigationServer3D : public NavigationServer3D {
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 fa7ccbeb701a..b60d8d073dca 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();
+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();
}
- if (r_path_rids) {
- r_path_rids->clear();
+
+ 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 (r_path_owners) {
- r_path_owners->clear();
+
+ 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();
}
- // Find the start poly and the end poly on this 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;
+ 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];
}
+ }
- // 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);
+ 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];
+ }
+ }
- 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;
- }
+ 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);
- 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;
- }
+ 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;
+
+ _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();
- }
+ _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
+ return;
+ }
- 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();
- }
+ _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;
+ }
- 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();
- }
+ 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());
+ }
- Vector path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
+ 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;
+ }
+}
+
+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;
+ }
+
+ const LocalVector &simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon);
- 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();
+ 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);
+ }
- 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 (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 (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();
+ 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);
+ }
- Vector path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
+ 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);
}
+}
- 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];
+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;
- 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];
- }
+ // Set the apex poly/point to the end point
+ gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id];
+
+ 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];
}
+ }
- Vector3 apex_point = end_point;
+ Vector3 apex_point = p_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 *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;
+ gd::NavigationPoly *p = apex_poly;
- path.push_back(end_point);
- APPEND_METADATA(end_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);
- }
+ 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;
- }
- }
+ 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);
- 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 = right_portal;
+ p = right_poly;
+ left_poly = p;
+ apex_poly = p;
+ left_portal = apex_point;
+ right_portal = apex_point;
- 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);
- path.push_back(apex_point);
- APPEND_METADATA(apex_poly->poly);
- }
+ skip = true;
}
+ }
- // Go to the previous polygon.
- if (p->back_navigation_poly_id != -1) {
- p = &navigation_polys[p->back_navigation_poly_id];
+ 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 {
- // The end
- p = nullptr;
+ 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);
}
}
- // 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);
+ // 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;
}
+ }
- 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 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);
+ }
+}
- } else {
- path.push_back(end_point);
- APPEND_METADATA(end_poly);
-
- // 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;
-
- 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);
- }
+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;
- np_id = navigation_polys[np_id].back_navigation_poly_id;
- }
+ _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
- path.push_back(begin_point);
- APPEND_METADATA(begin_poly);
+ // 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;
- 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();
+ _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;
}
- // 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());
+ _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;
- return path;
+ _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 109bb2f97123..a92520f9444e 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 739b031d061d..07ee96dc321f 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 feeb2cc54834..ce247b83c164 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 @@ class NavMap : public NavRid {
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 993a97638dd7..ea22fb0bab3d 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 f030473c4b74..786598f01a3a 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 faf138896a52..d5e162d41691 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 6c1f88e3490c..74aaf64b4e09 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 a1d5f2d109a4..91031bfc180f 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 @@ class NavigationPathQueryParameters2D : public RefCounted {
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 b0a5b0ad82b1..99c5318bed2d 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 2eb85db78757..66cad8dcd543 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 @@ class NavigationPathQueryParameters3D : public RefCounted {
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 856219f998d2..1320d15ea2e1 100644
--- a/servers/navigation/navigation_path_query_result_2d.h
+++ b/servers/navigation/navigation_path_query_result_2d.h
@@ -47,8 +47,8 @@ class NavigationPathQueryResult2D : public RefCounted {
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 fd8545a0c933..8c4e89b9b017 100644
--- a/servers/navigation/navigation_path_query_result_3d.h
+++ b/servers/navigation/navigation_path_query_result_3d.h
@@ -48,8 +48,8 @@ class NavigationPathQueryResult3D : public RefCounted {
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 7ae22b1d3a28..36192c140fc0 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 000000000000..14205ef536b0
--- /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 ceb5e909a3b9..cd0dd7e3a042 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 250183300f1d..e7c7cf065331 100644
--- a/servers/navigation_server_2d.h
+++ b/servers/navigation_server_2d.h
@@ -91,7 +91,7 @@ class NavigationServer2D : public Object {
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 @@ class NavigationServer2D : public Object {
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 @@ class NavigationServer2D : public Object {
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 5bc91830e642..c54e62f80e6d 100644
--- a/servers/navigation_server_2d_dummy.h
+++ b/servers/navigation_server_2d_dummy.h
@@ -50,7 +50,7 @@ class NavigationServer2DDummy : public NavigationServer2D {
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 @@ class NavigationServer2DDummy : public NavigationServer2D {
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 000000000000..633e4d5ddff0
--- /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 572309c42963..4c36df83f334 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 6dbbd3564810..df5f7658bb2a 100644
--- a/servers/navigation_server_3d.h
+++ b/servers/navigation_server_3d.h
@@ -103,7 +103,7 @@ class NavigationServer3D : public Object {
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 @@ class NavigationServer3D : public Object {
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 @@ class NavigationServer3D : public Object {
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 210c404365a2..418f7b373d25 100644
--- a/servers/navigation_server_3d_dummy.h
+++ b/servers/navigation_server_3d_dummy.h
@@ -55,7 +55,7 @@ class NavigationServer3DDummy : public NavigationServer3D {
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 @@ class NavigationServer3DDummy : public NavigationServer3D {
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 @@ class NavigationServer3DDummy : public NavigationServer3D {
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) {}