Merge pull request godotengine#90434 from smix8/path_simplify

Add navigation path simplification

Author: akien-mga

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;