Postprocess navmeshes to work around engine nav mesh baking issues #264

- Merges (rounds) vertices and collapses edges to avoid resolution issues
  in the navigation map.
- Removes overlapping polygon that baking has produced.

project/addons/terrain_3d/editor/components/baker.gd

@@ -168,6 +168,8 @@ func _bake_nav_region_nav_mesh(p_nav_region: NavigationRegion3D) -> void:
 
 	NavigationMeshGenerator.bake_from_source_geometry_data(nav_mesh, source_geometry_data)
 
+	_postprocess_nav_mesh(nav_mesh)
+
 	# Assign null first to force the debug display to actually update:
 	p_nav_region.set_navigation_mesh(null)
 	p_nav_region.set_navigation_mesh(nav_mesh)
@@ -180,6 +182,120 @@ func _bake_nav_region_nav_mesh(p_nav_region: NavigationRegion3D) -> void:
 	p_nav_region.bake_finished.emit()
 
 
+func _postprocess_nav_mesh(p_nav_mesh: NavigationMesh) -> void:
+	# Post-process the nav mesh to work around Godot issue #85548
+
+	# Round all the vertices in the nav_mesh to the nearest cell_size/cell_height so that it doesn't
+	# contain any edges shorter than cell_size/cell_height (one cause of #85548).
+	var vertices: PackedVector3Array = _postprocess_nav_mesh_round_vertices(p_nav_mesh)
+
+	# Rounding vertices can collapse some edges to 0 length. We remove these edges, and any polygons
+	# that have been reduced to 0 area.
+	var polygons: Array[PackedInt32Array] = _postprocess_nav_mesh_remove_empty_polygons(p_nav_mesh, vertices)
+
+	# Another cause of #85548 is baking producing overlapping polygons. We remove these.
+	_postprocess_nav_mesh_remove_overlapping_polygons(p_nav_mesh, vertices, polygons)
+
+	p_nav_mesh.clear_polygons()
+	p_nav_mesh.set_vertices(vertices)
+	for polygon in polygons:
+		p_nav_mesh.add_polygon(polygon)
+
+
+func _postprocess_nav_mesh_round_vertices(p_nav_mesh: NavigationMesh) -> PackedVector3Array:
+	assert(p_nav_mesh != null)
+	assert(p_nav_mesh.cell_size > 0.0)
+	assert(p_nav_mesh.cell_height > 0.0)
+
+	var cell_size: Vector3 = Vector3(p_nav_mesh.cell_size, p_nav_mesh.cell_height, p_nav_mesh.cell_size)
+
+	# Round a little harder to avoid rounding errors with non-power-of-two cell_size/cell_height
+	# causing the navigation map to put two non-matching edges in the same cell:
+	var round_factor := cell_size * 1.001
+
+	var vertices: PackedVector3Array = p_nav_mesh.get_vertices()
+	for i in range(vertices.size()):
+		vertices[i] = (vertices[i] / round_factor).floor() * round_factor
+	return vertices
+
+
+func _postprocess_nav_mesh_remove_empty_polygons(p_nav_mesh: NavigationMesh, p_vertices: PackedVector3Array) -> Array[PackedInt32Array]:
+	var polygons: Array[PackedInt32Array] = []
+
+	for i in range(p_nav_mesh.get_polygon_count()):
+		var old_polygon: PackedInt32Array = p_nav_mesh.get_polygon(i)
+		var new_polygon: PackedInt32Array = []
+
+		# Remove duplicate vertices (introduced by rounding) from the polygon:
+		var polygon_vertices: PackedVector3Array = []
+		for index in old_polygon:
+			var vertex: Vector3 = p_vertices[index]
+			if polygon_vertices.has(vertex):
+				continue
+			polygon_vertices.push_back(vertex)
+			new_polygon.push_back(index)
+
+		# If we removed some vertices, we might be able to remove the polygon too:
+		if new_polygon.size() <= 2:
+			continue
+		polygons.push_back(new_polygon)
+
+	return polygons
+
+
+func _postprocess_nav_mesh_remove_overlapping_polygons(p_nav_mesh: NavigationMesh, p_vertices: PackedVector3Array, p_polygons: Array[PackedInt32Array]) -> void:
+	# Occasionally, a baked nav mesh comes out with overlapping polygons:
+	# https://github.com/godotengine/godot/issues/85548#issuecomment-1839341071
+	# Until the bug is fixed in the engine, this function attempts to detect and remove overlapping
+	# polygons.
+
+	# This function has to make a choice of which polygon to remove when an overlap is detected,
+	# because in this case the nav mesh is ambiguous. To do this it uses a heuristic:
+	# (1) an 'overlap' is defined as an edge that is shared by 3 or more polygons.
+	# (2) a 'bad polygon' is defined as a polygon that contains 2 or more 'overlaps'.
+	# The function removes the 'bad polygons', which in practice seems to be enough to remove all
+	# overlaps without creating holes in the nav mesh.
+
+	var cell_size: Vector3 = Vector3(p_nav_mesh.cell_size, p_nav_mesh.cell_height, p_nav_mesh.cell_size)
+
+	# `edges` is going to map edges (vertex pairs) to arrays of polygons that contain that edge.
+	var edges: Dictionary = {}
+
+	for polygon_index in range(p_polygons.size()):
+		var polygon: PackedInt32Array = p_polygons[polygon_index]
+		for j in range(polygon.size()):
+			var vertex: Vector3 = p_vertices[polygon[j]]
+			var next_vertex: Vector3 = p_vertices[polygon[(j + 1) % polygon.size()]]
+
+			# edge_key is a key we can use in the edges dictionary that uniquely identifies the
+			# edge. We use cell coordinates here (Vector3i) because with a non-power-of-two
+			# cell_size, rounding errors can cause Vector3 vertices to not be equal.
+			# Array.sort IS defined for vector types - see the Godot docs. It's necessary here
+			# because polygons that share an edge can have their vertices in a different order.
+			var edge_key: Array = [Vector3i(vertex / cell_size), Vector3i(next_vertex / cell_size)]
+			edge_key.sort()
+
+			if !edges.has(edge_key):
+				edges[edge_key] = []
+			edges[edge_key].push_back(polygon_index)
+
+	var overlap_count: Dictionary = {}
+	for connections in edges.values():
+		if connections.size() <= 2:
+			continue
+		for polygon_index in connections:
+			overlap_count[polygon_index] = overlap_count.get(polygon_index, 0) + 1
+
+	var bad_polygons: Array = []
+	for polygon_index in overlap_count.keys():
+		if overlap_count[polygon_index] >= 2:
+			bad_polygons.push_back(polygon_index)
+
+	bad_polygons.sort()
+	for i in range(bad_polygons.size() - 1, -1, -1):
+		p_polygons.remove_at(bad_polygons[i])
+
+
 func set_up_navigation_popup() -> void:
 	if plugin.terrain:
 		bake_method = _set_up_navigation