From 1c134f4a3d8de7b56687cac11415a9fc6623858f Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Tue, 9 Apr 2024 04:27:54 +0200 Subject: [PATCH] Add navigation path simplification Adds navigation path simplification for NavigationServer and NavigationAgent. --- doc/classes/NavigationAgent2D.xml | 7 + doc/classes/NavigationAgent3D.xml | 7 + .../NavigationPathQueryParameters2D.xml | 7 + .../NavigationPathQueryParameters3D.xml | 7 + doc/classes/NavigationServer2D.xml | 9 ++ doc/classes/NavigationServer3D.xml | 9 ++ .../2d/godot_navigation_server_2d.cpp | 4 + .../2d/godot_navigation_server_2d.h | 2 + .../3d/godot_navigation_server_3d.cpp | 129 ++++++++++++++++++ .../3d/godot_navigation_server_3d.h | 7 + scene/2d/navigation_agent_2d.cpp | 26 ++++ scene/2d/navigation_agent_2d.h | 8 ++ scene/3d/navigation_agent_3d.cpp | 26 ++++ scene/3d/navigation_agent_3d.h | 8 ++ .../navigation_path_query_parameters_2d.cpp | 24 ++++ .../navigation_path_query_parameters_2d.h | 6 + .../navigation_path_query_parameters_3d.cpp | 24 ++++ .../navigation_path_query_parameters_3d.h | 6 + servers/navigation/navigation_utilities.h | 2 + servers/navigation_server_2d.cpp | 2 + servers/navigation_server_2d.h | 2 + servers/navigation_server_2d_dummy.h | 2 + servers/navigation_server_3d.cpp | 2 + servers/navigation_server_3d.h | 2 + servers/navigation_server_3d_dummy.h | 2 + 25 files changed, 330 insertions(+) diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml index 132ece53b04..6f0561e66e0 100644 --- a/doc/classes/NavigationAgent2D.xml +++ b/doc/classes/NavigationAgent2D.xml @@ -198,6 +198,13 @@ The radius of the avoidance agent. This is the "body" of the avoidance agent and not the avoidance maneuver starting radius (which is controlled by [member neighbor_distance]). Does not affect normal pathfinding. To change an actor's pathfinding radius bake [NavigationMesh] resources with a different [member NavigationMesh.agent_radius] property and use different navigation maps for each actor size. + + The path simplification amount in worlds units. + + + If [code]true[/code] a simplified version of the path will be returned with less critical path points removed. The simplification amount is controlled by [member simplify_epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + The distance threshold before the target is considered to be reached. On reaching the target, [signal target_reached] is emitted and navigation ends (see [method is_navigation_finished] and [signal navigation_finished]). You can make navigation end early by setting this property to a value greater than [member path_desired_distance] (navigation will end before reaching the last waypoint). diff --git a/doc/classes/NavigationAgent3D.xml b/doc/classes/NavigationAgent3D.xml index dd75cedf617..64ee35a84b5 100644 --- a/doc/classes/NavigationAgent3D.xml +++ b/doc/classes/NavigationAgent3D.xml @@ -204,6 +204,13 @@ The radius of the avoidance agent. This is the "body" of the avoidance agent and not the avoidance maneuver starting radius (which is controlled by [member neighbor_distance]). Does not affect normal pathfinding. To change an actor's pathfinding radius bake [NavigationMesh] resources with a different [member NavigationMesh.agent_radius] property and use different navigation maps for each actor size. + + The path simplification amount in worlds units. + + + If [code]true[/code] a simplified version of the path will be returned with less critical path points removed. The simplification amount is controlled by [member simplify_epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + The distance threshold before the target is considered to be reached. On reaching the target, [signal target_reached] is emitted and navigation ends (see [method is_navigation_finished] and [signal navigation_finished]). You can make navigation end early by setting this property to a value greater than [member path_desired_distance] (navigation will end before reaching the last waypoint). diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml index 7d9ecf61b0b..ce0a00bc83b 100644 --- a/doc/classes/NavigationPathQueryParameters2D.xml +++ b/doc/classes/NavigationPathQueryParameters2D.xml @@ -25,6 +25,13 @@ The pathfinding algorithm used in the path query. + + The path simplification amount in worlds units. + + + If [code]true[/code] a simplified version of the path will be returned with less critical path points removed. The simplification amount is controlled by [member simplify_epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + The pathfinding start position in global coordinates. diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml index 862f8a8347e..fcd913f73b5 100644 --- a/doc/classes/NavigationPathQueryParameters3D.xml +++ b/doc/classes/NavigationPathQueryParameters3D.xml @@ -25,6 +25,13 @@ The pathfinding algorithm used in the path query. + + The path simplification amount in worlds units. + + + If [code]true[/code] a simplified version of the path will be returned with less critical path points removed. The simplification amount is controlled by [member simplify_epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + The pathfinding start position in global coordinates. diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index baef7dc02de..a6f6abccbd7 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -947,6 +947,15 @@ If [code]true[/code] enables debug mode on the NavigationServer. + + + + + + Returns a simplified version of [param path] with less critical path points removed. The simplification amount is in worlds units and controlled by [param epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + + diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index 6be9d5165fb..6fcf033544f 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -1094,6 +1094,15 @@ If [code]true[/code] enables debug mode on the NavigationServer. + + + + + + Returns a simplified version of [param path] with less critical path points removed. The simplification amount is in worlds units and controlled by [param epsilon]. The simplification uses a variant of Ramer-Douglas-Peucker algorithm for curve point decimation. + Path simplification can be helpful to mitigate various path following issues that can arise with certain agent types and script behaviors. E.g. "steering" agents or avoidance in "open fields". + + diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index 28bcd163100..5eefbe42280 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -229,6 +229,10 @@ bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref GodotNavigationServer2D::simplify_path(const Vector &p_path, real_t p_epsilon) { + return vector_v3_to_v2(NavigationServer3D::get_singleton()->simplify_path(vector_v2_to_v3(p_path), p_epsilon)); +} + GodotNavigationServer2D::GodotNavigationServer2D() {} GodotNavigationServer2D::~GodotNavigationServer2D() {} diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h index a148887a653..ba375afd33d 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.h +++ b/modules/navigation/2d/godot_navigation_server_2d.h @@ -252,6 +252,8 @@ public: virtual void bake_from_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override; virtual void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override; virtual bool is_baking_navigation_polygon(Ref p_navigation_polygon) const override; + + virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) override; }; #endif // GODOT_NAVIGATION_SERVER_2D_H diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 15de33f58f4..301b4aa8f38 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -1381,11 +1381,140 @@ PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters & // add path postprocessing + if (r_query_result.path.size() > 2 && p_parameters.simplify_path) { + const LocalVector &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + { + Vector3 *w = r_query_result.path.ptrw(); + const Vector3 *r = r_query_result.path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + int32_t *w = r_query_result.path_types.ptrw(); + const int32_t *r = r_query_result.path_types.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_types.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + TypedArray simplified_path_rids; + simplified_path_rids.resize(indices_count); + for (uint32_t i = 0; i < indices_count; i++) { + simplified_path_rids[i] = r_query_result.path_rids[i]; + } + r_query_result.path_rids = simplified_path_rids; + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + int64_t *w = r_query_result.path_owner_ids.ptrw(); + const int64_t *r = r_query_result.path_owner_ids.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_owner_ids.resize(indices_count); + } + } + // add path stats return r_query_result; } +Vector GodotNavigationServer3D::simplify_path(const Vector &p_path, real_t p_epsilon) { + if (p_path.size() <= 2) { + return p_path; + } + + p_epsilon = MAX(0.0, p_epsilon); + + LocalVector simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + Vector simplified_path; + simplified_path.resize(indices_count); + + Vector3 *w = simplified_path.ptrw(); + const Vector3 *r = p_path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + + return simplified_path; +} + +LocalVector GodotNavigationServer3D::get_simplified_path_indices(const Vector &p_path, real_t p_epsilon) { + p_epsilon = MAX(0.0, p_epsilon); + real_t squared_epsilon = p_epsilon * p_epsilon; + + LocalVector valid_points; + valid_points.resize(p_path.size()); + for (uint32_t i = 0; i < valid_points.size(); i++) { + valid_points[i] = false; + } + + simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points); + + int valid_point_index = 0; + + for (bool valid : valid_points) { + if (valid) { + valid_point_index += 1; + } + } + + LocalVector simplified_path_indices; + simplified_path_indices.resize(valid_point_index); + valid_point_index = 0; + + for (uint32_t i = 0; i < valid_points.size(); i++) { + if (valid_points[i]) { + simplified_path_indices[valid_point_index] = i; + valid_point_index += 1; + } + } + + return simplified_path_indices; +} + +void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector &p_points, real_t p_epsilon, LocalVector &r_valid_points) { + r_valid_points[p_start_inx] = true; + r_valid_points[p_end_inx] = true; + + const Vector3 &start_point = p_points[p_start_inx]; + const Vector3 &end_point = p_points[p_end_inx]; + + Vector3 path_segment[2] = { start_point, end_point }; + + real_t point_max_distance = 0.0; + int point_max_index = 0; + + for (int i = p_start_inx; i < p_end_inx; i++) { + const Vector3 &checked_point = p_points[i]; + + const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment); + real_t distance_squared = closest_point.distance_squared_to(checked_point); + + if (distance_squared > point_max_distance) { + point_max_index = i; + point_max_distance = distance_squared; + } + } + + if (point_max_distance > p_epsilon) { + simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points); + simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points); + } +} + int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const { switch (p_info) { case INFO_ACTIVE_MAPS: { diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h index f7d991d47a7..89839ff459b 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.h +++ b/modules/navigation/3d/godot_navigation_server_3d.h @@ -264,6 +264,13 @@ public: virtual void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override; virtual bool is_baking_navigation_mesh(Ref p_navigation_mesh) const override; + virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) override; + +private: + static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector &p_points, real_t p_epsilon, LocalVector &r_valid_points); + static LocalVector get_simplified_path_indices(const Vector &p_path, real_t p_epsilon); + +public: COMMAND_1(free, RID, p_object); virtual void set_active(bool p_active) override; diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp index fee774fe2e6..5d14358120c 100644 --- a/scene/2d/navigation_agent_2d.cpp +++ b/scene/2d/navigation_agent_2d.cpp @@ -89,6 +89,12 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_target_position", "position"), &NavigationAgent2D::set_target_position); ClassDB::bind_method(D_METHOD("get_target_position"), &NavigationAgent2D::get_target_position); + ClassDB::bind_method(D_METHOD("set_simplify_path", "enabled"), &NavigationAgent2D::set_simplify_path); + ClassDB::bind_method(D_METHOD("get_simplify_path"), &NavigationAgent2D::get_simplify_path); + + ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationAgent2D::set_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationAgent2D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent2D::get_next_path_position); ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent2D::set_velocity_forced); @@ -129,6 +135,8 @@ void NavigationAgent2D::_bind_methods() { 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_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:px"), "set_simplify_epsilon", "get_simplify_epsilon"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -427,6 +435,24 @@ void NavigationAgent2D::set_path_postprocessing(const NavigationPathQueryParamet navigation_query->set_path_postprocessing(path_postprocessing); } +void NavigationAgent2D::set_simplify_path(bool p_enabled) { + simplify_path = p_enabled; + navigation_query->set_simplify_path(simplify_path); +} + +bool NavigationAgent2D::get_simplify_path() const { + return simplify_path; +} + +void NavigationAgent2D::set_simplify_epsilon(real_t p_epsilon) { + simplify_epsilon = MAX(0.0, p_epsilon); + navigation_query->set_simplify_epsilon(simplify_epsilon); +} + +real_t NavigationAgent2D::get_simplify_epsilon() const { + return simplify_epsilon; +} + void NavigationAgent2D::set_path_metadata_flags(BitField p_path_metadata_flags) { if (path_metadata_flags == p_path_metadata_flags) { return; diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h index 0e46086a81d..0acfc821624 100644 --- a/scene/2d/navigation_agent_2d.h +++ b/scene/2d/navigation_agent_2d.h @@ -63,6 +63,8 @@ class NavigationAgent2D : public Node { real_t time_horizon_obstacles = 0.0; real_t max_speed = 100.0; real_t path_max_distance = 100.0; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; Vector2 target_position; @@ -179,6 +181,12 @@ public: void set_target_position(Vector2 p_position); Vector2 get_target_position() const; + void set_simplify_path(bool p_enabled); + bool get_simplify_path() const; + + void set_simplify_epsilon(real_t p_epsilon); + real_t get_simplify_epsilon() const; + Vector2 get_next_path_position(); Ref get_current_navigation_result() const { return navigation_result; } diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp index eb52d4540e5..43c41c1b5b1 100644 --- a/scene/3d/navigation_agent_3d.cpp +++ b/scene/3d/navigation_agent_3d.cpp @@ -99,6 +99,12 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_target_position", "position"), &NavigationAgent3D::set_target_position); ClassDB::bind_method(D_METHOD("get_target_position"), &NavigationAgent3D::get_target_position); + ClassDB::bind_method(D_METHOD("set_simplify_path", "enabled"), &NavigationAgent3D::set_simplify_path); + ClassDB::bind_method(D_METHOD("get_simplify_path"), &NavigationAgent3D::get_simplify_path); + + ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationAgent3D::set_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationAgent3D::get_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent3D::get_next_path_position); ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent3D::set_velocity_forced); @@ -140,6 +146,8 @@ void NavigationAgent3D::_bind_methods() { 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_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:m"), "set_simplify_epsilon", "get_simplify_epsilon"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -464,6 +472,24 @@ void NavigationAgent3D::set_path_postprocessing(const NavigationPathQueryParamet navigation_query->set_path_postprocessing(path_postprocessing); } +void NavigationAgent3D::set_simplify_path(bool p_enabled) { + simplify_path = p_enabled; + navigation_query->set_simplify_path(simplify_path); +} + +bool NavigationAgent3D::get_simplify_path() const { + return simplify_path; +} + +void NavigationAgent3D::set_simplify_epsilon(real_t p_epsilon) { + simplify_epsilon = MAX(0.0, p_epsilon); + navigation_query->set_simplify_epsilon(simplify_epsilon); +} + +real_t NavigationAgent3D::get_simplify_epsilon() const { + return simplify_epsilon; +} + void NavigationAgent3D::set_path_metadata_flags(BitField p_path_metadata_flags) { if (path_metadata_flags == p_path_metadata_flags) { return; diff --git a/scene/3d/navigation_agent_3d.h b/scene/3d/navigation_agent_3d.h index 4eaed831492..ade6afd445e 100644 --- a/scene/3d/navigation_agent_3d.h +++ b/scene/3d/navigation_agent_3d.h @@ -66,6 +66,8 @@ class NavigationAgent3D : public Node { real_t time_horizon_obstacles = 0.0; real_t max_speed = 10.0; real_t path_max_distance = 5.0; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; Vector3 target_position; @@ -200,6 +202,12 @@ public: void set_target_position(Vector3 p_position); Vector3 get_target_position() const; + void set_simplify_path(bool p_enabled); + bool get_simplify_path() const; + + void set_simplify_epsilon(real_t p_epsilon); + real_t get_simplify_epsilon() const; + Vector3 get_next_path_position(); Ref get_current_navigation_result() const { return navigation_result; } diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp index 78da87d677f..6c1f88e3490 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.cpp +++ b/servers/navigation/navigation_path_query_parameters_2d.cpp @@ -119,6 +119,22 @@ BitField NavigationPathQuery return (int64_t)parameters.metadata_flags; } +void NavigationPathQueryParameters2D::set_simplify_path(bool p_enabled) { + parameters.simplify_path = p_enabled; +} + +bool NavigationPathQueryParameters2D::get_simplify_path() const { + return parameters.simplify_path; +} + +void NavigationPathQueryParameters2D::set_simplify_epsilon(real_t p_epsilon) { + parameters.simplify_epsilon = MAX(0.0, p_epsilon); +} + +real_t NavigationPathQueryParameters2D::get_simplify_epsilon() const { + return parameters.simplify_epsilon; +} + void NavigationPathQueryParameters2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_pathfinding_algorithm", "pathfinding_algorithm"), &NavigationPathQueryParameters2D::set_pathfinding_algorithm); ClassDB::bind_method(D_METHOD("get_pathfinding_algorithm"), &NavigationPathQueryParameters2D::get_pathfinding_algorithm); @@ -141,6 +157,12 @@ void NavigationPathQueryParameters2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_metadata_flags", "flags"), &NavigationPathQueryParameters2D::set_metadata_flags); ClassDB::bind_method(D_METHOD("get_metadata_flags"), &NavigationPathQueryParameters2D::get_metadata_flags); + ClassDB::bind_method(D_METHOD("set_simplify_path", "enabled"), &NavigationPathQueryParameters2D::set_simplify_path); + ClassDB::bind_method(D_METHOD("get_simplify_path"), &NavigationPathQueryParameters2D::get_simplify_path); + + ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationPathQueryParameters2D::set_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationPathQueryParameters2D::get_simplify_epsilon); + ADD_PROPERTY(PropertyInfo(Variant::RID, "map"), "set_map", "get_map"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "start_position"), "set_start_position", "get_start_position"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "target_position"), "set_target_position", "get_target_position"); @@ -148,6 +170,8 @@ void NavigationPathQueryParameters2D::_bind_methods() { 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, "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"); BIND_ENUM_CONSTANT(PATHFINDING_ALGORITHM_ASTAR); diff --git a/servers/navigation/navigation_path_query_parameters_2d.h b/servers/navigation/navigation_path_query_parameters_2d.h index 48bb93aa7c7..a1d5f2d109a 100644 --- a/servers/navigation/navigation_path_query_parameters_2d.h +++ b/servers/navigation/navigation_path_query_parameters_2d.h @@ -82,6 +82,12 @@ public: void set_metadata_flags(BitField p_flags); BitField get_metadata_flags() const; + + void set_simplify_path(bool p_enabled); + bool get_simplify_path() const; + + void set_simplify_epsilon(real_t p_epsilon); + real_t get_simplify_epsilon() const; }; VARIANT_ENUM_CAST(NavigationPathQueryParameters2D::PathfindingAlgorithm); diff --git a/servers/navigation/navigation_path_query_parameters_3d.cpp b/servers/navigation/navigation_path_query_parameters_3d.cpp index 52333edbc13..b0a5b0ad82b 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.cpp +++ b/servers/navigation/navigation_path_query_parameters_3d.cpp @@ -119,6 +119,22 @@ BitField NavigationPathQuery return (int64_t)parameters.metadata_flags; } +void NavigationPathQueryParameters3D::set_simplify_path(bool p_enabled) { + parameters.simplify_path = p_enabled; +} + +bool NavigationPathQueryParameters3D::get_simplify_path() const { + return parameters.simplify_path; +} + +void NavigationPathQueryParameters3D::set_simplify_epsilon(real_t p_epsilon) { + parameters.simplify_epsilon = MAX(0.0, p_epsilon); +} + +real_t NavigationPathQueryParameters3D::get_simplify_epsilon() const { + return parameters.simplify_epsilon; +} + void NavigationPathQueryParameters3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_pathfinding_algorithm", "pathfinding_algorithm"), &NavigationPathQueryParameters3D::set_pathfinding_algorithm); ClassDB::bind_method(D_METHOD("get_pathfinding_algorithm"), &NavigationPathQueryParameters3D::get_pathfinding_algorithm); @@ -141,6 +157,12 @@ void NavigationPathQueryParameters3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_metadata_flags", "flags"), &NavigationPathQueryParameters3D::set_metadata_flags); ClassDB::bind_method(D_METHOD("get_metadata_flags"), &NavigationPathQueryParameters3D::get_metadata_flags); + ClassDB::bind_method(D_METHOD("set_simplify_path", "enabled"), &NavigationPathQueryParameters3D::set_simplify_path); + ClassDB::bind_method(D_METHOD("get_simplify_path"), &NavigationPathQueryParameters3D::get_simplify_path); + + ClassDB::bind_method(D_METHOD("set_simplify_epsilon", "epsilon"), &NavigationPathQueryParameters3D::set_simplify_epsilon); + ClassDB::bind_method(D_METHOD("get_simplify_epsilon"), &NavigationPathQueryParameters3D::get_simplify_epsilon); + ADD_PROPERTY(PropertyInfo(Variant::RID, "map"), "set_map", "get_map"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "start_position"), "set_start_position", "get_start_position"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position"), "set_target_position", "get_target_position"); @@ -148,6 +170,8 @@ void NavigationPathQueryParameters3D::_bind_methods() { 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, "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"); BIND_ENUM_CONSTANT(PATHFINDING_ALGORITHM_ASTAR); diff --git a/servers/navigation/navigation_path_query_parameters_3d.h b/servers/navigation/navigation_path_query_parameters_3d.h index b12c2d49d51..2eb85db7875 100644 --- a/servers/navigation/navigation_path_query_parameters_3d.h +++ b/servers/navigation/navigation_path_query_parameters_3d.h @@ -82,6 +82,12 @@ public: void set_metadata_flags(BitField p_flags); BitField get_metadata_flags() const; + + void set_simplify_path(bool p_enabled); + bool get_simplify_path() const; + + void set_simplify_epsilon(real_t p_epsilon); + real_t get_simplify_epsilon() const; }; VARIANT_ENUM_CAST(NavigationPathQueryParameters3D::PathfindingAlgorithm); diff --git a/servers/navigation/navigation_utilities.h b/servers/navigation/navigation_utilities.h index 04d0ab0d98c..7ae22b1d3a2 100644 --- a/servers/navigation/navigation_utilities.h +++ b/servers/navigation/navigation_utilities.h @@ -66,6 +66,8 @@ struct PathQueryParameters { Vector3 target_position; uint32_t navigation_layers = 1; BitField metadata_flags = PATH_INCLUDE_ALL; + bool simplify_path = false; + real_t simplify_epsilon = 0.0; }; struct PathQueryResult { diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index d87ac00e327..625ae8abde1 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -165,6 +165,8 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data_async", "navigation_polygon", "source_geometry_data", "callback"), &NavigationServer2D::bake_from_source_geometry_data_async, DEFVAL(Callable())); ClassDB::bind_method(D_METHOD("is_baking_navigation_polygon", "navigation_polygon"), &NavigationServer2D::is_baking_navigation_polygon); + ClassDB::bind_method(D_METHOD("simplify_path", "path", "epsilon"), &NavigationServer2D::simplify_path); + ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer2D::free); ClassDB::bind_method(D_METHOD("set_debug_enabled", "enabled"), &NavigationServer2D::set_debug_enabled); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index d7dc9d6526c..876b09d5498 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -306,6 +306,8 @@ public: virtual void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) = 0; virtual bool is_baking_navigation_polygon(Ref p_navigation_polygon) const = 0; + virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) = 0; + NavigationServer2D(); ~NavigationServer2D() override; diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h index 94c6dfd6a79..5d4cfbf91b1 100644 --- a/servers/navigation_server_2d_dummy.h +++ b/servers/navigation_server_2d_dummy.h @@ -170,6 +170,8 @@ public: void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override {} bool is_baking_navigation_polygon(Ref p_navigation_polygon) const override { return false; } + Vector simplify_path(const Vector &p_path, real_t p_epsilon) override { return Vector(); } + void set_debug_enabled(bool p_enabled) {} bool get_debug_enabled() const { return false; } }; diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index e460bcb9c6b..7b54a24b599 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -186,6 +186,8 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data_async", "navigation_mesh", "source_geometry_data", "callback"), &NavigationServer3D::bake_from_source_geometry_data_async, DEFVAL(Callable())); ClassDB::bind_method(D_METHOD("is_baking_navigation_mesh", "navigation_mesh"), &NavigationServer3D::is_baking_navigation_mesh); + ClassDB::bind_method(D_METHOD("simplify_path", "path", "epsilon"), &NavigationServer3D::simplify_path); + ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer3D::free); ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer3D::set_active); diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index 975ffdee945..c23da782995 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -349,6 +349,8 @@ public: virtual void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) = 0; virtual bool is_baking_navigation_mesh(Ref p_navigation_mesh) const = 0; + virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) = 0; + NavigationServer3D(); ~NavigationServer3D() override; diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h index 9ba9b20d004..d98a0edb010 100644 --- a/servers/navigation_server_3d_dummy.h +++ b/servers/navigation_server_3d_dummy.h @@ -180,6 +180,8 @@ public: void bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override {} bool is_baking_navigation_mesh(Ref p_navigation_mesh) const override { return false; } + Vector simplify_path(const Vector &p_path, real_t p_epsilon) override { return Vector(); } + void free(RID p_object) override {} void set_active(bool p_active) override {} void process(real_t delta_time) override {}