Add means for fixing navmap synchronization errors

4 files changed, 50 insertions, 4 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index f6d94f280b..14c72f3db4 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -180,6 +180,20 @@ real_t GodotNavigationServer::map_get_cell_height(RID p_map) const {
 	return map->get_cell_height();
 }
 
+COMMAND_2(map_set_merge_rasterizer_cell_scale, RID, p_map, float, p_value) {
+	NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	map->set_merge_rasterizer_cell_scale(p_value);
+}
+
+float GodotNavigationServer::map_get_merge_rasterizer_cell_scale(RID p_map) const {
+	NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, false);
+
+	return map->get_merge_rasterizer_cell_scale();
+}
+
 COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled) {
 	NavMap *map = map_owner.get_or_null(p_map);
 	ERR_FAIL_NULL(map);
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index 6798978d21..f3bc1185d8 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -117,6 +117,9 @@ public:
 	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 override;
 
+	COMMAND_2(map_set_merge_rasterizer_cell_scale, RID, p_map, float, p_value);
+	virtual float map_get_merge_rasterizer_cell_scale(RID p_map) const override;
+
 	COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
 	virtual bool map_get_use_edge_connections(RID p_map) const override;
 
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 6429513b53..7edf4a0bd1 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -67,6 +67,7 @@ void NavMap::set_cell_size(real_t p_cell_size) {
 		return;
 	}
 	cell_size = p_cell_size;
+	_update_merge_rasterizer_cell_dimensions();
 	regenerate_polygons = true;
 }
 
@@ -75,6 +76,16 @@ void NavMap::set_cell_height(real_t p_cell_height) {
 		return;
 	}
 	cell_height = p_cell_height;
+	_update_merge_rasterizer_cell_dimensions();
+	regenerate_polygons = true;
+}
+
+void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
+	if (merge_rasterizer_cell_scale == p_value) {
+		return;
+	}
+	merge_rasterizer_cell_scale = p_value;
+	_update_merge_rasterizer_cell_dimensions();
 	regenerate_polygons = true;
 }
 
@@ -103,9 +114,9 @@ void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
 }
 
 gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
-	const int x = static_cast<int>(Math::floor(p_pos.x / cell_size));
-	const int y = static_cast<int>(Math::floor(p_pos.y / cell_height));
-	const int z = static_cast<int>(Math::floor(p_pos.z / cell_size));
+	const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size));
+	const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_height));
+	const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size));
 
 	gd::PointKey p;
 	p.key = 0;
@@ -923,7 +934,7 @@ void NavMap::sync() {
 					connections[ek].push_back(new_connection);
 				} else {
 					// The edge is already connected with another edge, skip.
-					ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'.");
+					ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
 				}
 			}
 		}
@@ -1365,6 +1376,11 @@ void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys
 	}
 }
 
+void NavMap::_update_merge_rasterizer_cell_dimensions() {
+	merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale;
+}
+
 NavMap::NavMap() {
 	avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
 	avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index e8cbe7e247..b00bddc0ab 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -56,6 +56,12 @@ class NavMap : public NavRid {
 	real_t cell_size = 0.25; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size.
 	real_t cell_height = 0.25; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height.
 
+	// For the inter-region merging to work, internal rasterization is performed.
+	float merge_rasterizer_cell_size = 0.25;
+	float merge_rasterizer_cell_height = 0.25;
+	// This value is used to control sensitivity of internal rasterizer.
+	float merge_rasterizer_cell_scale = 1.0;
+
 	bool use_edge_connections = true;
 	/// This value is used to detect the near edges to connect.
 	real_t edge_connection_margin = 0.25;
@@ -133,6 +139,11 @@ public:
 	void set_cell_height(real_t p_cell_height);
 	real_t get_cell_height() const { return cell_height; }
 
+	void set_merge_rasterizer_cell_scale(float p_value);
+	float get_merge_rasterizer_cell_scale() const {
+		return merge_rasterizer_cell_scale;
+	}
+
 	void set_use_edge_connections(bool p_enabled);
 	bool get_use_edge_connections() const {
 		return use_edge_connections;
@@ -217,6 +228,8 @@ private:
 	void _update_rvo_obstacles_tree_2d();
 	void _update_rvo_agents_tree_2d();
 	void _update_rvo_agents_tree_3d();
+
+	void _update_merge_rasterizer_cell_dimensions();
 };
 
 #endif // NAV_MAP_H