Add navigation path simplification

Adds navigation path simplification for NavigationServer and NavigationAgent.

4 files changed, 142 insertions, 0 deletions

diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp
index 28bcd16310..5eefbe4228 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<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() {}
diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h
index a148887a65..ba375afd33 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<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
diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp
index 15de33f58f..301b4aa8f3 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<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: {
diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h
index f7d991d47a..89839ff459 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<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;