Add navigation path simplification

Adds navigation path simplification for NavigationServer and NavigationAgent.

5 files changed, 62 insertions, 0 deletions

diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp
index 78da87d677..6c1f88e349 100644
--- a/servers/navigation/navigation_path_query_parameters_2d.cpp
+++ b/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);
 
diff --git a/servers/navigation/navigation_path_query_parameters_2d.h b/servers/navigation/navigation_path_query_parameters_2d.h
index 48bb93aa7c..a1d5f2d109 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<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);
diff --git a/servers/navigation/navigation_path_query_parameters_3d.cpp b/servers/navigation/navigation_path_query_parameters_3d.cpp
index 52333edbc1..b0a5b0ad82 100644
--- a/servers/navigation/navigation_path_query_parameters_3d.cpp
+++ b/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);
 
diff --git a/servers/navigation/navigation_path_query_parameters_3d.h b/servers/navigation/navigation_path_query_parameters_3d.h
index b12c2d49d5..2eb85db787 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<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);
diff --git a/servers/navigation/navigation_utilities.h b/servers/navigation/navigation_utilities.h
index 04d0ab0d98..7ae22b1d3a 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<PathMetadataFlags> metadata_flags = PATH_INCLUDE_ALL;
+	bool simplify_path = false;
+	real_t simplify_epsilon = 0.0;
 };
 
 struct PathQueryResult {