Add navigation path simplification

Adds navigation path simplification for NavigationServer and NavigationAgent.

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

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

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<NavigationPathQueryResult3D> get_current_navigation_result() const { return navigation_result; }

servers/navigation/navigation_path_query_parameters_2d.cpp

@@ -119,6 +119,22 @@ BitField<NavigationPathQueryParameters2D::PathMetadataFlags> 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);
 

servers/navigation/navigation_path_query_parameters_2d.h

@@ -82,6 +82,12 @@ public:
 
 	void set_metadata_flags(BitField<NavigationPathQueryParameters2D::PathMetadataFlags> p_flags);
 	BitField<NavigationPathQueryParameters2D::PathMetadataFlags> 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);

servers/navigation/navigation_path_query_parameters_3d.cpp

@@ -119,6 +119,22 @@ BitField<NavigationPathQueryParameters3D::PathMetadataFlags> 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);
 

servers/navigation/navigation_path_query_parameters_3d.h

@@ -82,6 +82,12 @@ public:
 
 	void set_metadata_flags(BitField<NavigationPathQueryParameters3D::PathMetadataFlags> p_flags);
 	BitField<NavigationPathQueryParameters3D::PathMetadataFlags> 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);

servers/navigation/navigation_utilities.h

@@ -66,6 +66,8 @@ struct PathQueryParameters {
 	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 {

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

servers/navigation_server_2d.h

@@ -306,6 +306,8 @@ public:
 	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()) = 0;
 	virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const = 0;
 
+	virtual Vector<Vector2> simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) = 0;
+
 	NavigationServer2D();
 	~NavigationServer2D() override;
 

servers/navigation_server_2d_dummy.h

@@ -170,6 +170,8 @@ public:
 	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 {}
 	bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const override { return false; }
 
+	Vector<Vector2> simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) override { return Vector<Vector2>(); }
+
 	void set_debug_enabled(bool p_enabled) {}
 	bool get_debug_enabled() const { return false; }
 };

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

servers/navigation_server_3d.h

@@ -349,6 +349,8 @@ public:
 	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()) = 0;
 	virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const = 0;
 
+	virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) = 0;
+
 	NavigationServer3D();
 	~NavigationServer3D() override;
 

servers/navigation_server_3d_dummy.h

@@ -180,6 +180,8 @@ public:
 	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 {}
 	bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override { return false; }
 
+	Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override { return Vector<Vector3>(); }
+
 	void free(RID p_object) override {}
 	void set_active(bool p_active) override {}
 	void process(real_t delta_time) override {}