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.

6 files changed, 86 insertions, 39 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;
diff --git a/scene/resources/navigation_mesh.cpp b/scene/resources/navigation_mesh.cpp
index 7a7bab636b..2866ae7219 100644
--- a/scene/resources/navigation_mesh.cpp
+++ b/scene/resources/navigation_mesh.cpp
@@ -35,10 +35,11 @@
 #endif // DEBUG_ENABLED
 
 void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
+	RWLockWrite write_lock(rwlock);
 	ERR_FAIL_COND(p_mesh.is_null());
 
 	vertices = Vector<Vector3>();
-	clear_polygons();
+	polygons.clear();
 
 	for (int i = 0; i < p_mesh->get_surface_count(); i++) {
 		if (p_mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
@@ -61,13 +62,12 @@ void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
 		const int *r = iarr.ptr();
 
 		for (int j = 0; j < rlen; j += 3) {
-			Vector<int> vi;
-			vi.resize(3);
-			vi.write[0] = r[j + 0] + from;
-			vi.write[1] = r[j + 1] + from;
-			vi.write[2] = r[j + 2] + from;
-
-			add_polygon(vi);
+			Polygon polygon;
+			polygon.indices.resize(3);
+			polygon.indices.write[0] = r[j + 0] + from;
+			polygon.indices.write[1] = r[j + 1] + from;
+			polygon.indices.write[2] = r[j + 2] + from;
+			polygons.push_back(polygon);
 		}
 	}
 }
@@ -304,15 +304,18 @@ Vector3 NavigationMesh::get_filter_baking_aabb_offset() const {
 }
 
 void NavigationMesh::set_vertices(const Vector<Vector3> &p_vertices) {
+	RWLockWrite write_lock(rwlock);
 	vertices = p_vertices;
 	notify_property_list_changed();
 }
 
 Vector<Vector3> NavigationMesh::get_vertices() const {
+	RWLockRead read_lock(rwlock);
 	return vertices;
 }
 
 void NavigationMesh::_set_polygons(const Array &p_array) {
+	RWLockWrite write_lock(rwlock);
 	polygons.resize(p_array.size());
 	for (int i = 0; i < p_array.size(); i++) {
 		polygons.write[i].indices = p_array[i];
@@ -321,6 +324,7 @@ void NavigationMesh::_set_polygons(const Array &p_array) {
 }
 
 Array NavigationMesh::_get_polygons() const {
+	RWLockRead read_lock(rwlock);
 	Array ret;
 	ret.resize(polygons.size());
 	for (int i = 0; i < ret.size(); i++) {
@@ -331,6 +335,7 @@ Array NavigationMesh::_get_polygons() const {
 }
 
 void NavigationMesh::add_polygon(const Vector<int> &p_polygon) {
+	RWLockWrite write_lock(rwlock);
 	Polygon polygon;
 	polygon.indices = p_polygon;
 	polygons.push_back(polygon);
@@ -338,23 +343,45 @@ void NavigationMesh::add_polygon(const Vector<int> &p_polygon) {
 }
 
 int NavigationMesh::get_polygon_count() const {
+	RWLockRead read_lock(rwlock);
 	return polygons.size();
 }
 
 Vector<int> NavigationMesh::get_polygon(int p_idx) {
+	RWLockRead read_lock(rwlock);
 	ERR_FAIL_INDEX_V(p_idx, polygons.size(), Vector<int>());
 	return polygons[p_idx].indices;
 }
 
 void NavigationMesh::clear_polygons() {
+	RWLockWrite write_lock(rwlock);
 	polygons.clear();
 }
 
 void NavigationMesh::clear() {
+	RWLockWrite write_lock(rwlock);
 	polygons.clear();
 	vertices.clear();
 }
 
+void NavigationMesh::set_data(const Vector<Vector3> &p_vertices, const Vector<Vector<int>> &p_polygons) {
+	RWLockWrite write_lock(rwlock);
+	vertices = p_vertices;
+	polygons.resize(p_polygons.size());
+	for (int i = 0; i < p_polygons.size(); i++) {
+		polygons.write[i].indices = p_polygons[i];
+	}
+}
+
+void NavigationMesh::get_data(Vector<Vector3> &r_vertices, Vector<Vector<int>> &r_polygons) {
+	RWLockRead read_lock(rwlock);
+	r_vertices = vertices;
+	r_polygons.resize(polygons.size());
+	for (int i = 0; i < polygons.size(); i++) {
+		r_polygons.write[i] = polygons[i].indices;
+	}
+}
+
 #ifdef DEBUG_ENABLED
 Ref<ArrayMesh> NavigationMesh::get_debug_mesh() {
 	if (debug_mesh.is_valid()) {
@@ -372,6 +399,8 @@ Ref<ArrayMesh> NavigationMesh::get_debug_mesh() {
 		return debug_mesh;
 	}
 
+	RWLockRead read_lock(rwlock);
+
 	int polygon_count = get_polygon_count();
 
 	if (polygon_count < 1) {
diff --git a/scene/resources/navigation_mesh.h b/scene/resources/navigation_mesh.h
index 136e1cf468..0ec2cc1bb1 100644
--- a/scene/resources/navigation_mesh.h
+++ b/scene/resources/navigation_mesh.h
@@ -31,10 +31,12 @@
 #ifndef NAVIGATION_MESH_H
 #define NAVIGATION_MESH_H
 
+#include "core/os/rw_lock.h"
 #include "scene/resources/mesh.h"
 
 class NavigationMesh : public Resource {
 	GDCLASS(NavigationMesh, Resource);
+	RWLock rwlock;
 
 	Vector<Vector3> vertices;
 	struct Polygon {
@@ -195,6 +197,9 @@ public:
 
 	void clear();
 
+	void set_data(const Vector<Vector3> &p_vertices, const Vector<Vector<int>> &p_polygons);
+	void get_data(Vector<Vector3> &r_vertices, Vector<Vector<int>> &r_polygons);
+
 #ifdef DEBUG_ENABLED
 	Ref<ArrayMesh> get_debug_mesh();
 #endif // DEBUG_ENABLED