Merge pull request #56879 from Scony/fix-navigation-merge-errors-3.x

[3.x] Fix navigation merge errors on too small triangles

doc/classes/Navigation.xml

@@ -57,6 +57,9 @@
 		</method>
 	</methods>
 	<members>
+		<member name="cell_height" type="float" setter="set_cell_height" getter="get_cell_height" default="0.2">
+			The cell height to use for fields.
+		</member>
 		<member name="cell_size" type="float" setter="set_cell_size" getter="get_cell_size" default="0.3">
 			The XZ plane cell size to use for fields.
 		</member>

doc/classes/NavigationMesh.xml

@@ -98,7 +98,7 @@
 		<member name="detail/sample_distance" type="float" setter="set_detail_sample_distance" getter="get_detail_sample_distance" default="6.0">
 			The sampling distance to use when generating the detail mesh, in cell unit.
 		</member>
-		<member name="detail/sample_max_error" type="float" setter="set_detail_sample_max_error" getter="get_detail_sample_max_error" default="1.0">
+		<member name="detail/sample_max_error" type="float" setter="set_detail_sample_max_error" getter="get_detail_sample_max_error" default="5.0">
 			The maximum distance the detail mesh surface should deviate from heightfield, in cell unit.
 		</member>
 		<member name="edge/max_error" type="float" setter="set_edge_max_error" getter="get_edge_max_error" default="1.3">

doc/classes/NavigationServer.xml

@@ -122,6 +122,13 @@
 				Create a new map.
 			</description>
 		</method>
+		<method name="map_get_cell_height" qualifiers="const">
+			<return type="float" />
+			<argument index="0" name="map" type="RID" />
+			<description>
+				Returns the map cell height.
+			</description>
+		</method>
 		<method name="map_get_cell_size" qualifiers="const">
 			<return type="float" />
 			<argument index="0" name="map" type="RID" />
@@ -202,6 +209,14 @@
 				Sets the map active.
 			</description>
 		</method>
+		<method name="map_set_cell_height" qualifiers="const">
+			<return type="void" />
+			<argument index="0" name="map" type="RID" />
+			<argument index="1" name="cell_height" type="float" />
+			<description>
+				Set the map cell height used to weld the navigation mesh polygons.
+			</description>
+		</method>
 		<method name="map_set_cell_size" qualifiers="const">
 			<return type="void" />
 			<argument index="0" name="map" type="RID" />

modules/navigation/godot_navigation_server.cpp

@@ -187,6 +187,20 @@ real_t GodotNavigationServer::map_get_cell_size(RID p_map) const {
 	return map->get_cell_size();
 }
 
+COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height) {
+	NavMap *map = map_owner.get(p_map);
+	ERR_FAIL_COND(map == nullptr);
+
+	map->set_cell_height(p_cell_height);
+}
+
+real_t GodotNavigationServer::map_get_cell_height(RID p_map) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == nullptr, 0);
+
+	return map->get_cell_height();
+}
+
 COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) {
 	NavMap *map = map_owner.get(p_map);
 	ERR_FAIL_COND(map == nullptr);

modules/navigation/godot_navigation_server.h

@@ -95,6 +95,9 @@ public:
 	COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size);
 	virtual real_t map_get_cell_size(RID p_map) const;
 
+	COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height);
+	virtual real_t map_get_cell_height(RID p_map) const;
+
 	COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
 	virtual real_t map_get_edge_connection_margin(RID p_map) const;
 

modules/navigation/nav_map.cpp

@@ -44,6 +44,7 @@
 NavMap::NavMap() :
 		up(0, 1, 0),
 		cell_size(0.3),
+		cell_height(0.2),
 		edge_connection_margin(5.0),
 		regenerate_polygons(true),
 		regenerate_links(true),
@@ -61,15 +62,20 @@ void NavMap::set_cell_size(float p_cell_size) {
 	regenerate_polygons = true;
 }
 
+void NavMap::set_cell_height(float p_cell_height) {
+	cell_height = p_cell_height;
+	regenerate_polygons = true;
+}
+
 void NavMap::set_edge_connection_margin(float p_edge_connection_margin) {
 	edge_connection_margin = p_edge_connection_margin;
 	regenerate_links = true;
 }
 
 gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
-	const int x = int(Math::floor(p_pos.x / cell_size));
-	const int y = int(Math::floor(p_pos.y / cell_size));
-	const int z = int(Math::floor(p_pos.z / cell_size));
+	const int x = static_cast<int>(Math::round(p_pos.x / cell_size));
+	const int y = static_cast<int>(Math::round(p_pos.y / cell_height));
+	const int z = static_cast<int>(Math::round(p_pos.z / cell_size));
 
 	gd::PointKey p;
 	p.key = 0;
@@ -640,7 +646,7 @@ void NavMap::sync() {
 					connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge;
 				} else {
 					// The edge is already connected with another edge, skip.
-					ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the Navigation's `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
+					ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. Either the Navigation's `cell_size` is different from the one used to generate the navigation mesh or `detail/sample_max_error` is too small. This will cause navigation problem.");
 				}
 			}
 		}

modules/navigation/nav_map.h

@@ -50,8 +50,9 @@ class NavMap : public NavRid {
 	Vector3 up;
 
 	/// To find the polygons edges the vertices are displaced in a grid where
-	/// each cell has the following cell_size.
+	/// each cell has the following cell_size and cell_height.
 	real_t cell_size;
+	real_t cell_height;
 
 	/// This value is used to detect the near edges to connect.
 	real_t edge_connection_margin;
@@ -95,6 +96,11 @@ public:
 		return cell_size;
 	}
 
+	void set_cell_height(float p_cell_height);
+	float get_cell_height() const {
+		return cell_height;
+	}
+
 	void set_edge_connection_margin(float p_edge_connection_margin);
 	float get_edge_connection_margin() const {
 		return edge_connection_margin;

scene/3d/navigation.cpp

@@ -66,6 +66,11 @@ void Navigation::set_cell_size(float p_cell_size) {
 	NavigationServer::get_singleton()->map_set_cell_size(map, cell_size);
 }
 
+void Navigation::set_cell_height(float p_cell_height) {
+	cell_height = p_cell_height;
+	NavigationServer::get_singleton()->map_set_cell_height(map, cell_height);
+}
+
 void Navigation::set_edge_connection_margin(float p_edge_connection_margin) {
 	edge_connection_margin = p_edge_connection_margin;
 	NavigationServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
@@ -86,11 +91,15 @@ void Navigation::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation::set_cell_size);
 	ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation::get_cell_size);
 
+	ClassDB::bind_method(D_METHOD("set_cell_height", "cell_height"), &Navigation::set_cell_height);
+	ClassDB::bind_method(D_METHOD("get_cell_height"), &Navigation::get_cell_height);
+
 	ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation::set_edge_connection_margin);
 	ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation::get_edge_connection_margin);
 
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_height"), "set_cell_height", "get_cell_height");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
 }
 
@@ -109,6 +118,7 @@ Navigation::Navigation() {
 	map = NavigationServer::get_singleton()->map_create();
 
 	set_cell_size(0.3);
+	set_cell_height(0.2);
 	set_edge_connection_margin(5.0); // Five meters, depends alot on the agents radius
 
 	up = Vector3(0, 1, 0);

scene/3d/navigation.h

@@ -41,6 +41,7 @@ class Navigation : public Spatial {
 
 	Vector3 up;
 	real_t cell_size;
+	real_t cell_height;
 	real_t edge_connection_margin;
 
 protected:
@@ -60,6 +61,11 @@ public:
 		return cell_size;
 	}
 
+	void set_cell_height(float p_cell_height);
+	float get_cell_height() const {
+		return cell_height;
+	}
+
 	void set_edge_connection_margin(float p_edge_connection_margin);
 	float get_edge_connection_margin() const {
 		return edge_connection_margin;

scene/resources/navigation_mesh.cpp

@@ -542,7 +542,7 @@ NavigationMesh::NavigationMesh() {
 	edge_max_error = 1.3f;
 	verts_per_poly = 6.0f;
 	detail_sample_distance = 6.0f;
-	detail_sample_max_error = 1.0f;
+	detail_sample_max_error = 5.0f;
 
 	partition_type = SAMPLE_PARTITION_WATERSHED;
 	parsed_geometry_type = PARSED_GEOMETRY_MESH_INSTANCES;

servers/navigation_server.cpp

@@ -44,6 +44,8 @@ void NavigationServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("map_get_up", "map"), &NavigationServer::map_get_up);
 	ClassDB::bind_method(D_METHOD("map_set_cell_size", "map", "cell_size"), &NavigationServer::map_set_cell_size);
 	ClassDB::bind_method(D_METHOD("map_get_cell_size", "map"), &NavigationServer::map_get_cell_size);
+	ClassDB::bind_method(D_METHOD("map_set_cell_height", "map", "cell_height"), &NavigationServer::map_set_cell_height);
+	ClassDB::bind_method(D_METHOD("map_get_cell_height", "map"), &NavigationServer::map_get_cell_height);
 	ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &NavigationServer::map_set_edge_connection_margin);
 	ClassDB::bind_method(D_METHOD("map_get_edge_connection_margin", "map"), &NavigationServer::map_get_edge_connection_margin);
 	ClassDB::bind_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize"), &NavigationServer::map_get_path);

servers/navigation_server.h

@@ -81,6 +81,12 @@ public:
 	/// Returns the map cell size.
 	virtual real_t map_get_cell_size(RID p_map) const = 0;
 
+	/// Set the map cell height used to weld the navigation mesh polygons.
+	virtual void map_set_cell_height(RID p_map, real_t p_cell_height) const = 0;
+
+	/// Returns the map cell height.
+	virtual real_t map_get_cell_height(RID p_map) const = 0;
+
 	/// Set the map edge connection margin used to weld the compatible region edges.
 	virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) const = 0;