Add navigation path simplification

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.
  </member>
+ <member name="simplify_epsilon" type="float" setter="set_simplify_epsilon" getter="get_simplify_epsilon" default="0.0">
+ The path simplification amount in worlds units.
+ </member>
+ <member name="simplify_path" type="bool" setter="set_simplify_path" getter="get_simplify_path" default="false">
+ 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".
+ </member>
  <member name="target_desired_distance" type="float" setter="set_target_desired_distance" getter="get_target_desired_distance" default="10.0">
  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).

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.
  </member>
+ <member name="simplify_epsilon" type="float" setter="set_simplify_epsilon" getter="get_simplify_epsilon" default="0.0">
+ The path simplification amount in worlds units.
+ </member>
+ <member name="simplify_path" type="bool" setter="set_simplify_path" getter="get_simplify_path" default="false">
+ 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".
+ </member>
  <member name="target_desired_distance" type="float" setter="set_target_desired_distance" getter="get_target_desired_distance" default="1.0">
  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).

doc/classes/NavigationPathQueryParameters2D.xml

@@ -25,6 +25,13 @@
  <member name="pathfinding_algorithm" type="int" setter="set_pathfinding_algorithm" getter="get_pathfinding_algorithm" enum="NavigationPathQueryParameters2D.PathfindingAlgorithm" default="0">
  The pathfinding algorithm used in the path query.
  </member>
+ <member name="simplify_epsilon" type="float" setter="set_simplify_epsilon" getter="get_simplify_epsilon" default="0.0">
+ The path simplification amount in worlds units.
+ </member>
+ <member name="simplify_path" type="bool" setter="set_simplify_path" getter="get_simplify_path" default="false">
+ 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".
+ </member>
  <member name="start_position" type="Vector2" setter="set_start_position" getter="get_start_position" default="Vector2(0, 0)">
  The pathfinding start position in global coordinates.
  </member>

doc/classes/NavigationPathQueryParameters3D.xml

@@ -25,6 +25,13 @@
  <member name="pathfinding_algorithm" type="int" setter="set_pathfinding_algorithm" getter="get_pathfinding_algorithm" enum="NavigationPathQueryParameters3D.PathfindingAlgorithm" default="0">
  The pathfinding algorithm used in the path query.
  </member>
+ <member name="simplify_epsilon" type="float" setter="set_simplify_epsilon" getter="get_simplify_epsilon" default="0.0">
+ The path simplification amount in worlds units.
+ </member>
+ <member name="simplify_path" type="bool" setter="set_simplify_path" getter="get_simplify_path" default="false">
+ 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".
+ </member>
  <member name="start_position" type="Vector3" setter="set_start_position" getter="get_start_position" default="Vector3(0, 0, 0)">
  The pathfinding start position in global coordinates.
  </member>

doc/classes/NavigationServer2D.xml

@@ -947,6 +947,15 @@
  If [code]true[/code] enables debug mode on the NavigationServer.
  </description>
  </method>
+ <method name="simplify_path">
+ <return type="PackedVector2Array" />
+ <param index="0" name="path" type="PackedVector2Array" />
+ <param index="1" name="epsilon" type="float" />
+ <description>
+ 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".
+ </description>
+ </method>
  </methods>
  <signals>
  <signal name="map_changed">

doc/classes/NavigationServer3D.xml

@@ -1094,6 +1094,15 @@
  If [code]true[/code] enables debug mode on the NavigationServer.
  </description>
  </method>
+ <method name="simplify_path">
+ <return type="PackedVector3Array" />
+ <param index="0" name="path" type="PackedVector3Array" />
+ <param index="1" name="epsilon" type="float" />
+ <description>
+ 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".
+ </description>
+ </method>
  </methods>
  <signals>
  <signal name="avoidance_debug_changed">

modules/navigation/2d/godot_navigation_server_2d.cpp

@@ -229,6 +229,10 @@ bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref<NavigationPolygon
 #endif
 }
 
+Vector<Vector2> GodotNavigationServer2D::simplify_path(const Vector<Vector2> &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() {}

modules/navigation/2d/godot_navigation_server_2d.h

@@ -252,6 +252,8 @@ class GodotNavigationServer2D : public NavigationServer2D {
  virtual void bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
  virtual void bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
  virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const override;
+
+ virtual Vector<Vector2> simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) override;
 };
 
 #endif // GODOT_NAVIGATION_SERVER_2D_H

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<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;
 }
 
+Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) {
+ if (p_path.size() <= 2) {
+ return p_path;
+ }
+
+ p_epsilon = MAX(0.0, p_epsilon);
+
+ LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon);
+
+ uint32_t indices_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]];
+ }
+
+ 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: {

modules/navigation/3d/godot_navigation_server_3d.h

@@ -264,6 +264,13 @@ class GodotNavigationServer3D : public NavigationServer3D {
  virtual void bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
  virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override;
 
+ 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);
 
  virtual void set_active(bool p_active) override;

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<NavigationPathQueryParameters2D::PathMetadataFlags> p_path_metadata_flags) {
  if (path_metadata_flags == p_path_metadata_flags) {
  return;

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 @@ class NavigationAgent2D : public Node {
  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<NavigationPathQueryResult2D> get_current_navigation_result() const { return navigation_result; }