Fix thread use causing navigation mesh data corruption

Fixes navigation mesh data corruption caused by thread use that changed vertices or polygons while the navigation mesh was processed, e.g. by server sync or baking.

4 files changed, 44 insertions, 31 deletions

diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp
index 6cbfd93088..430d527844 100644
--- a/modules/navigation/3d/godot_navigation_server_3d.cpp
+++ b/modules/navigation/3d/godot_navigation_server_3d.cpp
@@ -486,7 +486,7 @@ COMMAND_2(region_set_navigation_mesh, RID, p_region, Ref<NavigationMesh>, p_navi
 	NavRegion *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_NULL(region);
 
-	region->set_mesh(p_navigation_mesh);
+	region->set_navigation_mesh(p_navigation_mesh);
 }
 
 #ifndef DISABLE_DEPRECATED
diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp
index df0bdc9537..a0b3ee1cac 100644
--- a/modules/navigation/3d/nav_mesh_generator_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp
@@ -894,6 +894,7 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation
 	bake_state = "Converting to native navigation mesh..."; // step #10
 
 	Vector<Vector3> nav_vertices;
+	Vector<Vector<int>> nav_polygons;
 
 	HashMap<Vector3, int> recast_vertex_to_native_index;
 	LocalVector<int> recast_index_to_native_index;
@@ -912,8 +913,6 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation
 			recast_index_to_native_index[i] = *existing_index_ptr;
 		}
 	}
-	p_navigation_mesh->set_vertices(nav_vertices);
-	p_navigation_mesh->clear_polygons();
 
 	for (int i = 0; i < detail_mesh->nmeshes; i++) {
 		const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4];
@@ -933,10 +932,12 @@ void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<Navigation
 			nav_indices.write[1] = recast_index_to_native_index[index2];
 			nav_indices.write[2] = recast_index_to_native_index[index3];
 
-			p_navigation_mesh->add_polygon(nav_indices);
+			nav_polygons.push_back(nav_indices);
 		}
 	}
 
+	p_navigation_mesh->set_data(nav_vertices, nav_polygons);
+
 	bake_state = "Cleanup..."; // step #11
 
 	rcFreePolyMesh(poly_mesh);
diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
index 9cb235d79f..fc1db391ae 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -74,10 +74,34 @@ void NavRegion::set_transform(Transform3D p_transform) {
 	}
 	transform = p_transform;
 	polygons_dirty = true;
+
+#ifdef DEBUG_ENABLED
+	if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
+		ERR_PRINT_ONCE("Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
+	}
+#endif // DEBUG_ENABLED
 }
 
-void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) {
-	mesh = p_mesh;
+void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
+#ifdef DEBUG_ENABLED
+	if (map && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) {
+		ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size())));
+	}
+
+	if (map && !Math::is_equal_approx(double(map->get_cell_height()), double(p_navigation_mesh->get_cell_height()))) {
+		ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_height()), double(map->get_cell_height())));
+	}
+#endif // DEBUG_ENABLED
+
+	RWLockWrite write_lock(navmesh_rwlock);
+
+	pending_navmesh_vertices.clear();
+	pending_navmesh_polygons.clear();
+
+	if (p_navigation_mesh.is_valid()) {
+		p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
+	}
+
 	polygons_dirty = true;
 }
 
@@ -202,33 +226,20 @@ void NavRegion::update_polygons() {
 		return;
 	}
 
-	if (mesh.is_null()) {
-		return;
-	}
-
-#ifdef DEBUG_ENABLED
-	if (!Math::is_equal_approx(double(map->get_cell_size()), double(mesh->get_cell_size()))) {
-		ERR_PRINT_ONCE(vformat("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(mesh->get_cell_size()), double(map->get_cell_size())));
-	}
-
-	if (!Math::is_equal_approx(double(map->get_cell_height()), double(mesh->get_cell_height()))) {
-		ERR_PRINT_ONCE(vformat("Navigation map synchronization error. Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(mesh->get_cell_height()), double(map->get_cell_height())));
-	}
+	RWLockRead read_lock(navmesh_rwlock);
 
-	if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
-		ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
+	if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) {
+		return;
 	}
-#endif // DEBUG_ENABLED
 
-	Vector<Vector3> vertices = mesh->get_vertices();
-	int len = vertices.size();
+	int len = pending_navmesh_vertices.size();
 	if (len == 0) {
 		return;
 	}
 
-	const Vector3 *vertices_r = vertices.ptr();
+	const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
 
-	polygons.resize(mesh->get_polygon_count());
+	polygons.resize(pending_navmesh_polygons.size());
 
 	real_t _new_region_surface_area = 0.0;
 
@@ -238,7 +249,7 @@ void NavRegion::update_polygons() {
 		polygon.owner = this;
 		polygon.surface_area = 0.0;
 
-		Vector<int> navigation_mesh_polygon = mesh->get_polygon(navigation_mesh_polygon_index);
+		Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
 		navigation_mesh_polygon_index += 1;
 
 		int navigation_mesh_polygon_size = navigation_mesh_polygon.size();
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
index a9cfc53c7e..ebc082bd2f 100644
--- a/modules/navigation/nav_region.h
+++ b/modules/navigation/nav_region.h
@@ -34,12 +34,12 @@
 #include "nav_base.h"
 #include "nav_utils.h"
 
+#include "core/os/rw_lock.h"
 #include "scene/resources/navigation_mesh.h"
 
 class NavRegion : public NavBase {
 	NavMap *map = nullptr;
 	Transform3D transform;
-	Ref<NavigationMesh> mesh;
 	Vector<gd::Edge::Connection> connections;
 	bool enabled = true;
 
@@ -52,6 +52,10 @@ class NavRegion : public NavBase {
 
 	real_t surface_area = 0.0;
 
+	RWLock navmesh_rwlock;
+	Vector<Vector3> pending_navmesh_vertices;
+	Vector<Vector<int>> pending_navmesh_polygons;
+
 public:
 	NavRegion() {
 		type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
@@ -79,10 +83,7 @@ public:
 		return transform;
 	}
 
-	void set_mesh(Ref<NavigationMesh> p_mesh);
-	const Ref<NavigationMesh> get_mesh() const {
-		return mesh;
-	}
+	void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
 
 	Vector<gd::Edge::Connection> &get_connections() {
 		return connections;