From 4cc8748c478f495b4fff665f70cdc3e941910fad Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Mon, 17 Jul 2023 12:20:09 +0200
Subject: [PATCH] Make navigation map spatial queries thread-safe
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Makes navigation map spatial queries thread-safe by adding a readers–writer lock.
---
 modules/navigation/nav_map.cpp | 33 ++++++++++++++++++++++++++++-----
 modules/navigation/nav_map.h   |  2 ++
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 6429513b5372..39da583b9dcc 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -116,7 +116,11 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 }
 
 Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
-	ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector<Vector3>(), "NavigationServer map query failed because it was made before first map synchronization.");
+	RWLockRead read_lock(map_rwlock);
+	if (map_update_id == 0) {
+		ERR_FAIL_V_MSG(Vector<Vector3>(), "NavigationServer map query failed because it was made before first map synchronization.");
+	}
+
 	// Clear metadata outputs.
 	if (r_path_types) {
 		r_path_types->clear();
@@ -576,7 +580,11 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 }
 
 Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
-	ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	RWLockRead read_lock(map_rwlock);
+	if (map_update_id == 0) {
+		ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	}
+
 	bool use_collision = p_use_collision;
 	Vector3 closest_point;
 	real_t closest_point_d = FLT_MAX;
@@ -624,24 +632,35 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
 }
 
 Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
-	ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	RWLockRead read_lock(map_rwlock);
+	if (map_update_id == 0) {
+		ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	}
 	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
 	return cp.point;
 }
 
 Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
-	ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	RWLockRead read_lock(map_rwlock);
+	if (map_update_id == 0) {
+		ERR_FAIL_V_MSG(Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
+	}
 	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
 	return cp.normal;
 }
 
 RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
-	ERR_FAIL_COND_V_MSG(map_update_id == 0, RID(), "NavigationServer map query failed because it was made before first map synchronization.");
+	RWLockRead read_lock(map_rwlock);
+	if (map_update_id == 0) {
+		ERR_FAIL_V_MSG(RID(), "NavigationServer map query failed because it was made before first map synchronization.");
+	}
 	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
 	return cp.owner;
 }
 
 gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
+	RWLockRead read_lock(map_rwlock);
+
 	gd::ClosestPointQueryResult result;
 	real_t closest_point_ds = FLT_MAX;
 
@@ -770,6 +789,8 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) {
 }
 
 Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(map_rwlock);
+
 	const LocalVector<NavRegion *> map_regions = get_regions();
 
 	if (map_regions.is_empty()) {
@@ -834,6 +855,8 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly)
 }
 
 void NavMap::sync() {
+	RWLockWrite write_lock(map_rwlock);
+
 	// Performance Monitor
 	int _new_pm_region_count = regions.size();
 	int _new_pm_agent_count = agents.size();
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index e8cbe7e2474e..21d95db5f051 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -48,6 +48,8 @@ class NavAgent;
 class NavObstacle;
 
 class NavMap : public NavRid {
+	RWLock map_rwlock;
+
 	/// Map Up
 	Vector3 up = Vector3(0, 1, 0);