mirrors/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-01-22 10:32:54 -05:00
Code Issues Projects Releases Packages Wiki Activity

Despaghettify NavigationServer path queries

Browse source 
Despaghettify NavigationServer path queries.
This commit is contained in:
smix8 2024-11-29 18:22:26 +01:00
parent c2e4ae782a
commit 476479419b
32 changed files with 984 additions and 655 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
doc/classes/NavigationPathQueryParameters2D.xml
View file

@ -49,6 +49,9 @@
<constant name="PATH_POSTPROCESSING_EDGECENTERED" value="1" enum="PathPostProcessing">
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.
</constant>
<constant name="PATH_POSTPROCESSING_NONE" value="2" enum="PathPostProcessing">
Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm.
</constant>
<constant name="PATH_METADATA_INCLUDE_NONE" value="0" enum="PathMetadataFlags" is_bitfield="true">
Don't include any additional metadata about the returned path.
</constant>

3
doc/classes/NavigationPathQueryParameters3D.xml
View file

@ -49,6 +49,9 @@
<constant name="PATH_POSTPROCESSING_EDGECENTERED" value="1" enum="PathPostProcessing">
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.
</constant>
<constant name="PATH_POSTPROCESSING_NONE" value="2" enum="PathPostProcessing">
Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm.
</constant>
<constant name="PATH_METADATA_INCLUDE_NONE" value="0" enum="PathMetadataFlags" is_bitfield="true">
Don't include any additional metadata about the returned path.
</constant>

7
doc/classes/NavigationServer2D.xml
View file

@ -533,7 +533,7 @@
Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map].
</description>
</method>
<method name="map_get_path" qualifiers="const">
<method name="map_get_path">
<return type="PackedVector2Array" />
<param index="0" name="map" type="RID" />
<param index="1" name="origin" type="Vector2" />
@ -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.
</description>
</method>
<method name="query_path" qualifiers="const">
<method name="query_path">
<return type="void" />
<param index="0" name="parameters" type="NavigationPathQueryParameters2D" />
<param index="1" name="result" type="NavigationPathQueryResult2D" />
<param index="2" name="callback" type="Callable" default="Callable()" />
<description>
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.
</description>
</method>
<method name="region_create">

7
doc/classes/NavigationServer3D.xml
View file

@ -605,7 +605,7 @@
Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map].
</description>
</method>
<method name="map_get_path" qualifiers="const">
<method name="map_get_path">
<return type="PackedVector3Array" />
<param index="0" name="map" type="RID" />
<param index="1" name="origin" type="Vector3" />
@ -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.
</description>
</method>
<method name="query_path" qualifiers="const">
<method name="query_path">
<return type="void" />
<param index="0" name="parameters" type="NavigationPathQueryParameters3D" />
<param index="1" name="result" type="NavigationPathQueryResult3D" />
<param index="2" name="callback" type="Callable" default="Callable()" />
<description>
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.
</description>
</method>
<method name="region_bake_navigation_mesh" deprecated="This method is deprecated due to core threading changes. To upgrade existing code, first create a [NavigationMeshSourceGeometryData3D] resource. Use this resource with [method parse_source_geometry_data] to parse the [SceneTree] for nodes that should contribute to the navigation mesh baking. The [SceneTree] parsing needs to happen on the main thread. After the parsing is finished use the resource with [method bake_from_source_geometry_data] to bake a navigation mesh.">

3
doc/classes/ProjectSettings.xml
View file

@ -2162,6 +2162,9 @@
<member name="navigation/baking/use_crash_prevention_checks" type="bool" setter="" getter="" default="true">
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.
</member>
<member name="navigation/pathfinding/max_threads" type="int" setter="" getter="" default="4">
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.
</member>
<member name="network/limits/debugger/max_chars_per_second" type="int" setter="" getter="" default="32768">
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.
</member>

13
misc/extension_api_validation/4.3-stable.expected
View file

@ -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.

54
modules/navigation/2d/godot_navigation_server_2d.cpp
View file

@ -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<Vector2> 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<Vector2> 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<Vector2> 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<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> 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<NavigationPathQueryParameters3D> query_parameters;
query_parameters.instantiate();
p_query_result->set_path(vector_v3_to_v2(_query_result.path));
p_query_result->set_path_types(_query_result.path_types);
p_query_result->set_path_rids(_query_result.path_rids);
p_query_result->set_path_owner_ids(_query_result.path_owner_ids);
query_parameters->set_map(p_query_parameters->get_map());
query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position()));
query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position()));
query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers());
query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
switch (p_query_parameters->get_path_postprocessing()) {
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
} break;
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED);
} break;
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE);
} break;
default: {
WARN_PRINT("No match for used PathPostProcessing - fallback to default");
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
} break;
}
query_parameters->set_metadata_flags((int64_t)p_query_parameters->get_metadata_flags());
query_parameters->set_simplify_path(p_query_parameters->get_simplify_path());
query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon());
Ref<NavigationPathQueryResult3D> query_result;
query_result.instantiate();
NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback);
p_query_result->set_path(vector_v3_to_v2(query_result->get_path()));
p_query_result->set_path_types(query_result->get_path_types());
p_query_result->set_path_rids(query_result->get_path_rids());
p_query_result->set_path_owner_ids(query_result->get_path_owner_ids());
}
RID GodotNavigationServer2D::source_geometry_parser_create() {

4
modules/navigation/2d/godot_navigation_server_2d.h
View file

@ -68,7 +68,7 @@ public:
virtual real_t map_get_edge_connection_margin(RID p_map) const override;
virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override;
virtual real_t map_get_link_connection_radius(RID p_map) const override;
virtual Vector<Vector2> 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<Vector2> 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<RID> map_get_links(RID p_map) const override;
@ -242,7 +242,7 @@ public:
virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) override;
virtual void init() override;
virtual void sync() override;

196
modules/navigation/3d/godot_navigation_server_3d.cpp
View file

@ -237,11 +237,29 @@ real_t GodotNavigationServer3D::map_get_link_connection_radius(RID p_map) const
return map->get_link_connection_radius();
}
Vector<Vector3> GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
Vector<Vector3> 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<Vector3>());
return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers, nullptr, nullptr, nullptr);
Ref<NavigationPathQueryParameters3D> 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<NavigationPathQueryResult3D> query_result;
query_result.instantiate();
query_path(query_parameters, query_result);
return query_result->get_path();
}
Vector3 GodotNavigationServer3D::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@ -1384,86 +1402,14 @@ void GodotNavigationServer3D::finish() {
#endif // _3D_DISABLED
}
PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters &p_parameters) const {
PathQueryResult r_query_result;
void GodotNavigationServer3D::query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
ERR_FAIL_COND(p_query_parameters.is_null());
ERR_FAIL_COND(p_query_result.is_null());
const NavMap *map = map_owner.get_or_null(p_parameters.map);
ERR_FAIL_NULL_V(map, r_query_result);
NavMap *map = map_owner.get_or_null(p_query_parameters->get_map());
ERR_FAIL_NULL(map);
// run the pathfinding
if (p_parameters.pathfinding_algorithm == PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
// while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing
if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
r_query_result.path = map->get_path(
p_parameters.start_position,
p_parameters.target_position,
true,
p_parameters.navigation_layers,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
} else if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
r_query_result.path = map->get_path(
p_parameters.start_position,
p_parameters.target_position,
false,
p_parameters.navigation_layers,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
}
} else {
return r_query_result;
}
// add path postprocessing
if (r_query_result.path.size() > 2 && p_parameters.simplify_path) {
const LocalVector<uint32_t> &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<RID> simplified_path_rids;
simplified_path_rids.resize(indices_count);
for (uint32_t i = 0; i < indices_count; i++) {
simplified_path_rids[i] = r_query_result.path_rids[i];
}
r_query_result.path_rids = simplified_path_rids;
}
if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
int64_t *w = r_query_result.path_owner_ids.ptrw();
const int64_t *r = r_query_result.path_owner_ids.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
r_query_result.path_owner_ids.resize(indices_count);
}
}
// add path stats
return r_query_result;
NavMeshQueries3D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
}
RID GodotNavigationServer3D::source_geometry_parser_create() {
@ -1490,86 +1436,32 @@ Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_
p_epsilon = MAX(0.0, p_epsilon);
LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon);
LocalVector<Vector3> source_path;
{
source_path.resize(p_path.size());
const Vector3 *r = p_path.ptr();
for (uint32_t i = 0; i < p_path.size(); i++) {
source_path[i] = r[i];
}
}
uint32_t indices_count = simplified_path_indices.size();
LocalVector<uint32_t> simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(source_path, p_epsilon);
uint32_t index_count = simplified_path_indices.size();
Vector<Vector3> simplified_path;
simplified_path.resize(indices_count);
Vector3 *w = simplified_path.ptrw();
const Vector3 *r = p_path.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
{
simplified_path.resize(index_count);
Vector3 *w = simplified_path.ptrw();
const Vector3 *r = source_path.ptr();
for (uint32_t i = 0; i < index_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
}
return simplified_path;
}
LocalVector<uint32_t> GodotNavigationServer3D::get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon) {
p_epsilon = MAX(0.0, p_epsilon);
real_t squared_epsilon = p_epsilon * p_epsilon;
LocalVector<bool> 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<uint32_t> simplified_path_indices;
simplified_path_indices.resize(valid_point_index);
valid_point_index = 0;
for (uint32_t i = 0; i < valid_points.size(); i++) {
if (valid_points[i]) {
simplified_path_indices[valid_point_index] = i;
valid_point_index += 1;
}
}
return simplified_path_indices;
}
void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) {
r_valid_points[p_start_inx] = true;
r_valid_points[p_end_inx] = true;
const Vector3 &start_point = p_points[p_start_inx];
const Vector3 &end_point = p_points[p_end_inx];
Vector3 path_segment[2] = { start_point, end_point };
real_t point_max_distance = 0.0;
int point_max_index = 0;
for (int i = p_start_inx; i < p_end_inx; i++) {
const Vector3 &checked_point = p_points[i];
const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment);
real_t distance_squared = closest_point.distance_squared_to(checked_point);
if (distance_squared > point_max_distance) {
point_max_index = i;
point_max_distance = distance_squared;
}
}
if (point_max_distance > p_epsilon) {
simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points);
simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points);
}
}
int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const {
switch (p_info) {
case INFO_ACTIVE_MAPS: {

10
modules/navigation/3d/godot_navigation_server_3d.h
View file

@ -40,6 +40,8 @@
#include "core/templates/local_vector.h"
#include "core/templates/rid.h"
#include "core/templates/rid_owner.h"
#include "servers/navigation/navigation_path_query_parameters_3d.h"
#include "servers/navigation/navigation_path_query_result_3d.h"
#include "servers/navigation_server_3d.h"
/// The commands are functions executed during the `sync` phase.
@ -130,7 +132,7 @@ public:
COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
virtual real_t map_get_link_connection_radius(RID p_map) const override;
virtual Vector<Vector3> 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<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override;
@ -273,10 +275,6 @@ public:
virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
private:
static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
static LocalVector<uint32_t> get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon);
public:
COMMAND_1(free, RID, p_object);
@ -288,7 +286,7 @@ public:
virtual void sync() override;
virtual void finish() override;
virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override;
int get_process_info(ProcessInfo p_info) const override;

777
modules/navigation/3d/nav_mesh_queries_3d.cpp
View file

@ -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<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
const LocalVector<gd::Polygon> &region_polygons = p_polygons;
@ -127,87 +128,225 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
}
}
Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
// Clear metadata outputs.
if (r_path_types) {
r_path_types->clear();
}
if (r_path_rids) {
r_path_rids->clear();
}
if (r_path_owners) {
r_path_owners->clear();
void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
p_query_task.path_meta_point_types.resize(2);
p_query_task.path_meta_point_types[0] = begin_poly->owner->get_type();
p_query_task.path_meta_point_types[1] = end_poly->owner->get_type();
}
// Find the start poly and the end poly on this map.
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
p_query_task.path_meta_point_rids.resize(2);
p_query_task.path_meta_point_rids[0] = begin_poly->owner->get_self();
p_query_task.path_meta_point_rids[1] = end_poly->owner->get_self();
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
p_query_task.path_meta_point_owners.resize(2);
p_query_task.path_meta_point_owners[0] = begin_poly->owner->get_owner_id();
p_query_task.path_meta_point_owners[1] = end_poly->owner->get_owner_id();
}
p_query_task.path_points.resize(2);
p_query_task.path_points[0] = begin_point;
p_query_task.path_points[1] = end_point;
}
void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) {
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
}
p_query_task.path_points.push_back(p_point);
}
void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> 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<Vector3> path_points;
Vector<int32_t> path_meta_point_types;
TypedArray<RID> path_meta_point_rids;
Vector<int64_t> path_meta_point_owners;
{
path_points.resize(path_point_size);
Vector3 *w = path_points.ptrw();
const Vector3 *r = query_task.path_points.ptr();
for (uint32_t i = 0; i < path_point_size; i++) {
w[i] = r[i];
}
}
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
path_meta_point_types.resize(path_point_size);
int32_t *w = path_meta_point_types.ptrw();
const int32_t *r = query_task.path_meta_point_types.ptr();
for (uint32_t i = 0; i < path_point_size; i++) {
w[i] = r[i];
}
}
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
path_meta_point_rids.resize(path_point_size);
for (uint32_t i = 0; i < path_point_size; i++) {
path_meta_point_rids[i] = query_task.path_meta_point_rids[i];
}
}
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
path_meta_point_owners.resize(path_point_size);
int64_t *w = path_meta_point_owners.ptrw();
const int64_t *r = query_task.path_meta_point_owners.ptr();
for (uint32_t i = 0; i < path_point_size; i++) {
w[i] = r[i];
}
}
p_query_result->set_path(path_points);
p_query_result->set_path_types(path_meta_point_types);
p_query_result->set_path_rids(path_meta_point_rids);
p_query_result->set_path_owner_ids(path_meta_point_owners);
if (query_task.callback.is_valid()) {
if (emit_callback(query_task.callback)) {
query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_DISPATCHED;
} else {
query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_FAILED;
}
}
}
void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
p_query_task.path_points.clear();
p_query_task.path_meta_point_types.clear();
p_query_task.path_meta_point_rids.clear();
p_query_task.path_meta_point_owners.clear();
// Find begin polyon and begin position closest to start position and
// end polyon and end position closest to target position on the map.
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
Vector3 begin_point;
Vector3 end_point;
real_t begin_d = FLT_MAX;
real_t end_d = FLT_MAX;
// Find the initial poly and the end poly on this map.
for (const gd::Polygon &p : p_polygons) {
// Only consider the polygon if it in a region with compatible layers.
if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) {
continue;
}
// For each face check the distance between the origin/destination
for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 point = face.get_closest_point_to(p_origin);
real_t distance_to_point = point.distance_to(p_origin);
if (distance_to_point < begin_d) {
begin_d = distance_to_point;
begin_poly = &p;
begin_point = point;
}
point = face.get_closest_point_to(p_destination);
distance_to_point = point.distance_to(p_destination);
if (distance_to_point < end_d) {
end_d = distance_to_point;
end_poly = &p;
end_point = point;
}
}
}
_query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point);
// Check for trivial cases
if (!begin_poly || !end_poly) {
return Vector<Vector3>();
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
return;
}
if (begin_poly == end_poly) {
if (r_path_types) {
r_path_types->resize(2);
r_path_types->write[0] = begin_poly->owner->get_type();
r_path_types->write[1] = end_poly->owner->get_type();
}
if (r_path_rids) {
r_path_rids->resize(2);
(*r_path_rids)[0] = begin_poly->owner->get_self();
(*r_path_rids)[1] = end_poly->owner->get_self();
}
if (r_path_owners) {
r_path_owners->resize(2);
r_path_owners->write[0] = begin_poly->owner->get_owner_id();
r_path_owners->write[1] = end_poly->owner->get_owner_id();
}
Vector<Vector3> path;
path.resize(2);
path.write[0] = begin_point;
path.write[1] = end_point;
return path;
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
return;
}
_query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point);
// Post-Process path.
switch (p_query_task.path_postprocessing) {
case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
} break;
case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
_path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
} break;
case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
_path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
} break;
default: {
WARN_PRINT("No match for used PathPostProcessing - fallback to default");
_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
} break;
}
p_query_task.path_points.invert();
p_query_task.path_meta_point_types.invert();
p_query_task.path_meta_point_rids.invert();
p_query_task.path_meta_point_owners.invert();
if (p_query_task.simplify_path) {
_query_task_simplified_path_points(p_query_task);
}
#ifdef DEBUG_ENABLED
// Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
}
#endif // DEBUG_ENABLED
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
}
void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &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<gd::NavigationPoly> navigation_polys;
navigation_polys.resize(p_polygons.size() + p_link_polygons_size);
LocalVector<gd::NavigationPoly> &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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
// Heap of polygons to travel next.
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
while (true) {
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
// it is shorter, update the polygon.
if (neighbor_poly.traversable_poly_index < traversable_polys.size() &&
new_traveled_distance < neighbor_poly.traveled_distance) {
neighbor_poly.back_navigation_poly_id = least_cost_id;
neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
neighbor_poly.back_navigation_edge = connection.edge;
neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
@ -278,7 +418,7 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
} else {
// Initialize the matching navigation polygon.
neighbor_poly.poly = connection.polygon;
neighbor_poly.back_navigation_poly_id = least_cost_id;
neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
neighbor_poly.back_navigation_edge = connection.edge;
neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
@ -307,11 +447,11 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
// Set as end point the furthest reachable point.
end_poly = reachable_end;
end_d = FLT_MAX;
real_t end_d = FLT_MAX;
for (size_t point_id = 2; point_id < end_poly->points.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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
bool closest_point_on_start_poly = false;
for (size_t point_id = 2; point_id < begin_poly->points.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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
}
if (closest_point_on_start_poly) {
// No point to run PostProcessing when start and end convex polygon is the same.
if (r_path_types) {
r_path_types->resize(2);
r_path_types->write[0] = begin_poly->owner->get_type();
r_path_types->write[1] = begin_poly->owner->get_type();
}
if (r_path_rids) {
r_path_rids->resize(2);
(*r_path_rids)[0] = begin_poly->owner->get_self();
(*r_path_rids)[1] = begin_poly->owner->get_self();
}
if (r_path_owners) {
r_path_owners->resize(2);
r_path_owners->write[0] = begin_poly->owner->get_owner_id();
r_path_owners->write[1] = begin_poly->owner->get_owner_id();
}
Vector<Vector3> 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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
}
navigation_polys[begin_poly->id].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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
}
// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
least_cost_id = traversable_polys.pop()->poly->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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
// We did not find a route but we have both a start polygon and an end polygon at this point.
// Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
if (!found_route) {
end_d = FLT_MAX;
real_t end_d = FLT_MAX;
// Search all faces of the start polygon for the closest point to our target position.
for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
Vector3 spoint = f.get_closest_point_to(p_destination);
real_t dpoint = spoint.distance_to(p_destination);
Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
real_t dpoint = spoint.distance_to(p_query_task.target_position);
if (dpoint < end_d) {
end_point = spoint;
end_d = dpoint;
}
}
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point);
return;
}
}
if (r_path_types) {
r_path_types->resize(2);
r_path_types->write[0] = begin_poly->owner->get_type();
r_path_types->write[1] = begin_poly->owner->get_type();
}
if (r_path_rids) {
r_path_rids->resize(2);
(*r_path_rids)[0] = begin_poly->owner->get_self();
(*r_path_rids)[1] = begin_poly->owner->get_self();
}
if (r_path_owners) {
r_path_owners->resize(2);
r_path_owners->write[0] = begin_poly->owner->get_owner_id();
r_path_owners->write[1] = begin_poly->owner->get_owner_id();
}
Vector<Vector3> path;
path.resize(2);
path.write[0] = begin_point;
path.write[1] = end_point;
return path;
void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
return;
}
Vector<Vector3> path;
// Optimize the path.
if (p_optimize) {
// Set the apex poly/point to the end point
gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
const LocalVector<uint32_t> &simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon);
Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
if (end_point.is_equal_approx(back_edge_closest_point)) {
// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
if (apex_poly->back_navigation_poly_id != -1) {
apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
}
uint32_t index_count = simplified_path_indices.size();
{
Vector3 *points_ptr = p_query_task.path_points.ptr();
for (uint32_t i = 0; i < index_count; i++) {
points_ptr[i] = points_ptr[simplified_path_indices[i]];
}
p_query_task.path_points.resize(index_count);
}
Vector3 apex_point = end_point;
gd::NavigationPoly *left_poly = apex_poly;
Vector3 left_portal = apex_point;
gd::NavigationPoly *right_poly = apex_poly;
Vector3 right_portal = apex_point;
gd::NavigationPoly *p = apex_poly;
path.push_back(end_point);
APPEND_METADATA(end_poly);
while (p) {
// Set left and right points of the pathway between polygons.
Vector3 left = p->back_navigation_edge_pathway_start;
Vector3 right = p->back_navigation_edge_pathway_end;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
SWAP(left, right);
}
bool skip = false;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
//process
if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
left_poly = p;
left_portal = left;
} else {
clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners, p_map_up);
apex_point = right_portal;
p = right_poly;
left_poly = p;
apex_poly = p;
left_portal = apex_point;
right_portal = apex_point;
path.push_back(apex_point);
APPEND_METADATA(apex_poly->poly);
skip = true;
}
}
if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
//process
if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
right_poly = p;
right_portal = right;
} else {
clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners, p_map_up);
apex_point = left_portal;
p = left_poly;
right_poly = p;
apex_poly = p;
right_portal = apex_point;
left_portal = apex_point;
path.push_back(apex_point);
APPEND_METADATA(apex_poly->poly);
}
}
// Go to the previous polygon.
if (p->back_navigation_poly_id != -1) {
p = &navigation_polys[p->back_navigation_poly_id];
} else {
// The end
p = nullptr;
}
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
int32_t *types_ptr = p_query_task.path_meta_point_types.ptr();
for (uint32_t i = 0; i < index_count; i++) {
types_ptr[i] = types_ptr[simplified_path_indices[i]];
}
p_query_task.path_meta_point_types.resize(index_count);
}
// If the last point is not the begin point, add it to the list.
if (path[path.size() - 1] != begin_point) {
path.push_back(begin_point);
APPEND_METADATA(begin_poly);
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
RID *rids_ptr = p_query_task.path_meta_point_rids.ptr();
for (uint32_t i = 0; i < index_count; i++) {
rids_ptr[i] = rids_ptr[simplified_path_indices[i]];
}
p_query_task.path_meta_point_rids.resize(index_count);
}
path.reverse();
if (r_path_types) {
r_path_types->reverse();
}
if (r_path_rids) {
r_path_rids->reverse();
}
if (r_path_owners) {
r_path_owners->reverse();
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr();
for (uint32_t i = 0; i < index_count; i++) {
owners_ptr[i] = owners_ptr[simplified_path_indices[i]];
}
p_query_task.path_meta_point_owners.resize(index_count);
}
}
} else {
path.push_back(end_point);
APPEND_METADATA(end_poly);
void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) {
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
// Add mid points
int np_id = least_cost_id;
while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
if (navigation_polys[np_id].back_navigation_edge != -1) {
int prev = navigation_polys[np_id].back_navigation_edge;
int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
// Set the apex poly/point to the end point
gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id];
path.push_back(point);
APPEND_METADATA(navigation_polys[np_id].poly);
} else {
path.push_back(navigation_polys[np_id].entry);
APPEND_METADATA(navigation_polys[np_id].poly);
}
np_id = navigation_polys[np_id].back_navigation_poly_id;
}
path.push_back(begin_point);
APPEND_METADATA(begin_poly);
path.reverse();
if (r_path_types) {
r_path_types->reverse();
}
if (r_path_rids) {
r_path_rids->reverse();
}
if (r_path_owners) {
r_path_owners->reverse();
Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway);
if (p_end_point.is_equal_approx(back_edge_closest_point)) {
// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
if (apex_poly->back_navigation_poly_id != -1) {
apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id];
}
}
// Ensure post conditions (path arrays MUST match in size).
CRASH_COND(r_path_types && path.size() != r_path_types->size());
CRASH_COND(r_path_rids && path.size() != r_path_rids->size());
CRASH_COND(r_path_owners && path.size() != r_path_owners->size());
Vector3 apex_point = p_end_point;
return path;
gd::NavigationPoly *left_poly = apex_poly;
Vector3 left_portal = apex_point;
gd::NavigationPoly *right_poly = apex_poly;
Vector3 right_portal = apex_point;
gd::NavigationPoly *p = apex_poly;
_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
while (p) {
// Set left and right points of the pathway between polygons.
Vector3 left = p->back_navigation_edge_pathway_start;
Vector3 right = p->back_navigation_edge_pathway_end;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
SWAP(left, right);
}
bool skip = false;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
//process
if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
left_poly = p;
left_portal = left;
} else {
clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up);
apex_point = right_portal;
p = right_poly;
left_poly = p;
apex_poly = p;
left_portal = apex_point;
right_portal = apex_point;
_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
skip = true;
}
}
if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
//process
if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
right_poly = p;
right_portal = right;
} else {
clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up);
apex_point = left_portal;
p = left_poly;
right_poly = p;
apex_poly = p;
right_portal = apex_point;
left_portal = apex_point;
_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
}
}
// Go to the previous polygon.
if (p->back_navigation_poly_id != -1) {
p = &p_path_corridor[p->back_navigation_poly_id];
} else {
// The end
p = nullptr;
}
}
// If the last point is not the begin point, add it to the list.
if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) {
_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
}
}
void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
// Add mid points.
int np_id = p_least_cost_id;
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
if (p_path_corridor[np_id].back_navigation_edge != -1) {
int prev = p_path_corridor[np_id].back_navigation_edge;
int prev_n = (p_path_corridor[np_id].back_navigation_edge + 1) % p_path_corridor[np_id].poly->points.size();
Vector3 point = (p_path_corridor[np_id].poly->points[prev].pos + p_path_corridor[np_id].poly->points[prev_n].pos) * 0.5;
_query_task_push_back_point_with_metadata(p_query_task, point, p_path_corridor[np_id].poly);
} else {
_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
}
np_id = p_path_corridor[np_id].back_navigation_poly_id;
}
_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
}
void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
// Add mid points.
int np_id = p_least_cost_id;
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
np_id = p_path_corridor[np_id].back_navigation_poly_id;
}
_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
}
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &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<gd::Polygon> &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<gd::Pol
return cp.owner;
}
void NavMeshQueries3D::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up) {
Vector3 from = path[path.size() - 1];
void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &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<gd::NavigationPoly> &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<uint32_t> NavMeshQueries3D::get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon) {
p_epsilon = MAX(0.0, p_epsilon);
real_t squared_epsilon = p_epsilon * p_epsilon;
LocalVector<bool> 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<uint32_t> 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<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &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

77
modules/navigation/3d/nav_mesh_queries_3d.h
View file

@ -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<gd::NavigationPoly> path_corridor;
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer> 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<PathMetadataFlags> 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<Vector3> path_points;
LocalVector<int32_t> path_meta_point_types;
LocalVector<RID> path_meta_point_rids;
LocalVector<int64_t> path_meta_point_owners;
Ref<NavigationPathQueryParameters3D> query_parameters;
Ref<NavigationPathQueryResult3D> 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<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
static Vector<Vector3> polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size);
static Vector3 polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
static Vector3 polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static Vector3 polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up);
static void map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &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<gd::Polygon> &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<gd::Polygon> &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<gd::NavigationPoly> &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<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
};
#endif // _3D_DISABLED

67
modules/navigation/nav_map.cpp
View file

@ -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<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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<Vector3>();
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() {

9
modules/navigation/nav_map.h
View file

@ -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<NavObstacle>::List obstacles;
} sync_dirty_requests;
LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
int path_query_slots_max = 4;
Mutex path_query_slots_mutex;
Semaphore path_query_slots_semaphore;
public:
NavMap();
~NavMap();
@ -176,7 +182,8 @@ public:
gd::PointKey get_point_key(const Vector3 &p_pos) const;
Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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;

9
modules/navigation/nav_utils.h
View file

@ -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 {

2
scene/2d/navigation_agent_2d.cpp
View file

@ -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");

2
scene/3d/navigation_agent_3d.cpp
View file

@ -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");

80
servers/navigation/navigation_path_query_parameters_2d.cpp
View file

@ -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<NavigationPathQueryParameters2D::PathMetadataFlags> p_flags) {
parameters.metadata_flags = (int64_t)p_flags;
metadata_flags = (int64_t)p_flags;
}
BitField<NavigationPathQueryParameters2D::PathMetadataFlags> 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);

25
servers/navigation/navigation_path_query_parameters_2d.h
View file

@ -37,19 +37,18 @@
class NavigationPathQueryParameters2D : public RefCounted {
GDCLASS(NavigationPathQueryParameters2D, RefCounted);
NavigationUtilities::PathQueryParameters parameters;
protected:
static void _bind_methods();
public:
enum PathfindingAlgorithm {
PATHFINDING_ALGORITHM_ASTAR = 0,
PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR,
};
enum PathPostProcessing {
PATH_POSTPROCESSING_CORRIDORFUNNEL = 0,
PATH_POSTPROCESSING_EDGECENTERED,
PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL,
PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED,
PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE,
};
enum PathMetadataFlags {
@ -60,16 +59,26 @@ public:
PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL
};
const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; }
private:
PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR;
PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL;
RID map;
Vector2 start_position;
Vector2 target_position;
uint32_t navigation_layers = 1;
BitField<PathMetadataFlags> 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;

84
servers/navigation/navigation_path_query_parameters_3d.cpp
View file

@ -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<NavigationPathQueryParameters3D::PathMetadataFlags> p_flags) {
parameters.metadata_flags = (int64_t)p_flags;
metadata_flags = (int64_t)p_flags;
}
BitField<NavigationPathQueryParameters3D::PathMetadataFlags> 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);

33
servers/navigation/navigation_path_query_parameters_3d.h
View file

@ -37,19 +37,18 @@
class NavigationPathQueryParameters3D : public RefCounted {
GDCLASS(NavigationPathQueryParameters3D, RefCounted);
NavigationUtilities::PathQueryParameters parameters;
protected:
static void _bind_methods();
public:
enum PathfindingAlgorithm {
PATHFINDING_ALGORITHM_ASTAR = 0,
PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR,
};
enum PathPostProcessing {
PATH_POSTPROCESSING_CORRIDORFUNNEL = 0,
PATH_POSTPROCESSING_EDGECENTERED,
PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL,
PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED,
PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE,
};
enum PathMetadataFlags {
@ -60,22 +59,32 @@ public:
PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL
};
const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; }
private:
PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR;
PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL;
RID map;
Vector3 start_position;
Vector3 target_position;
uint32_t navigation_layers = 1;
BitField<PathMetadataFlags> 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;

4
servers/navigation/navigation_path_query_result_2d.h
View file

@ -47,8 +47,8 @@ protected:
public:
enum PathSegmentType {
PATH_SEGMENT_TYPE_REGION = 0,
PATH_SEGMENT_TYPE_LINK = 1,
PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION,
PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK,
};
void set_path(const Vector<Vector2> &p_path);

4
servers/navigation/navigation_path_query_result_3d.h
View file

@ -48,8 +48,8 @@ protected:
public:
enum PathSegmentType {
PATH_SEGMENT_TYPE_REGION = 0,
PATH_SEGMENT_TYPE_LINK = 1,
PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION,
PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK,
};
void set_path(const Vector<Vector3> &p_path);

20
servers/navigation/navigation_utilities.h
View file

@ -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<PathMetadataFlags> metadata_flags = PATH_INCLUDE_ALL;
bool simplify_path = false;
real_t simplify_epsilon = 0.0;
};
struct PathQueryResult {
PackedVector3Array path;
PackedInt32Array path_types;
TypedArray<RID> path_rids;
PackedInt64Array path_owner_ids;
};
} //namespace NavigationUtilities
#endif // NAVIGATION_UTILITIES_H

46
servers/navigation_server_2d.compat.inc Normal file
View file

@ -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<Vector2> 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<NavigationServer2D *>(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers);
}
void NavigationServer2D::_query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
return const_cast<NavigationServer2D *>(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

3
servers/navigation_server_2d.cpp
View file

@ -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);

12
servers/navigation_server_2d.h
View file

@ -91,7 +91,7 @@ public:
virtual real_t map_get_link_connection_radius(RID p_map) const = 0;
/// Returns the navigation path to reach the destination from the origin.
virtual Vector<Vector2> 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<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0;
virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const = 0;
virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const = 0;
@ -293,7 +293,7 @@ public:
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const = 0;
/// Returns a customized navigation path using a query parameters object
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const = 0;
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) = 0;
virtual void init() = 0;
virtual void sync() = 0;
@ -318,6 +318,14 @@ public:
void set_debug_enabled(bool p_enabled);
bool get_debug_enabled() const;
protected:
#ifndef DISABLE_DEPRECATED
Vector<Vector2> _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<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> 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;

4
servers/navigation_server_2d_dummy.h
View file

@ -50,7 +50,7 @@ public:
real_t map_get_edge_connection_margin(RID p_map) const override { return 0; }
void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {}
real_t map_get_link_connection_radius(RID p_map) const override { return 0; }
Vector<Vector2> 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<Vector2>(); }
Vector<Vector2> 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>(); }
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<RID> map_get_links(RID p_map) const override { return TypedArray<RID>(); }
@ -158,7 +158,7 @@ public:
void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {}
uint32_t obstacle_get_avoidance_layers(RID p_agent) const override { return 0; }
void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override {}
void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) override {}
void init() override {}
void sync() override {}

46
servers/navigation_server_3d.compat.inc Normal file
View file

@ -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<Vector3> 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<NavigationServer3D *>(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers);
}
void NavigationServer3D::_query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const {
return const_cast<NavigationServer3D *>(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

17
servers/navigation_server_3d.cpp
View file

@ -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<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> 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;

13
servers/navigation_server_3d.h
View file

@ -103,7 +103,7 @@ public:
virtual real_t map_get_link_connection_radius(RID p_map) const = 0;
/// Returns the navigation path to reach the destination from the origin.
virtual Vector<Vector3> 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<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0;
virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const = 0;
virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const = 0;
@ -343,9 +343,7 @@ public:
virtual void finish() = 0;
/// Returns a customized navigation path using a query parameters object
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const;
virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const = 0;
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) = 0;
#ifndef _3D_DISABLED
virtual void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) = 0;
@ -380,6 +378,13 @@ public:
void set_debug_enabled(bool p_enabled);
bool get_debug_enabled() const;
protected:
#ifndef DISABLE_DEPRECATED
Vector<Vector3> _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<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const;
static void _bind_compatibility_methods();
#endif
private:
bool debug_enabled = false;

5
servers/navigation_server_3d_dummy.h
View file

@ -55,7 +55,7 @@ public:
real_t map_get_edge_connection_margin(RID p_map) const override { return 0; }
void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {}
real_t map_get_link_connection_radius(RID p_map) const override { return 0; }
Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const override { return Vector<Vector3>(); }
Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) override { return Vector<Vector3>(); }
Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const override { return Vector3(); }
Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override { return Vector3(); }
Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override { return Vector3(); }
@ -178,6 +178,8 @@ public:
void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {}
uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override { return 0; }
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override {}
#ifndef _3D_DISABLED
void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override {}
void bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override {}
@ -197,7 +199,6 @@ public:
void sync() override {}
void finish() override {}
NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override { return NavigationUtilities::PathQueryResult(); }
int get_process_info(ProcessInfo p_info) const override { return 0; }
void set_debug_enabled(bool p_enabled) {}