1 files changed, 45 insertions, 127 deletions

diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
index 9cb235d79f..2c91b80af2 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -32,6 +32,8 @@
 
 #include "nav_map.h"
 
+#include "3d/nav_mesh_queries_3d.h"
+
 void NavRegion::set_map(NavMap *p_map) {
 	if (map == p_map) {
 		return;
@@ -44,8 +46,6 @@ void NavRegion::set_map(NavMap *p_map) {
 	map = p_map;
 	polygons_dirty = true;
 
-	connections.clear();
-
 	if (map) {
 		map->add_region(this);
 	}
@@ -74,115 +74,63 @@ void NavRegion::set_transform(Transform3D p_transform) {
 	}
 	transform = p_transform;
 	polygons_dirty = true;
-}
-
-void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) {
-	mesh = p_mesh;
-	polygons_dirty = true;
-}
 
-int NavRegion::get_connections_count() const {
-	if (!map) {
-		return 0;
+#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.");
 	}
-	return connections.size();
-}
-
-Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const {
-	ERR_FAIL_NULL_V(map, Vector3());
-	ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
-	return connections[p_connection_id].pathway_start;
-}
-
-Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const {
-	ERR_FAIL_NULL_V(map, Vector3());
-	ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
-	return connections[p_connection_id].pathway_end;
+#endif // DEBUG_ENABLED
 }
 
-Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
-	if (!get_enabled()) {
-		return Vector3();
+void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
+#ifdef DEBUG_ENABLED
+	if (map && p_navigation_mesh.is_valid() && !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())));
 	}
 
-	const LocalVector<gd::Polygon> &region_polygons = get_polygons();
-
-	if (region_polygons.is_empty()) {
-		return Vector3();
+	if (map && p_navigation_mesh.is_valid() && !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
 
-	if (p_uniformly) {
-		real_t accumulated_area = 0;
-		RBMap<real_t, uint32_t> region_area_map;
-
-		for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
-			const gd::Polygon &region_polygon = region_polygons[rp_index];
-			real_t polyon_area = region_polygon.surface_area;
-
-			if (polyon_area == 0.0) {
-				continue;
-			}
-			region_area_map[accumulated_area] = rp_index;
-			accumulated_area += polyon_area;
-		}
-		if (region_area_map.is_empty() || accumulated_area == 0) {
-			// All polygons have no real surface / no area.
-			return Vector3();
-		}
-
-		real_t region_area_map_pos = Math::random(real_t(0), accumulated_area);
-
-		RBMap<real_t, uint32_t>::Iterator region_E = region_area_map.find_closest(region_area_map_pos);
-		ERR_FAIL_COND_V(!region_E, Vector3());
-		uint32_t rrp_polygon_index = region_E->value;
-		ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3());
-
-		const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index];
-
-		real_t accumulated_polygon_area = 0;
-		RBMap<real_t, uint32_t> polygon_area_map;
+	RWLockWrite write_lock(navmesh_rwlock);
 
-		for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) {
-			real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area();
+	pending_navmesh_vertices.clear();
+	pending_navmesh_polygons.clear();
 
-			if (face_area == 0.0) {
-				continue;
-			}
-			polygon_area_map[accumulated_polygon_area] = rpp_index;
-			accumulated_polygon_area += face_area;
-		}
-		if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) {
-			// All faces have no real surface / no area.
-			return Vector3();
-		}
-
-		real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area);
-
-		RBMap<real_t, uint32_t>::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos);
-		ERR_FAIL_COND_V(!polygon_E, Vector3());
-		uint32_t rrp_face_index = polygon_E->value;
-		ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3());
+	if (p_navigation_mesh.is_valid()) {
+		p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
+	}
 
-		const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+	polygons_dirty = true;
+}
 
-		Vector3 face_random_position = face.get_random_point_inside();
-		return face_random_position;
+Vector3 NavRegion::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
+	RWLockRead read_lock(region_rwlock);
 
-	} else {
-		uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1);
+	return NavMeshQueries3D::polygons_get_closest_point_to_segment(
+			get_polygons(), p_from, p_to, p_use_collision);
+}
 
-		const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index];
+gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
+	RWLockRead read_lock(region_rwlock);
 
-		uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
+	return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
+}
 
-		const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(region_rwlock);
 
-		Vector3 face_random_position = face.get_random_point_inside();
-		return face_random_position;
+	if (!get_enabled()) {
+		return Vector3();
 	}
+
+	return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
 }
 
 bool NavRegion::sync() {
+	RWLockWrite write_lock(region_rwlock);
+
 	bool something_changed = polygons_dirty /* || something_dirty? */;
 
 	update_polygons();
@@ -202,33 +150,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 +173,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();
@@ -266,9 +201,6 @@ void NavRegion::update_polygons() {
 		polygon.surface_area = _new_polygon_surface_area;
 		_new_region_surface_area += _new_polygon_surface_area;
 
-		Vector3 polygon_center;
-		real_t sum(0);
-
 		for (int j(0); j < navigation_mesh_polygon_size; j++) {
 			int idx = indices[j];
 			if (idx < 0 || idx >= len) {
@@ -279,25 +211,11 @@ void NavRegion::update_polygons() {
 			Vector3 point_position = transform.xform(vertices_r[idx]);
 			polygon.points[j].pos = point_position;
 			polygon.points[j].key = map->get_point_key(point_position);
-
-			polygon_center += point_position; // Composing the center of the polygon
-
-			if (j >= 2) {
-				Vector3 epa = transform.xform(vertices_r[indices[j - 2]]);
-				Vector3 epb = transform.xform(vertices_r[indices[j - 1]]);
-
-				sum += map->get_up().dot((epb - epa).cross(point_position - epa));
-			}
 		}
 
 		if (!valid) {
 			ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
 		}
-
-		polygon.clockwise = sum > 0;
-		if (!navigation_mesh_polygon.is_empty()) {
-			polygon.center = polygon_center / real_t(navigation_mesh_polygon.size());
-		}
 	}
 
 	surface_area = _new_region_surface_area;