Merge pull request #106670 from smix8/ref_iterations

Change navigation region and link updates to an async process

core/config/project_settings.cpp

@@ -1707,6 +1707,7 @@ ProjectSettings::ProjectSettings() {
 
 #if !defined(NAVIGATION_2D_DISABLED) || !defined(NAVIGATION_3D_DISABLED)
 	GLOBAL_DEF("navigation/world/map_use_async_iterations", true);
+	GLOBAL_DEF("navigation/world/region_use_async_iterations", true);
 
 	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true);
 	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true);

doc/classes/NavigationServer3D.xml

@@ -1058,6 +1058,13 @@
 				Returns the travel cost of this [param region].
 			</description>
 		</method>
+		<method name="region_get_use_async_iterations" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="region" type="RID" />
+			<description>
+				Returns [code]true[/code] if the [param region] uses an async synchronization process that runs on a background thread.
+			</description>
+		</method>
 		<method name="region_get_use_edge_connections" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="region" type="RID" />
@@ -1139,6 +1146,14 @@
 				Sets the [param travel_cost] for this [param region].
 			</description>
 		</method>
+		<method name="region_set_use_async_iterations">
+			<return type="void" />
+			<param index="0" name="region" type="RID" />
+			<param index="1" name="enabled" type="bool" />
+			<description>
+				If [param enabled] is [code]true[/code] the [param region] uses an async synchronization process that runs on a background thread.
+			</description>
+		</method>
 		<method name="region_set_use_edge_connections">
 			<return type="void" />
 			<param index="0" name="region" type="RID" />

doc/classes/ProjectSettings.xml

@@ -2341,6 +2341,9 @@
 		<member name="navigation/world/map_use_async_iterations" type="bool" setter="" getter="" default="true">
 			If enabled, navigation map synchronization uses an async process that runs on a background thread. This avoids stalling the main thread but adds an additional delay to any navigation map change.
 		</member>
+		<member name="navigation/world/region_use_async_iterations" type="bool" setter="" getter="" default="true">
+			If enabled, navigation region synchronization uses an async process that runs on a background thread. This avoids stalling the main thread but adds an additional delay to any navigation region change.
+		</member>
 		<member name="network/limits/debugger/max_chars_per_second" type="int" setter="" getter="" default="32768">
 			Maximum number of characters allowed to send as output from the debugger. Over this value, content is dropped. This helps not to stall the debugger connection.
 		</member>

modules/navigation_3d/3d/godot_navigation_server_3d.cpp

@@ -397,6 +397,19 @@ uint32_t GodotNavigationServer3D::region_get_iteration_id(RID p_region) const {
 	return region->get_iteration_id();
 }
 
+COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled) {
+	NavRegion3D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+	region->set_use_async_iterations(p_enabled);
+}
+
+bool GodotNavigationServer3D::region_get_use_async_iterations(RID p_region) const {
+	NavRegion3D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, false);
+
+	return region->get_use_async_iterations();
+}
+
 COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled) {
 	NavRegion3D *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_NULL(region);

modules/navigation_3d/3d/godot_navigation_server_3d.h

@@ -149,6 +149,9 @@ public:
 	virtual RID region_create() override;
 	virtual uint32_t region_get_iteration_id(RID p_region) const override;
 
+	COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled);
+	virtual bool region_get_use_async_iterations(RID p_region) const override;
+
 	COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
 	virtual bool region_get_enabled(RID p_region) const override;
 

modules/navigation_3d/3d/nav_base_iteration_3d.h

@@ -32,10 +32,13 @@
 
 #include "../nav_utils_3d.h"
 
+#include "core/object/ref_counted.h"
 #include "servers/navigation/navigation_utilities.h"
 
-struct NavBaseIteration3D {
-	uint32_t id = UINT32_MAX;
+class NavBaseIteration3D : public RefCounted {
+	GDCLASS(NavBaseIteration3D, RefCounted);
+
+public:
 	bool enabled = true;
 	uint32_t navigation_layers = 1;
 	real_t enter_cost = 0.0;
@@ -45,6 +48,7 @@ struct NavBaseIteration3D {
 	RID owner_rid;
 	bool owner_use_edge_connections = false;
 	LocalVector<Nav3D::Polygon> navmesh_polygons;
+	LocalVector<LocalVector<Nav3D::Connection>> internal_connections;
 
 	bool get_enabled() const { return enabled; }
 	NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
@@ -55,4 +59,10 @@ struct NavBaseIteration3D {
 	real_t get_travel_cost() const { return travel_cost; }
 	bool get_use_edge_connections() const { return owner_use_edge_connections; }
 	const LocalVector<Nav3D::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
+	const LocalVector<LocalVector<Nav3D::Connection>> &get_internal_connections() const { return internal_connections; }
+
+	virtual ~NavBaseIteration3D() {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
 };

modules/navigation_3d/3d/nav_map_builder_3d.cpp

@@ -77,26 +77,23 @@ void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild3D
 	PerformanceData &performance_data = r_build.performance_data;
 	NavMapIteration3D *map_iteration = r_build.map_iteration;
 
-	LocalVector<NavRegionIteration3D> &regions = map_iteration->region_iterations;
-	HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
+	const LocalVector<Ref<NavRegionIteration3D>> &regions = map_iteration->region_iterations;
+	HashMap<const NavBaseIteration3D *, LocalVector<Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	map_iteration->navbases_polygons_external_connections.clear();
 
 	// Remove regions connections.
 	region_external_connections.clear();
-	for (const NavRegionIteration3D &region : regions) {
-		region_external_connections[region.id] = LocalVector<Edge::Connection>();
-	}
 
 	// Copy all region polygons in the map.
 	int polygon_count = 0;
-	for (NavRegionIteration3D &region : regions) {
-		if (!region.get_enabled()) {
-			continue;
-		}
-		LocalVector<Polygon> &polygons_source = region.navmesh_polygons;
-		for (uint32_t n = 0; n < polygons_source.size(); n++) {
-			polygons_source[n].id = polygon_count;
-			polygon_count++;
-		}
+	for (const Ref<NavRegionIteration3D> &region : regions) {
+		const uint32_t polygons_size = region->navmesh_polygons.size();
+		polygon_count += polygons_size;
+
+		region_external_connections[region.ptr()] = LocalVector<Connection>();
+		map_iteration->navbases_polygons_external_connections[region.ptr()] = LocalVector<LocalVector<Connection>>();
+		map_iteration->navbases_polygons_external_connections[region.ptr()].resize(polygons_size);
 	}
 
 	performance_data.pm_polygon_count = polygon_count;
@@ -107,7 +104,7 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
 	PerformanceData &performance_data = r_build.performance_data;
 	NavMapIteration3D *map_iteration = r_build.map_iteration;
 	int polygon_count = r_build.polygon_count;
-	const Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
+
 	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
 
 	// Group all edges per key.
@@ -115,41 +112,33 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
 	connection_pairs_map.reserve(polygon_count);
 	int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
 
-	for (NavRegionIteration3D &region : map_iteration->region_iterations) {
-		if (!region.get_enabled()) {
-			continue;
-		}
-
-		for (Polygon &poly : region.navmesh_polygons) {
-			for (uint32_t p = 0; p < poly.vertices.size(); p++) {
-				const int next_point = (p + 1) % poly.vertices.size();
-				const EdgeKey ek(get_point_key(poly.vertices[p], merge_rasterizer_cell_size), get_point_key(poly.vertices[next_point], merge_rasterizer_cell_size));
+	for (const Ref<NavRegionIteration3D> &region : map_iteration->region_iterations) {
+		for (const ConnectableEdge &connectable_edge : region->get_external_edges()) {
+			const EdgeKey &ek = connectable_edge.ek;
 
-				HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
-				if (!pair_it) {
-					pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
-					performance_data.pm_edge_count += 1;
-					++free_edges_count;
+			HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
+			if (!pair_it) {
+				pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
+				performance_data.pm_edge_count += 1;
+				++free_edges_count;
+			}
+			EdgeConnectionPair &pair = pair_it->value;
+			if (pair.size < 2) {
+				// Add the polygon/edge tuple to this key.
+				Connection new_connection;
+				new_connection.polygon = &region->navmesh_polygons[connectable_edge.polygon_index];
+				new_connection.pathway_start = connectable_edge.pathway_start;
+				new_connection.pathway_end = connectable_edge.pathway_end;
+
+				pair.connections[pair.size] = new_connection;
+				++pair.size;
+				if (pair.size == 2) {
+					--free_edges_count;
 				}
-				EdgeConnectionPair &pair = pair_it->value;
-				if (pair.size < 2) {
-					// Add the polygon/edge tuple to this key.
-					Edge::Connection new_connection;
-					new_connection.polygon = &poly;
-					new_connection.edge = p;
-					new_connection.pathway_start = poly.vertices[p];
-					new_connection.pathway_end = poly.vertices[next_point];
-
-					pair.connections[pair.size] = new_connection;
-					++pair.size;
-					if (pair.size == 2) {
-						--free_edges_count;
-					}
 
-				} else {
-					// The edge is already connected with another edge, skip.
-					ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
-				}
+			} else {
+				// The edge is already connected with another edge, skip.
+				ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
 			}
 		}
 	}
@@ -161,23 +150,28 @@ void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBui
 	PerformanceData &performance_data = r_build.performance_data;
 
 	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
-	LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
+	LocalVector<Connection> &free_edges = r_build.iter_free_edges;
 	int free_edges_count = r_build.free_edge_count;
 	bool use_edge_connections = r_build.use_edge_connections;
 
 	free_edges.clear();
 	free_edges.reserve(free_edges_count);
 
+	NavMapIteration3D *map_iteration = r_build.map_iteration;
+
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
+
 	for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
 		const EdgeConnectionPair &pair = pair_it.value;
 		if (pair.size == 2) {
 			// Connect edge that are shared in different polygons.
-			const Edge::Connection &c1 = pair.connections[0];
-			const Edge::Connection &c2 = pair.connections[1];
-			c1.polygon->edges[c1.edge].connections.push_back(c2);
-			c2.polygon->edges[c2.edge].connections.push_back(c1);
-			// Note: The pathway_start/end are full for those connection and do not need to be modified.
-			performance_data.pm_edge_merge_count += 1;
+			const Connection &c1 = pair.connections[0];
+			const Connection &c2 = pair.connections[1];
+
+			navbases_polygons_external_connections[c1.polygon->owner][c1.polygon->id].push_back(c2);
+			navbases_polygons_external_connections[c2.polygon->owner][c2.polygon->id].push_back(c1);
+			performance_data.pm_edge_connection_count += 1;
+
 		} else {
 			CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
 			if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
@@ -192,8 +186,11 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 	NavMapIteration3D *map_iteration = r_build.map_iteration;
 
 	real_t edge_connection_margin = r_build.edge_connection_margin;
-	LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
-	HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	LocalVector<Connection> &free_edges = r_build.iter_free_edges;
+	HashMap<const NavBaseIteration3D *, LocalVector<Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
 
 	// Find the compatible near edges.
 	//
@@ -207,18 +204,18 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 	const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
 
 	for (uint32_t i = 0; i < free_edges.size(); i++) {
-		const Edge::Connection &free_edge = free_edges[i];
-		Vector3 edge_p1 = free_edge.polygon->vertices[free_edge.edge];
-		Vector3 edge_p2 = free_edge.polygon->vertices[(free_edge.edge + 1) % free_edge.polygon->vertices.size()];
+		const Connection &free_edge = free_edges[i];
+		const Vector3 &edge_p1 = free_edge.pathway_start;
+		const Vector3 &edge_p2 = free_edge.pathway_end;
 
 		for (uint32_t j = 0; j < free_edges.size(); j++) {
-			const Edge::Connection &other_edge = free_edges[j];
+			const Connection &other_edge = free_edges[j];
 			if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
 				continue;
 			}
 
-			Vector3 other_edge_p1 = other_edge.polygon->vertices[other_edge.edge];
-			Vector3 other_edge_p2 = other_edge.polygon->vertices[(other_edge.edge + 1) % other_edge.polygon->vertices.size()];
+			const Vector3 &other_edge_p1 = other_edge.pathway_start;
+			const Vector3 &other_edge_p2 = other_edge.pathway_end;
 
 			// Compute the projection of the opposite edge on the current one
 			Vector3 edge_vector = edge_p2 - edge_p1;
@@ -252,13 +249,14 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 			}
 
 			// The edges can now be connected.
-			Edge::Connection new_connection = other_edge;
+			Connection new_connection = other_edge;
 			new_connection.pathway_start = (self1 + other1) / 2.0;
 			new_connection.pathway_end = (self2 + other2) / 2.0;
-			free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+			//free_edge.polygon->connections.push_back(new_connection);
 
 			// Add the connection to the region_connection map.
-			region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
+			region_external_connections[free_edge.polygon->owner].push_back(new_connection);
+			navbases_polygons_external_connections[free_edge.polygon->owner][free_edge.polygon->id].push_back(new_connection);
 			performance_data.pm_edge_connection_count += 1;
 		}
 	}
@@ -269,18 +267,28 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
 
 	real_t link_connection_radius = r_build.link_connection_radius;
 
-	LocalVector<NavLinkIteration3D> &links = map_iteration->link_iterations;
+	const LocalVector<Ref<NavLinkIteration3D>> &links = map_iteration->link_iterations;
+
 	int polygon_count = r_build.polygon_count;
 
 	real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
 
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
+	LocalVector<Nav3D::Polygon> &navlink_polygons = map_iteration->navlink_polygons;
+	navlink_polygons.clear();
+	navlink_polygons.resize(links.size());
+	uint32_t navlink_index = 0;
+
 	// Search for polygons within range of a nav link.
-	for (NavLinkIteration3D &link : links) {
-		if (!link.get_enabled()) {
-			continue;
-		}
-		const Vector3 link_start_pos = link.get_start_position();
-		const Vector3 link_end_pos = link.get_end_position();
+	for (const Ref<NavLinkIteration3D> &link : links) {
+		polygon_count++;
+		Polygon &new_polygon = navlink_polygons[navlink_index++];
+
+		new_polygon.id = 0;
+		new_polygon.owner = link.ptr();
+
+		const Vector3 link_start_pos = link->get_start_position();
+		const Vector3 link_end_pos = link->get_end_position();
 
 		Polygon *closest_start_polygon = nullptr;
 		real_t closest_start_sqr_dist = link_connection_radius_sqr;
@@ -290,16 +298,13 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
 		real_t closest_end_sqr_dist = link_connection_radius_sqr;
 		Vector3 closest_end_point;
 
-		for (NavRegionIteration3D &region : map_iteration->region_iterations) {
-			if (!region.get_enabled()) {
-				continue;
-			}
-			AABB region_bounds = region.get_bounds().grow(link_connection_radius);
+		for (const Ref<NavRegionIteration3D> &region : map_iteration->region_iterations) {
+			AABB region_bounds = region->get_bounds().grow(link_connection_radius);
 			if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
 				continue;
 			}
 
-			for (Polygon &polyon : region.navmesh_polygons) {
+			for (Polygon &polyon : region->navmesh_polygons) {
 				for (uint32_t point_id = 2; point_id < polyon.vertices.size(); point_id += 1) {
 					const Face3 face(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]);
 
@@ -332,14 +337,6 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
 
 		// If we have both a start and end point, then create a synthetic polygon to route through.
 		if (closest_start_polygon && closest_end_polygon) {
-			link.navmesh_polygons.clear();
-			link.navmesh_polygons.resize(1);
-			Polygon &new_polygon = link.navmesh_polygons[0];
-			new_polygon.id = polygon_count++;
-			new_polygon.owner = &link;
-
-			new_polygon.edges.clear();
-			new_polygon.edges.resize(4);
 			new_polygon.vertices.resize(4);
 
 			// Build a set of vertices that create a thin polygon going from the start to the end point.
@@ -350,36 +347,38 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
 
 			// Setup connections to go forward in the link.
 			{
-				Edge::Connection entry_connection;
+				Connection entry_connection;
 				entry_connection.polygon = &new_polygon;
 				entry_connection.edge = -1;
 				entry_connection.pathway_start = new_polygon.vertices[0];
 				entry_connection.pathway_end = new_polygon.vertices[1];
-				closest_start_polygon->edges[0].connections.push_back(entry_connection);
+				navbases_polygons_external_connections[closest_start_polygon->owner][closest_start_polygon->id].push_back(entry_connection);
 
-				Edge::Connection exit_connection;
+				Connection exit_connection;
 				exit_connection.polygon = closest_end_polygon;
 				exit_connection.edge = -1;
 				exit_connection.pathway_start = new_polygon.vertices[2];
 				exit_connection.pathway_end = new_polygon.vertices[3];
-				new_polygon.edges[2].connections.push_back(exit_connection);
+				navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav3D::Connection>());
+				navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
 			}
 
 			// If the link is bi-directional, create connections from the end to the start.
-			if (link.is_bidirectional()) {
-				Edge::Connection entry_connection;
+			if (link->is_bidirectional()) {
+				Connection entry_connection;
 				entry_connection.polygon = &new_polygon;
 				entry_connection.edge = -1;
 				entry_connection.pathway_start = new_polygon.vertices[2];
 				entry_connection.pathway_end = new_polygon.vertices[3];
-				closest_end_polygon->edges[0].connections.push_back(entry_connection);
+				navbases_polygons_external_connections[closest_end_polygon->owner][closest_end_polygon->id].push_back(entry_connection);
 
-				Edge::Connection exit_connection;
+				Connection exit_connection;
 				exit_connection.polygon = closest_start_polygon;
 				exit_connection.edge = -1;
 				exit_connection.pathway_start = new_polygon.vertices[0];
 				exit_connection.pathway_end = new_polygon.vertices[1];
-				new_polygon.edges[0].connections.push_back(exit_connection);
+				navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav3D::Connection>());
+				navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
 			}
 		}
 	}
@@ -392,12 +391,35 @@ void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild3D &r_buil
 
 	map_iteration->navmesh_polygon_count = r_build.polygon_count;
 
+	uint32_t navmesh_polygon_count = r_build.polygon_count;
+	uint32_t total_polygon_count = navmesh_polygon_count;
+
 	map_iteration->path_query_slots_mutex.lock();
 	for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
 		p_path_query_slot.traversable_polys.clear();
-		p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
+		p_path_query_slot.traversable_polys.reserve(navmesh_polygon_count * 0.25);
 		p_path_query_slot.path_corridor.clear();
-		p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count);
+
+		p_path_query_slot.path_corridor.resize(total_polygon_count);
+
+		p_path_query_slot.poly_to_id.clear();
+		p_path_query_slot.poly_to_id.reserve(total_polygon_count);
+
+		int polygon_id = 0;
+		for (Ref<NavRegionIteration3D> &region : map_iteration->region_iterations) {
+			for (const Polygon &polygon : region->navmesh_polygons) {
+				p_path_query_slot.poly_to_id[&polygon] = polygon_id;
+				polygon_id++;
+			}
+		}
+
+		for (const Polygon &polygon : map_iteration->navlink_polygons) {
+			p_path_query_slot.poly_to_id[&polygon] = polygon_id;
+			polygon_id++;
+		}
+
+		DEV_ASSERT(p_path_query_slot.path_corridor.size() == p_path_query_slot.poly_to_id.size());
 	}
+
 	map_iteration->path_query_slots_mutex.unlock();
 }

modules/navigation_3d/3d/nav_map_iteration_3d.h

@@ -37,9 +37,9 @@
 #include "core/math/math_defs.h"
 #include "core/os/semaphore.h"
 
-struct NavLinkIteration3D;
+class NavLinkIteration3D;
 class NavRegion3D;
-struct NavRegionIteration3D;
+class NavRegionIteration3D;
 struct NavMapIteration3D;
 
 struct NavMapIterationBuild3D {
@@ -52,7 +52,7 @@ struct NavMapIterationBuild3D {
 	int free_edge_count = 0;
 
 	HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
-	LocalVector<Nav3D::Edge::Connection> iter_free_edges;
+	LocalVector<Nav3D::Connection> iter_free_edges;
 
 	NavMapIteration3D *map_iteration = nullptr;
 
@@ -76,19 +76,34 @@ struct NavMapIteration3D {
 
 	Vector3 map_up;
 
-	LocalVector<NavRegionIteration3D> region_iterations;
-	LocalVector<NavLinkIteration3D> link_iterations;
+	LocalVector<Ref<NavRegionIteration3D>> region_iterations;
+	LocalVector<Ref<NavLinkIteration3D>> link_iterations;
 
 	int navmesh_polygon_count = 0;
 
 	// The edge connections that the map builds on top with the edge connection margin.
-	HashMap<uint32_t, LocalVector<Nav3D::Edge::Connection>> external_region_connections;
+	HashMap<const NavBaseIteration3D *, LocalVector<Nav3D::Connection>> external_region_connections;
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> navbases_polygons_external_connections;
 
-	HashMap<NavRegion3D *, uint32_t> region_ptr_to_region_id;
+	LocalVector<Nav3D::Polygon> navlink_polygons;
+
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>> region_ptr_to_region_iteration;
 
 	LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
 	Mutex path_query_slots_mutex;
 	Semaphore path_query_slots_semaphore;
+
+	void clear() {
+		map_up = Vector3();
+		navmesh_polygon_count = 0;
+
+		region_iterations.clear();
+		link_iterations.clear();
+		external_region_connections.clear();
+		navbases_polygons_external_connections.clear();
+		navlink_polygons.clear();
+		region_ptr_to_region_iteration.clear();
+	}
 };
 
 class NavMapIterationRead3D {

modules/navigation_3d/3d/nav_mesh_queries_3d.cpp

@@ -233,22 +233,15 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
 	real_t begin_d = FLT_MAX;
 	real_t end_d = FLT_MAX;
 
-	const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
+	const LocalVector<Ref<NavRegionIteration3D>> &regions = p_map_iteration.region_iterations;
 
-	for (const NavRegionIteration3D &region : regions) {
-		if (!region.get_enabled()) {
-			continue;
-		}
-
-		if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) {
-			continue;
-		}
-		if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) {
+	for (const Ref<NavRegionIteration3D> &region : regions) {
+		if (!_query_task_is_connection_owner_usable(p_query_task, region.ptr())) {
 			continue;
 		}
 
 		// Find the initial poly and the end poly on this map.
-		for (const Polygon &p : region.get_navmesh_polygons()) {
+		for (const Polygon &p : region->get_navmesh_polygons()) {
 			// Only consider the polygon if it in a region with compatible layers.
 			if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
 				continue;
@@ -278,7 +271,47 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
 	}
 }
 
-void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task) {
+void NavMeshQueries3D::_query_task_search_polygon_connections(NavMeshPathQueryTask3D &p_query_task, const Connection &p_connection, uint32_t p_least_cost_id, const NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector3 &p_end_point) {
+	const NavBaseIteration3D *connection_owner = p_connection.polygon->owner;
+	ERR_FAIL_NULL(connection_owner);
+	const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner);
+	if (!owner_is_usable) {
+		return;
+	}
+
+	Heap<NavigationPoly *, NavPolyTravelCostGreaterThan, NavPolyHeapIndexer>
+			&traversable_polys = p_query_task.path_query_slot->traversable_polys;
+	LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+	real_t poly_travel_cost = p_least_cost_poly.poly->owner->get_travel_cost();
+
+	Vector3 new_entry = Geometry3D::get_closest_point_to_segment(p_least_cost_poly.entry, p_connection.pathway_start, p_connection.pathway_end);
+	real_t new_traveled_distance = p_least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + p_poly_enter_cost + p_least_cost_poly.traveled_distance;
+
+	// Check if the neighbor polygon has already been processed.
+	NavigationPoly &neighbor_poly = navigation_polys[p_query_task.path_query_slot->poly_to_id[p_connection.polygon]];
+	if (new_traveled_distance < neighbor_poly.traveled_distance) {
+		// Add the polygon to the heap of polygons to traverse next.
+		neighbor_poly.back_navigation_poly_id = p_least_cost_id;
+		neighbor_poly.back_navigation_edge = p_connection.edge;
+		neighbor_poly.back_navigation_edge_pathway_start = p_connection.pathway_start;
+		neighbor_poly.back_navigation_edge_pathway_end = p_connection.pathway_end;
+		neighbor_poly.traveled_distance = new_traveled_distance;
+		neighbor_poly.distance_to_destination =
+				new_entry.distance_to(p_end_point) *
+				connection_owner->get_travel_cost();
+		neighbor_poly.entry = new_entry;
+
+		if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
+			traversable_polys.shift(neighbor_poly.traversable_poly_index);
+		} else {
+			neighbor_poly.poly = p_connection.polygon;
+			traversable_polys.push(&neighbor_poly);
+		}
+	}
+}
+
+void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration) {
 	const Vector3 p_target_position = p_query_task.target_position;
 	const Polygon *begin_poly = p_query_task.begin_polygon;
 	const Polygon *end_poly = p_query_task.end_polygon;
@@ -296,7 +329,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	}
 
 	// Initialize the matching navigation polygon.
-	NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
+	NavigationPoly &begin_navigation_poly = navigation_polys[p_query_task.path_query_slot->poly_to_id[begin_poly]];
 	begin_navigation_poly.poly = begin_poly;
 	begin_navigation_poly.entry = begin_point;
 	begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
@@ -304,7 +337,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	begin_navigation_poly.traveled_distance = 0.f;
 
 	// This is an implementation of the A* algorithm.
-	uint32_t least_cost_id = begin_poly->id;
+	uint32_t least_cost_id = p_query_task.path_query_slot->poly_to_id[begin_poly];
 	bool found_route = false;
 
 	const Polygon *reachable_end = nullptr;
@@ -312,49 +345,29 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	bool is_reachable = true;
 	real_t poly_enter_cost = 0.0;
 
+	const HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = p_map_iteration.navbases_polygons_external_connections;
+
 	while (true) {
 		const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
-		real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
-
-		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
-		for (const Edge &edge : least_cost_poly.poly->edges) {
-			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
-			for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
-				const Edge::Connection &connection = edge.connections[connection_index];
-
-				const NavBaseIteration3D *connection_owner = connection.polygon->owner;
-				const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner);
-				if (!owner_is_usable) {
-					continue;
-				}
 
-				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, connection.pathway_start, connection.pathway_end);
-				const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
-
-				// Check if the neighbor polygon has already been processed.
-				NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
-				if (new_traveled_distance < neighbor_poly.traveled_distance) {
-					// Add the polygon to the heap of polygons to traverse next.
-					neighbor_poly.back_navigation_poly_id = least_cost_id;
-					neighbor_poly.back_navigation_edge = connection.edge;
-					neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
-					neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
-					neighbor_poly.traveled_distance = new_traveled_distance;
-					neighbor_poly.distance_to_destination =
-							new_entry.distance_to(end_point) *
-							connection_owner->get_travel_cost();
-					neighbor_poly.entry = new_entry;
-
-					if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
-						traversable_polys.shift(neighbor_poly.traversable_poly_index);
-					} else {
-						neighbor_poly.poly = connection.polygon;
-						traversable_polys.push(&neighbor_poly);
-					}
-				}
+		const NavBaseIteration3D *least_cost_navbase = least_cost_poly.poly->owner;
+
+		const uint32_t navbase_local_polygon_id = least_cost_poly.poly->id;
+		const LocalVector<LocalVector<Connection>> &navbase_polygons_to_connections = least_cost_poly.poly->owner->get_internal_connections();
+
+		if (navbase_polygons_to_connections.size() > 0) {
+			const LocalVector<Connection> &polygon_connections = navbase_polygons_to_connections[navbase_local_polygon_id];
+
+			for (const Connection &connection : polygon_connections) {
+				_query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point);
 			}
 		}
 
+		// Search region external navmesh polygon connections, aka connections to other regions created by outline edge merge or links.
+		for (const Connection &connection : navbases_polygons_external_connections[least_cost_navbase][navbase_local_polygon_id]) {
+			_query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point);
+		}
+
 		poly_enter_cost = 0;
 		// When the heap of traversable polygons is empty at this point it means the end polygon is
 		// unreachable.
@@ -370,6 +383,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			// Set as end point the furthest reachable point.
 			end_poly = reachable_end;
 			real_t end_d = FLT_MAX;
+
 			for (uint32_t point_id = 2; point_id < end_poly->vertices.size(); point_id++) {
 				Face3 f(end_poly->vertices[0], end_poly->vertices[point_id - 1], end_poly->vertices[point_id]);
 				Vector3 spoint = f.get_closest_point_to(p_target_position);
@@ -382,6 +396,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 
 			// Search all faces of start polygon as well.
 			bool closest_point_on_start_poly = false;
+
 			for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) {
 				Face3 f(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]);
 				Vector3 spoint = f.get_closest_point_to(p_target_position);
@@ -407,13 +422,14 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 				nav_poly.poly = nullptr;
 				nav_poly.traveled_distance = FLT_MAX;
 			}
-			navigation_polys[begin_poly->id].poly = begin_poly;
-			navigation_polys[begin_poly->id].traveled_distance = 0;
-			least_cost_id = begin_poly->id;
+			uint32_t _bp_id = p_query_task.path_query_slot->poly_to_id[begin_poly];
+			navigation_polys[_bp_id].poly = begin_poly;
+			navigation_polys[_bp_id].traveled_distance = 0;
+			least_cost_id = _bp_id;
 			reachable_end = nullptr;
 		} else {
 			// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
-			least_cost_id = traversable_polys.pop()->poly->id;
+			least_cost_id = p_query_task.path_query_slot->poly_to_id[traversable_polys.pop()->poly];
 
 			// Store the farthest reachable end polygon in case our goal is not reachable.
 			if (is_reachable) {
@@ -431,6 +447,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			}
 
 			if (navigation_polys[least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) {
+				ERR_FAIL_NULL(least_cost_poly.poly->owner);
 				poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
 			}
 		}
@@ -441,6 +458,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	if (!found_route) {
 		real_t end_d = FLT_MAX;
 		// Search all faces of the start polygon for the closest point to our target position.
+
 		for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) {
 			Face3 f(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]);
 			Vector3 spoint = f.get_closest_point_to(p_target_position);
@@ -483,7 +501,7 @@ void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D
 		return;
 	}
 
-	_query_task_build_path_corridor(p_query_task);
+	_query_task_build_path_corridor(p_query_task, p_map_iteration);
 
 	if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
 		return;
@@ -722,9 +740,9 @@ Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMa
 	Vector3 closest_point;
 	real_t closest_point_distance = FLT_MAX;
 
-	const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
-	for (const NavRegionIteration3D &region : regions) {
-		for (const Polygon &polygon : region.get_navmesh_polygons()) {
+	const LocalVector<Ref<NavRegionIteration3D>> &regions = p_map_iteration.region_iterations;
+	for (const Ref<NavRegionIteration3D> &region : regions) {
+		for (const Polygon &polygon : region->get_navmesh_polygons()) {
 			// For each face check the distance to the segment.
 			for (uint32_t point_id = 2; point_id < polygon.vertices.size(); point_id += 1) {
 				const Face3 face(polygon.vertices[0], polygon.vertices[point_id - 1], polygon.vertices[point_id]);
@@ -802,9 +820,9 @@ ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(c
 	ClosestPointQueryResult result;
 	real_t closest_point_distance_squared = FLT_MAX;
 
-	const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
-	for (const NavRegionIteration3D &region : regions) {
-		for (const Polygon &polygon : region.get_navmesh_polygons()) {
+	const LocalVector<Ref<NavRegionIteration3D>> &regions = p_map_iteration.region_iterations;
+	for (const Ref<NavRegionIteration3D> &region : regions) {
+		for (const Polygon &polygon : region->get_navmesh_polygons()) {
 			Vector3 plane_normal = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]);
 			Vector3 closest_on_polygon;
 			real_t closest = FLT_MAX;
@@ -881,8 +899,8 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration3D
 	accessible_regions.reserve(p_map_iteration.region_iterations.size());
 
 	for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
-		const NavRegionIteration3D &region = p_map_iteration.region_iterations[i];
-		if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
+		const Ref<NavRegionIteration3D> &region = p_map_iteration.region_iterations[i];
+		if (!region->get_enabled() || (p_navigation_layers & region->get_navigation_layers()) == 0) {
 			continue;
 		}
 		accessible_regions.push_back(i);
@@ -898,9 +916,9 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration3D
 		RBMap<real_t, uint32_t> accessible_regions_area_map;
 
 		for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
-			const NavRegionIteration3D &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
+			const Ref<NavRegionIteration3D> &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
 
-			real_t region_surface_area = region.surface_area;
+			real_t region_surface_area = region->surface_area;
 
 			if (region_surface_area == 0.0f) {
 				continue;
@@ -921,16 +939,16 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration3D
 		uint32_t random_region_index = E->value;
 		ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
 
-		const NavRegionIteration3D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+		const Ref<NavRegionIteration3D> &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
 
-		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+		return NavMeshQueries3D::polygons_get_random_point(random_region->navmesh_polygons, p_navigation_layers, p_uniformly);
 
 	} else {
 		uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
 
-		const NavRegionIteration3D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+		const Ref<NavRegionIteration3D> &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
 
-		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+		return NavMeshQueries3D::polygons_get_random_point(random_region->navmesh_polygons, p_navigation_layers, p_uniformly);
 	}
 }
 
@@ -1119,8 +1137,15 @@ void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_tas
 }
 
 bool NavMeshQueries3D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner) {
+	ERR_FAIL_NULL_V(p_owner, false);
+
 	bool owner_usable = true;
 
+	if (!p_owner->get_enabled()) {
+		owner_usable = false;
+		return owner_usable;
+	}
+
 	if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) {
 		// Not usable. No matching bit between task filter bitmask and owner bitmask.
 		owner_usable = false;

modules/navigation_3d/3d/nav_mesh_queries_3d.h

@@ -32,6 +32,8 @@
 
 #include "../nav_utils_3d.h"
 
+#include "core/templates/a_hash_map.h"
+
 #include "servers/navigation/navigation_path_query_parameters_3d.h"
 #include "servers/navigation/navigation_path_query_result_3d.h"
 #include "servers/navigation/navigation_utilities.h"
@@ -48,6 +50,7 @@ public:
 		Heap<Nav3D::NavigationPoly *, Nav3D::NavPolyTravelCostGreaterThan, Nav3D::NavPolyHeapIndexer> traversable_polys;
 		bool in_use = false;
 		uint32_t slot_index = 0;
+		AHashMap<const Nav3D::Polygon *, uint32_t> poly_to_id;
 	};
 
 	struct NavMeshPathQueryTask3D {
@@ -133,7 +136,7 @@ public:
 	static void query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
 	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const Nav3D::Polygon *p_point_polygon);
 	static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
-	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
 	static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task);
 	static void _query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task);
 	static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task);
@@ -141,6 +144,8 @@ public:
 	static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
 	static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner);
 
+	static void _query_task_search_polygon_connections(NavMeshPathQueryTask3D &p_query_task, const Nav3D::Connection &p_connection, uint32_t p_least_cost_id, const Nav3D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector3 &p_end_point);
+
 	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
 	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
 };

modules/navigation_3d/3d/nav_region_builder_3d.cpp

@@ -0,0 +1,256 @@
+/**************************************************************************/
+/*  nav_region_builder_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_region_builder_3d.h"
+
+#include "../nav_map_3d.h"
+#include "../nav_region_3d.h"
+#include "nav_region_iteration_3d.h"
+
+using namespace Nav3D;
+
+void NavRegionBuilder3D::build_iteration(NavRegionIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	performance_data.pm_polygon_count = 0;
+	performance_data.pm_edge_count = 0;
+	performance_data.pm_edge_merge_count = 0;
+	performance_data.pm_edge_connection_count = 0;
+	performance_data.pm_edge_free_count = 0;
+
+	_build_step_process_navmesh_data(r_build);
+
+	_build_step_find_edge_connection_pairs(r_build);
+
+	_build_step_merge_edge_connection_pairs(r_build);
+
+	_build_update_iteration(r_build);
+}
+
+void NavRegionBuilder3D::_build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build) {
+	Vector<Vector3> _navmesh_vertices = r_build.navmesh_data.vertices;
+	Vector<Vector<int>> _navmesh_polygons = r_build.navmesh_data.polygons;
+
+	if (_navmesh_vertices.is_empty() || _navmesh_polygons.is_empty()) {
+		return;
+	}
+
+	PerformanceData &performance_data = r_build.performance_data;
+	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
+
+	const Transform3D &region_transform = region_iteration->transform;
+	LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
+
+	const int vertex_count = _navmesh_vertices.size();
+
+	const Vector3 *vertices_ptr = _navmesh_vertices.ptr();
+	const Vector<int> *polygons_ptr = _navmesh_polygons.ptr();
+
+	navmesh_polygons.resize(_navmesh_polygons.size());
+
+	real_t _new_region_surface_area = 0.0;
+	AABB _new_region_bounds;
+
+	bool first_vertex = true;
+
+	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
+		Polygon &polygon = navmesh_polygons[i];
+		polygon.id = i;
+		polygon.owner = region_iteration.ptr();
+		polygon.surface_area = 0.0;
+
+		Vector<int> polygon_indices = polygons_ptr[i];
+
+		int polygon_size = polygon_indices.size();
+		if (polygon_size < 3) {
+			continue;
+		}
+
+		const int *indices_ptr = polygon_indices.ptr();
+
+		bool polygon_valid = true;
+
+		polygon.vertices.resize(polygon_size);
+
+		{
+			real_t _new_polygon_surface_area = 0.0;
+
+			for (int j(2); j < polygon_size; j++) {
+				const Face3 face = Face3(
+						region_transform.xform(vertices_ptr[indices_ptr[0]]),
+						region_transform.xform(vertices_ptr[indices_ptr[j - 1]]),
+						region_transform.xform(vertices_ptr[indices_ptr[j]]));
+
+				_new_polygon_surface_area += face.get_area();
+			}
+
+			polygon.surface_area = _new_polygon_surface_area;
+			_new_region_surface_area += _new_polygon_surface_area;
+		}
+
+		for (int j(0); j < polygon_size; j++) {
+			int vertex_index = indices_ptr[j];
+			if (vertex_index < 0 || vertex_index >= vertex_count) {
+				polygon_valid = false;
+				break;
+			}
+
+			const Vector3 point_position = region_transform.xform(vertices_ptr[vertex_index]);
+			polygon.vertices[j] = point_position;
+
+			if (first_vertex) {
+				first_vertex = false;
+				_new_region_bounds.position = point_position;
+			} else {
+				_new_region_bounds.expand_to(point_position);
+			}
+		}
+
+		if (!polygon_valid) {
+			polygon.surface_area = 0.0;
+			polygon.vertices.clear();
+			ERR_FAIL_COND_MSG(!polygon_valid, "Corrupted navigation mesh set on region. The indices of a polygon are out of range.");
+		}
+	}
+
+	region_iteration->surface_area = _new_region_surface_area;
+	region_iteration->bounds = _new_region_bounds;
+
+	performance_data.pm_polygon_count = navmesh_polygons.size();
+}
+
+Nav3D::PointKey NavRegionBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
+	const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
+	const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
+	const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	p.z = z;
+	return p;
+}
+
+Nav3D::EdgeKey NavRegionBuilder3D::get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size) {
+	EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size));
+	return ek;
+}
+
+void NavRegionBuilder3D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	const Vector3 &map_cell_size = r_build.map_cell_size;
+	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
+	LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+	connection_pairs_map.clear();
+
+	region_iteration->internal_connections.clear();
+	region_iteration->internal_connections.resize(navmesh_polygons.size());
+
+	region_iteration->external_edges.clear();
+
+	int free_edges_count = 0;
+
+	for (Polygon &poly : region_iteration->navmesh_polygons) {
+		for (uint32_t p = 0; p < poly.vertices.size(); p++) {
+			const int next_point = (p + 1) % poly.vertices.size();
+			const EdgeKey ek = get_edge_key(poly.vertices[p], poly.vertices[next_point], map_cell_size);
+
+			HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
+			if (!pair_it) {
+				pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
+				performance_data.pm_edge_count += 1;
+				++free_edges_count;
+			}
+			EdgeConnectionPair &pair = pair_it->value;
+			if (pair.size < 2) {
+				// Add the polygon/edge tuple to this key.
+				Connection new_connection;
+				new_connection.polygon = &poly;
+				new_connection.edge = p;
+				new_connection.pathway_start = poly.vertices[p];
+				new_connection.pathway_end = poly.vertices[next_point];
+
+				pair.connections[pair.size] = new_connection;
+				++pair.size;
+				if (pair.size == 2) {
+					--free_edges_count;
+				}
+
+			} else {
+				// The edge is already connected with another edge, skip.
+				ERR_FAIL_COND_MSG(pair.size >= 2, "Navigation region synchronization error. More than 2 edges tried to occupy the same map rasterization space. This is a logical error in the navigation mesh caused by overlap or too densely placed edges.");
+			}
+		}
+	}
+
+	performance_data.pm_edge_free_count = free_edges_count;
+}
+
+void NavRegionBuilder3D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+
+	for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
+		const EdgeConnectionPair &pair = pair_it.value;
+		if (pair.size == 2) {
+			// Connect edge that are shared in different polygons.
+			const Connection &c1 = pair.connections[0];
+			const Connection &c2 = pair.connections[1];
+			region_iteration->internal_connections[c1.polygon->id].push_back(c2);
+			region_iteration->internal_connections[c2.polygon->id].push_back(c1);
+			performance_data.pm_edge_merge_count += 1;
+
+		} else {
+			ERR_FAIL_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
+
+			const Connection &connection = pair.connections[0];
+
+			ConnectableEdge ce;
+			ce.ek = pair_it.key;
+			ce.polygon_index = connection.polygon->id;
+			ce.pathway_start = connection.pathway_start;
+			ce.pathway_end = connection.pathway_end;
+
+			region_iteration->external_edges.push_back(ce);
+		}
+	}
+}
+
+void NavRegionBuilder3D::_build_update_iteration(NavRegionIterationBuild3D &r_build) {
+	ERR_FAIL_NULL(r_build.region);
+	// Stub. End of the build.
+}

modules/navigation_3d/3d/nav_region_builder_3d.h

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  nav_region_builder_3d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_3d.h"
+
+struct NavRegionIterationBuild3D;
+
+class NavRegionBuilder3D {
+	static void _build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build);
+	static void _build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
+	static void _build_update_iteration(NavRegionIterationBuild3D &r_build);
+
+public:
+	static Nav3D::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
+	static Nav3D::EdgeKey get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size);
+
+	static void build_iteration(NavRegionIterationBuild3D &r_build);
+};

modules/navigation_3d/3d/nav_region_iteration_3d.h

@@ -32,15 +32,61 @@
 
 #include "../nav_utils_3d.h"
 #include "nav_base_iteration_3d.h"
+#include "scene/resources/navigation_mesh.h"
 
 #include "core/math/aabb.h"
 
-struct NavRegionIteration3D : NavBaseIteration3D {
+class NavRegion3D;
+class NavRegionIteration3D;
+
+struct NavRegionIterationBuild3D {
+	Nav3D::PerformanceData performance_data;
+
+	NavRegion3D *region = nullptr;
+
+	Vector3 map_cell_size;
+	Transform3D region_transform;
+
+	struct NavMeshData {
+		Vector<Vector3> vertices;
+		Vector<Vector<int>> polygons;
+
+		void clear() {
+			vertices.clear();
+			polygons.clear();
+		}
+	} navmesh_data;
+
+	Ref<NavRegionIteration3D> region_iteration;
+
+	HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
+
+	void reset() {
+		performance_data.reset();
+
+		navmesh_data.clear();
+		region_iteration = Ref<NavRegionIteration3D>();
+		iter_connection_pairs_map.clear();
+	}
+};
+
+class NavRegionIteration3D : public NavBaseIteration3D {
+	GDCLASS(NavRegionIteration3D, NavBaseIteration3D);
+
+public:
 	Transform3D transform;
 	real_t surface_area = 0.0;
 	AABB bounds;
+	LocalVector<Nav3D::ConnectableEdge> external_edges;
 
 	const Transform3D &get_transform() const { return transform; }
 	real_t get_surface_area() const { return surface_area; }
 	AABB get_bounds() const { return bounds; }
+	const LocalVector<Nav3D::ConnectableEdge> &get_external_edges() const { return external_edges; }
+
+	virtual ~NavRegionIteration3D() override {
+		external_edges.clear();
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
 };

modules/navigation_3d/nav_link_3d.cpp

@@ -44,7 +44,7 @@ void NavLink3D::set_map(NavMap3D *p_map) {
 	}
 
 	map = p_map;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	if (map) {
 		map->add_link(this);
@@ -57,9 +57,7 @@ void NavLink3D::set_enabled(bool p_enabled) {
 		return;
 	}
 	enabled = p_enabled;
-
-	// TODO: This should not require a full rebuild as the link has not really changed.
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -69,7 +67,7 @@ void NavLink3D::set_bidirectional(bool p_bidirectional) {
 		return;
 	}
 	bidirectional = p_bidirectional;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -79,7 +77,7 @@ void NavLink3D::set_start_position(const Vector3 p_position) {
 		return;
 	}
 	start_position = p_position;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -89,7 +87,7 @@ void NavLink3D::set_end_position(const Vector3 p_position) {
 		return;
 	}
 	end_position = p_position;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -99,7 +97,7 @@ void NavLink3D::set_navigation_layers(uint32_t p_navigation_layers) {
 		return;
 	}
 	navigation_layers = p_navigation_layers;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -110,7 +108,7 @@ void NavLink3D::set_enter_cost(real_t p_enter_cost) {
 		return;
 	}
 	enter_cost = new_enter_cost;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -121,7 +119,7 @@ void NavLink3D::set_travel_cost(real_t p_travel_cost) {
 		return;
 	}
 	travel_cost = new_travel_cost;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -131,21 +129,59 @@ void NavLink3D::set_owner_id(ObjectID p_owner_id) {
 		return;
 	}
 	owner_id = p_owner_id;
-	link_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
 
-bool NavLink3D::is_dirty() const {
-	return link_dirty;
+bool NavLink3D::sync() {
+	bool requires_map_update = false;
+	if (!map) {
+		return requires_map_update;
+	}
+
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
+		iteration_ready = false;
+		requires_map_update = true;
+	}
+
+	return requires_map_update;
 }
 
-void NavLink3D::sync() {
-	if (link_dirty) {
-		iteration_id = iteration_id % UINT32_MAX + 1;
+void NavLink3D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
 	}
 
-	link_dirty = false;
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
+
+	Ref<NavLinkIteration3D> new_iteration;
+	new_iteration.instantiate();
+
+	new_iteration->navigation_layers = get_navigation_layers();
+	new_iteration->enter_cost = get_enter_cost();
+	new_iteration->travel_cost = get_travel_cost();
+	new_iteration->owner_object_id = get_owner_id();
+	new_iteration->owner_type = get_type();
+	new_iteration->owner_rid = get_self();
+
+	new_iteration->enabled = get_enabled();
+	new_iteration->start_position = get_start_position();
+	new_iteration->end_position = get_end_position();
+	new_iteration->bidirectional = is_bidirectional();
+
+	RWLockWrite write_lock(iteration_rwlock);
+	ERR_FAIL_COND(iteration.is_null());
+	iteration = Ref<NavLinkIteration3D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = new_iteration;
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	iteration_building = false;
+	iteration_ready = true;
 }
 
 void NavLink3D::request_sync() {
@@ -160,27 +196,19 @@ void NavLink3D::cancel_sync_request() {
 	}
 }
 
+Ref<NavLinkIteration3D> NavLink3D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
+
 NavLink3D::NavLink3D() :
 		sync_dirty_request_list_element(this) {
 	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+	iteration.instantiate();
 }
 
 NavLink3D::~NavLink3D() {
 	cancel_sync_request();
-}
-
-void NavLink3D::get_iteration_update(NavLinkIteration3D &r_iteration) {
-	r_iteration.navigation_layers = get_navigation_layers();
-	r_iteration.enter_cost = get_enter_cost();
-	r_iteration.travel_cost = get_travel_cost();
-	r_iteration.owner_object_id = get_owner_id();
-	r_iteration.owner_type = get_type();
-	r_iteration.owner_rid = get_self();
-
-	r_iteration.enabled = get_enabled();
-	r_iteration.start_position = get_start_position();
-	r_iteration.end_position = get_end_position();
-	r_iteration.bidirectional = is_bidirectional();
 
-	r_iteration.navmesh_polygons.clear();
+	iteration = Ref<NavLinkIteration3D>();
 }

modules/navigation_3d/nav_link_3d.h

@@ -34,7 +34,10 @@
 #include "nav_base_3d.h"
 #include "nav_utils_3d.h"
 
-struct NavLinkIteration3D : NavBaseIteration3D {
+class NavLinkIteration3D : public NavBaseIteration3D {
+	GDCLASS(NavLinkIteration3D, NavBaseIteration3D);
+
+public:
 	bool bidirectional = true;
 	Vector3 start_position;
 	Vector3 end_position;
@@ -42,6 +45,11 @@ struct NavLinkIteration3D : NavBaseIteration3D {
 	Vector3 get_start_position() const { return start_position; }
 	Vector3 get_end_position() const { return end_position; }
 	bool is_bidirectional() const { return bidirectional; }
+
+	virtual ~NavLinkIteration3D() override {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
 };
 
 #include "core/templates/self_list.h"
@@ -53,12 +61,20 @@ class NavLink3D : public NavBase3D {
 	Vector3 end_position;
 	bool enabled = true;
 
-	bool link_dirty = true;
-
 	SelfList<NavLink3D> sync_dirty_request_list_element;
 
 	uint32_t iteration_id = 0;
 
+	mutable RWLock iteration_rwlock;
+	Ref<NavLinkIteration3D> iteration;
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
+
 public:
 	NavLink3D();
 	~NavLink3D();
@@ -94,10 +110,9 @@ public:
 	virtual void set_travel_cost(real_t p_travel_cost) override;
 	virtual void set_owner_id(ObjectID p_owner_id) override;
 
-	bool is_dirty() const;
-	void sync();
+	bool sync();
 	void request_sync();
 	void cancel_sync_request();
 
-	void get_iteration_update(NavLinkIteration3D &r_iteration);
+	Ref<NavLinkIteration3D> get_iteration();
 };

modules/navigation_3d/nav_map_3d.cpp

@@ -231,6 +231,8 @@ ClosestPointQueryResult NavMap3D::get_closest_point_info(const Vector3 &p_point)
 }
 
 void NavMap3D::add_region(NavRegion3D *p_region) {
+	DEV_ASSERT(!regions.has(p_region));
+
 	regions.push_back(p_region);
 	iteration_dirty = true;
 }
@@ -242,6 +244,8 @@ void NavMap3D::remove_region(NavRegion3D *p_region) {
 }
 
 void NavMap3D::add_link(NavLink3D *p_link) {
+	DEV_ASSERT(!links.has(p_link));
+
 	links.push_back(p_link);
 	iteration_dirty = true;
 }
@@ -365,49 +369,22 @@ void NavMap3D::_build_iteration() {
 	iteration_build.edge_connection_margin = get_edge_connection_margin();
 	iteration_build.link_connection_radius = get_link_connection_radius();
 
-	uint32_t enabled_region_count = 0;
-	uint32_t enabled_link_count = 0;
-
-	for (NavRegion3D *region : regions) {
-		if (!region->get_enabled()) {
-			continue;
-		}
-		enabled_region_count++;
-	}
-	for (NavLink3D *link : links) {
-		if (!link->get_enabled()) {
-			continue;
-		}
-		enabled_link_count++;
-	}
+	next_map_iteration.clear();
 
-	next_map_iteration.region_ptr_to_region_id.clear();
-
-	next_map_iteration.region_iterations.clear();
-	next_map_iteration.link_iterations.clear();
-
-	next_map_iteration.region_iterations.resize(enabled_region_count);
-	next_map_iteration.link_iterations.resize(enabled_link_count);
+	next_map_iteration.region_iterations.resize(regions.size());
+	next_map_iteration.link_iterations.resize(links.size());
 
 	uint32_t region_id_count = 0;
 	uint32_t link_id_count = 0;
 
 	for (NavRegion3D *region : regions) {
-		if (!region->get_enabled()) {
-			continue;
-		}
-		NavRegionIteration3D &region_iteration = next_map_iteration.region_iterations[region_id_count];
-		region_iteration.id = region_id_count++;
-		region->get_iteration_update(region_iteration);
-		next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id;
+		const Ref<NavRegionIteration3D> region_iteration = region->get_iteration();
+		next_map_iteration.region_iterations[region_id_count++] = region_iteration;
+		next_map_iteration.region_ptr_to_region_iteration[region] = region_iteration;
 	}
 	for (NavLink3D *link : links) {
-		if (!link->get_enabled()) {
-			continue;
-		}
-		NavLinkIteration3D &link_iteration = next_map_iteration.link_iterations[link_id_count];
-		link_iteration.id = link_id_count++;
-		link->get_iteration_update(link_iteration);
+		const Ref<NavLinkIteration3D> link_iteration = link->get_iteration();
+		next_map_iteration.link_iterations[link_id_count++] = link_iteration;
 	}
 
 	next_map_iteration.map_up = get_up();
@@ -435,9 +412,6 @@ void NavMap3D::_sync_iteration() {
 		return;
 	}
 
-	performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count;
-	performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count;
-	performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count;
 	performance_data.pm_edge_connection_count = iteration_build.performance_data.pm_edge_connection_count;
 	performance_data.pm_edge_free_count = iteration_build.performance_data.pm_edge_free_count;
 
@@ -459,6 +433,8 @@ void NavMap3D::sync() {
 	performance_data.pm_link_count = links.size();
 	performance_data.pm_obstacle_count = obstacles.size();
 
+	_sync_async_tasks();
+
 	_sync_dirty_map_update_requests();
 
 	if (iteration_dirty && !iteration_building && !iteration_ready) {
@@ -482,6 +458,16 @@ void NavMap3D::sync() {
 	map_settings_dirty = false;
 
 	_sync_avoidance();
+
+	performance_data.pm_polygon_count = 0;
+	performance_data.pm_edge_count = 0;
+	performance_data.pm_edge_merge_count = 0;
+
+	for (NavRegion3D *region : regions) {
+		performance_data.pm_polygon_count += region->get_pm_polygon_count();
+		performance_data.pm_edge_count += region->get_pm_edge_count();
+		performance_data.pm_edge_merge_count += region->get_pm_edge_merge_count();
+	}
 }
 
 void NavMap3D::_sync_avoidance() {
@@ -671,9 +657,9 @@ int NavMap3D::get_region_connections_count(NavRegion3D *p_region) const {
 
 	GET_MAP_ITERATION_CONST();
 
-	HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
 	if (found_id) {
-		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		HashMap<const NavBaseIteration3D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
 		if (found_connections) {
 			return found_connections->value.size();
 		}
@@ -687,9 +673,9 @@ Vector3 NavMap3D::get_region_connection_pathway_start(NavRegion3D *p_region, int
 
 	GET_MAP_ITERATION_CONST();
 
-	HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
 	if (found_id) {
-		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		HashMap<const NavBaseIteration3D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
 		if (found_connections) {
 			ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
 			return found_connections->value[p_connection_id].pathway_start;
@@ -704,9 +690,9 @@ Vector3 NavMap3D::get_region_connection_pathway_end(NavRegion3D *p_region, int p
 
 	GET_MAP_ITERATION_CONST();
 
-	HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
 	if (found_id) {
-		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		HashMap<const NavBaseIteration3D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
 		if (found_connections) {
 			ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
 			return found_connections->value[p_connection_id].pathway_end;
@@ -720,56 +706,60 @@ void NavMap3D::add_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_reque
 	if (p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.regions.add(p_sync_request);
+	RWLockWrite write_lock(sync_dirty_requests.regions.rwlock);
+	sync_dirty_requests.regions.list.add(p_sync_request);
 }
 
 void NavMap3D::add_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request) {
 	if (p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.links.add(p_sync_request);
+	RWLockWrite write_lock(sync_dirty_requests.links.rwlock);
+	sync_dirty_requests.links.list.add(p_sync_request);
 }
 
 void NavMap3D::add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
 	if (p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.agents.add(p_sync_request);
+	sync_dirty_requests.agents.list.add(p_sync_request);
 }
 
 void NavMap3D::add_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
 	if (p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.obstacles.add(p_sync_request);
+	sync_dirty_requests.obstacles.list.add(p_sync_request);
 }
 
 void NavMap3D::remove_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request) {
 	if (!p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.regions.remove(p_sync_request);
+	RWLockWrite write_lock(sync_dirty_requests.regions.rwlock);
+	sync_dirty_requests.regions.list.remove(p_sync_request);
 }
 
 void NavMap3D::remove_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request) {
 	if (!p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.links.remove(p_sync_request);
+	RWLockWrite write_lock(sync_dirty_requests.links.rwlock);
+	sync_dirty_requests.links.list.remove(p_sync_request);
 }
 
 void NavMap3D::remove_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
 	if (!p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.agents.remove(p_sync_request);
+	sync_dirty_requests.agents.list.remove(p_sync_request);
 }
 
 void NavMap3D::remove_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
 	if (!p_sync_request->in_list()) {
 		return;
 	}
-	sync_dirty_requests.obstacles.remove(p_sync_request);
+	sync_dirty_requests.obstacles.list.remove(p_sync_request);
 }
 
 void NavMap3D::_sync_dirty_map_update_requests() {
@@ -781,41 +771,69 @@ void NavMap3D::_sync_dirty_map_update_requests() {
 		iteration_dirty = true;
 	}
 
-	if (!iteration_dirty) {
-		iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first();
-	}
-
 	// Sync NavRegions.
-	for (SelfList<NavRegion3D> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
-		element->self()->sync();
+	RWLockWrite write_lock_regions(sync_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion3D> *element = sync_dirty_requests.regions.list.first(); element; element = element->next()) {
+		bool requires_map_update = element->self()->sync();
+		if (requires_map_update) {
+			iteration_dirty = true;
+		}
 	}
-	sync_dirty_requests.regions.clear();
+	sync_dirty_requests.regions.list.clear();
 
 	// Sync NavLinks.
-	for (SelfList<NavLink3D> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
-		element->self()->sync();
+	RWLockWrite write_lock_links(sync_dirty_requests.links.rwlock);
+	for (SelfList<NavLink3D> *element = sync_dirty_requests.links.list.first(); element; element = element->next()) {
+		bool requires_map_update = element->self()->sync();
+		if (requires_map_update) {
+			iteration_dirty = true;
+		}
 	}
-	sync_dirty_requests.links.clear();
+	sync_dirty_requests.links.list.clear();
 }
 
 void NavMap3D::_sync_dirty_avoidance_update_requests() {
 	// Sync NavAgents.
 	if (!agents_dirty) {
-		agents_dirty = sync_dirty_requests.agents.first();
+		agents_dirty = sync_dirty_requests.agents.list.first();
 	}
-	for (SelfList<NavAgent3D> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
+	for (SelfList<NavAgent3D> *element = sync_dirty_requests.agents.list.first(); element; element = element->next()) {
 		element->self()->sync();
 	}
-	sync_dirty_requests.agents.clear();
+	sync_dirty_requests.agents.list.clear();
 
 	// Sync NavObstacles.
 	if (!obstacles_dirty) {
-		obstacles_dirty = sync_dirty_requests.obstacles.first();
+		obstacles_dirty = sync_dirty_requests.obstacles.list.first();
 	}
-	for (SelfList<NavObstacle3D> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
+	for (SelfList<NavObstacle3D> *element = sync_dirty_requests.obstacles.list.first(); element; element = element->next()) {
 		element->self()->sync();
 	}
-	sync_dirty_requests.obstacles.clear();
+	sync_dirty_requests.obstacles.list.clear();
+}
+
+void NavMap3D::add_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request) {
+	if (p_async_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(async_dirty_requests.regions.rwlock);
+	async_dirty_requests.regions.list.add(p_async_request);
+}
+
+void NavMap3D::remove_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request) {
+	if (!p_async_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(async_dirty_requests.regions.rwlock);
+	async_dirty_requests.regions.list.remove(p_async_request);
+}
+
+void NavMap3D::_sync_async_tasks() {
+	// Sync NavRegions that run async thread tasks.
+	RWLockWrite write_lock_regions(async_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion3D> *element = async_dirty_requests.regions.list.first(); element; element = element->next()) {
+		element->self()->sync_async_tasks();
+	}
 }
 
 void NavMap3D::set_use_async_iterations(bool p_enabled) {
@@ -870,4 +888,9 @@ NavMap3D::~NavMap3D() {
 		WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
 		iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
 	}
+
+	RWLockWrite write_lock(iteration_slot_rwlock);
+	for (NavMapIteration3D &iteration_slot : iteration_slots) {
+		iteration_slot.clear();
+	}
 }

modules/navigation_3d/nav_map_3d.h

@@ -110,12 +110,31 @@ class NavMap3D : public NavRid3D {
 	Nav3D::PerformanceData performance_data;
 
 	struct {
-		SelfList<NavRegion3D>::List regions;
-		SelfList<NavLink3D>::List links;
-		SelfList<NavAgent3D>::List agents;
-		SelfList<NavObstacle3D>::List obstacles;
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion3D>::List list;
+		} regions;
+		struct {
+			RWLock rwlock;
+			SelfList<NavLink3D>::List list;
+		} links;
+		struct {
+			RWLock rwlock;
+			SelfList<NavAgent3D>::List list;
+		} agents;
+		struct {
+			RWLock rwlock;
+			SelfList<NavObstacle3D>::List list;
+		} obstacles;
 	} sync_dirty_requests;
 
+	struct {
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion3D>::List list;
+		} regions;
+	} async_dirty_requests;
+
 	int path_query_slots_max = 4;
 
 	bool use_async_iterations = true;
@@ -125,7 +144,6 @@ class NavMap3D : public NavRid3D {
 	mutable RWLock iteration_slot_rwlock;
 
 	NavMapIterationBuild3D iteration_build;
-	bool iteration_build_use_threads = false;
 	WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
 	static void _build_iteration_threaded(void *p_arg);
 
@@ -236,6 +254,9 @@ public:
 	Vector3 get_region_connection_pathway_start(NavRegion3D *p_region, int p_connection_id) const;
 	Vector3 get_region_connection_pathway_end(NavRegion3D *p_region, int p_connection_id) const;
 
+	void add_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request);
+	void remove_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request);
+
 	void add_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request);
 	void add_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request);
 	void add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request);
@@ -252,6 +273,7 @@ public:
 private:
 	void _sync_dirty_map_update_requests();
 	void _sync_dirty_avoidance_update_requests();
+	void _sync_async_tasks();
 
 	void compute_single_step(uint32_t index, NavAgent3D **agent);
 

modules/navigation_3d/nav_region_3d.cpp

@@ -32,9 +32,10 @@
 
 #include "nav_map_3d.h"
 
-#include "3d/nav_map_builder_3d.h"
 #include "3d/nav_mesh_queries_3d.h"
+#include "3d/nav_region_builder_3d.h"
 #include "3d/nav_region_iteration_3d.h"
+#include "core/config/project_settings.h"
 
 using namespace Nav3D;
 
@@ -43,6 +44,7 @@ void NavRegion3D::set_map(NavMap3D *p_map) {
 		return;
 	}
 
+	cancel_async_thread_join();
 	cancel_sync_request();
 
 	if (map) {
@@ -50,11 +52,14 @@ void NavRegion3D::set_map(NavMap3D *p_map) {
 	}
 
 	map = p_map;
-	polygons_dirty = true;
+	iteration_dirty = true;
 
 	if (map) {
 		map->add_region(this);
 		request_sync();
+		if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+			request_async_thread_join();
+		}
 	}
 }
 
@@ -63,9 +68,7 @@ void NavRegion3D::set_enabled(bool p_enabled) {
 		return;
 	}
 	enabled = p_enabled;
-
-	// TODO: This should not require a full rebuild as the region has not really changed.
-	polygons_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -73,7 +76,7 @@ void NavRegion3D::set_enabled(bool p_enabled) {
 void NavRegion3D::set_use_edge_connections(bool p_enabled) {
 	if (use_edge_connections != p_enabled) {
 		use_edge_connections = p_enabled;
-		polygons_dirty = true;
+		iteration_dirty = true;
 	}
 
 	request_sync();
@@ -84,7 +87,7 @@ void NavRegion3D::set_transform(Transform3D p_transform) {
 		return;
 	}
 	transform = p_transform;
-	polygons_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 
@@ -106,16 +109,9 @@ void NavRegion3D::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
 	}
 #endif // DEBUG_ENABLED
 
-	RWLockWrite write_lock(navmesh_rwlock);
-
-	pending_navmesh_vertices.clear();
-	pending_navmesh_polygons.clear();
+	navmesh = p_navigation_mesh;
 
-	if (p_navigation_mesh.is_valid()) {
-		p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
-	}
-
-	polygons_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -148,7 +144,7 @@ void NavRegion3D::set_navigation_layers(uint32_t p_navigation_layers) {
 		return;
 	}
 	navigation_layers = p_navigation_layers;
-	region_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -159,7 +155,7 @@ void NavRegion3D::set_enter_cost(real_t p_enter_cost) {
 		return;
 	}
 	enter_cost = new_enter_cost;
-	region_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -170,7 +166,7 @@ void NavRegion3D::set_travel_cost(real_t p_travel_cost) {
 		return;
 	}
 	travel_cost = new_travel_cost;
-	region_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
@@ -180,139 +176,150 @@ void NavRegion3D::set_owner_id(ObjectID p_owner_id) {
 		return;
 	}
 	owner_id = p_owner_id;
-	region_dirty = true;
+	iteration_dirty = true;
 
 	request_sync();
 }
 
-bool NavRegion3D::sync() {
-	RWLockWrite write_lock(region_rwlock);
+void NavRegion3D::scratch_polygons() {
+	iteration_dirty = true;
 
-	bool something_changed = region_dirty || polygons_dirty;
+	request_sync();
+}
+
+real_t NavRegion3D::get_surface_area() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_surface_area();
+}
 
-	region_dirty = false;
+AABB NavRegion3D::get_bounds() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_bounds();
+}
 
-	update_polygons();
+LocalVector<Nav3D::Polygon> const &NavRegion3D::get_polygons() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_navmesh_polygons();
+}
 
-	if (something_changed) {
-		iteration_id = iteration_id % UINT32_MAX + 1;
+bool NavRegion3D::sync() {
+	bool requires_map_update = false;
+	if (!map) {
+		return requires_map_update;
+	}
+
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
 	}
 
-	return something_changed;
+	if (iteration_ready) {
+		_sync_iteration();
+		requires_map_update = true;
+	}
+
+	return requires_map_update;
 }
 
-void NavRegion3D::update_polygons() {
-	if (!polygons_dirty) {
-		return;
+void NavRegion3D::sync_async_tasks() {
+	if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+		if (WorkerThreadPool::get_singleton()->is_task_completed(iteration_build_thread_task_id)) {
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+
+			iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+			iteration_building = false;
+			iteration_ready = true;
+			request_sync();
+		}
 	}
-	navmesh_polygons.clear();
-	surface_area = 0.0;
-	bounds = AABB();
-	polygons_dirty = false;
+}
 
-	if (map == nullptr) {
+void NavRegion3D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
 		return;
 	}
 
-	RWLockRead read_lock(navmesh_rwlock);
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
 
-	if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) {
-		return;
-	}
+	iteration_build.reset();
 
-	int len = pending_navmesh_vertices.size();
-	if (len == 0) {
-		return;
+	if (navmesh.is_valid()) {
+		navmesh->get_data(iteration_build.navmesh_data.vertices, iteration_build.navmesh_data.polygons);
 	}
 
-	const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
+	iteration_build.map_cell_size = map->get_merge_rasterizer_cell_size();
 
-	navmesh_polygons.resize(pending_navmesh_polygons.size());
+	Ref<NavRegionIteration3D> new_iteration;
+	new_iteration.instantiate();
 
-	real_t _new_region_surface_area = 0.0;
-	AABB _new_bounds;
+	new_iteration->navigation_layers = get_navigation_layers();
+	new_iteration->enter_cost = get_enter_cost();
+	new_iteration->travel_cost = get_travel_cost();
+	new_iteration->owner_object_id = get_owner_id();
+	new_iteration->owner_type = get_type();
+	new_iteration->owner_rid = get_self();
+	new_iteration->enabled = get_enabled();
+	new_iteration->transform = get_transform();
+	new_iteration->owner_use_edge_connections = get_use_edge_connections();
 
-	bool first_vertex = true;
-	int navigation_mesh_polygon_index = 0;
+	iteration_build.region_iteration = new_iteration;
 
-	for (Polygon &polygon : navmesh_polygons) {
-		polygon.surface_area = 0.0;
+	if (use_async_iterations) {
+		iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavRegion3D::_build_iteration_threaded, &iteration_build, true, SNAME("NavRegionBuilder3D"));
+		request_async_thread_join();
+	} else {
+		NavRegionBuilder3D::build_iteration(iteration_build);
 
-		Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
-		navigation_mesh_polygon_index += 1;
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
 
-		int navigation_mesh_polygon_size = navigation_mesh_polygon.size();
-		if (navigation_mesh_polygon_size < 3) {
-			continue;
-		}
+void NavRegion3D::_build_iteration_threaded(void *p_arg) {
+	NavRegionIterationBuild3D *_iteration_build = static_cast<NavRegionIterationBuild3D *>(p_arg);
 
-		const int *indices = navigation_mesh_polygon.ptr();
-		bool valid(true);
+	NavRegionBuilder3D::build_iteration(*_iteration_build);
+}
 
-		polygon.vertices.resize(navigation_mesh_polygon_size);
-		polygon.edges.resize(navigation_mesh_polygon_size);
+void NavRegion3D::_sync_iteration() {
+	if (iteration_building || !iteration_ready) {
+		return;
+	}
 
-		real_t _new_polygon_surface_area = 0.0;
+	performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count;
+	performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count;
+	performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count;
 
-		for (int j(2); j < navigation_mesh_polygon_size; j++) {
-			const Face3 face = Face3(
-					transform.xform(vertices_r[indices[0]]),
-					transform.xform(vertices_r[indices[j - 1]]),
-					transform.xform(vertices_r[indices[j]]));
+	RWLockWrite write_lock(iteration_rwlock);
+	ERR_FAIL_COND(iteration.is_null());
+	iteration = Ref<NavRegionIteration3D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = iteration_build.region_iteration;
+	iteration_build.region_iteration = Ref<NavRegionIteration3D>();
+	DEV_ASSERT(iteration_build.region_iteration.is_null());
+	iteration_id = iteration_id % UINT32_MAX + 1;
 
-			_new_polygon_surface_area += face.get_area();
-		}
+	iteration_ready = false;
 
-		polygon.surface_area = _new_polygon_surface_area;
-		_new_region_surface_area += _new_polygon_surface_area;
-
-		for (int j(0); j < navigation_mesh_polygon_size; j++) {
-			int idx = indices[j];
-			if (idx < 0 || idx >= len) {
-				valid = false;
-				break;
-			}
-
-			Vector3 point_position = transform.xform(vertices_r[idx]);
-			polygon.vertices[j] = point_position;
-
-			if (first_vertex) {
-				first_vertex = false;
-				_new_bounds.position = point_position;
-			} else {
-				_new_bounds.expand_to(point_position);
-			}
-		}
+	cancel_async_thread_join();
+}
 
-		if (!valid) {
-			ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
-		}
-	}
+Ref<NavRegionIteration3D> NavRegion3D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
 
-	surface_area = _new_region_surface_area;
-	bounds = _new_bounds;
+void NavRegion3D::request_async_thread_join() {
+	DEV_ASSERT(map);
+	if (map && !async_list_element.in_list()) {
+		map->add_region_async_thread_join_request(&async_list_element);
+	}
 }
 
-void NavRegion3D::get_iteration_update(NavRegionIteration3D &r_iteration) {
-	r_iteration.navigation_layers = get_navigation_layers();
-	r_iteration.enter_cost = get_enter_cost();
-	r_iteration.travel_cost = get_travel_cost();
-	r_iteration.owner_object_id = get_owner_id();
-	r_iteration.owner_type = get_type();
-	r_iteration.owner_rid = get_self();
-
-	r_iteration.enabled = get_enabled();
-	r_iteration.transform = get_transform();
-	r_iteration.owner_use_edge_connections = get_use_edge_connections();
-	r_iteration.bounds = get_bounds();
-	r_iteration.surface_area = get_surface_area();
-
-	r_iteration.navmesh_polygons.clear();
-	r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
-	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
-		Polygon &navmesh_polygon = navmesh_polygons[i];
-		navmesh_polygon.owner = &r_iteration;
-		r_iteration.navmesh_polygons[i] = navmesh_polygon;
+void NavRegion3D::cancel_async_thread_join() {
+	if (map && async_list_element.in_list()) {
+		map->remove_region_async_thread_join_request(&async_list_element);
 	}
 }
 
@@ -328,11 +335,42 @@ void NavRegion3D::cancel_sync_request() {
 	}
 }
 
+void NavRegion3D::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavRegion3D::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
 NavRegion3D::NavRegion3D() :
-		sync_dirty_request_list_element(this) {
+		sync_dirty_request_list_element(this), async_list_element(this) {
 	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
+	iteration_build.region = this;
+	iteration.instantiate();
+
+#ifdef THREADS_ENABLED
+	use_async_iterations = GLOBAL_GET("navigation/world/region_use_async_iterations");
+#else
+	use_async_iterations = false;
+#endif
 }
 
 NavRegion3D::~NavRegion3D() {
+	cancel_async_thread_join();
 	cancel_sync_request();
+
+	if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+		WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+		iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+	}
+
+	iteration_build.region = nullptr;
+	iteration_build.region_iteration = Ref<NavRegionIteration3D>();
+	iteration = Ref<NavRegionIteration3D>();
 }

modules/navigation_3d/nav_region_3d.h

@@ -36,7 +36,7 @@
 #include "core/os/rw_lock.h"
 #include "scene/resources/navigation_mesh.h"
 
-struct NavRegionIteration3D;
+#include "3d/nav_region_iteration_3d.h"
 
 class NavRegion3D : public NavBase3D {
 	RWLock region_rwlock;
@@ -47,21 +47,30 @@ class NavRegion3D : public NavBase3D {
 
 	bool use_edge_connections = true;
 
-	bool region_dirty = true;
-	bool polygons_dirty = true;
-
-	LocalVector<Nav3D::Polygon> navmesh_polygons;
-
-	real_t surface_area = 0.0;
 	AABB bounds;
 
-	RWLock navmesh_rwlock;
-	Vector<Vector3> pending_navmesh_vertices;
-	Vector<Vector<int>> pending_navmesh_polygons;
+	Ref<NavigationMesh> navmesh;
+
+	Nav3D::PerformanceData performance_data;
 
 	uint32_t iteration_id = 0;
 
 	SelfList<NavRegion3D> sync_dirty_request_list_element;
+	mutable RWLock iteration_rwlock;
+	Ref<NavRegionIteration3D> iteration;
+
+	NavRegionIterationBuild3D iteration_build;
+	bool use_async_iterations = true;
+	SelfList<NavRegion3D> async_list_element;
+	WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+	static void _build_iteration_threaded(void *p_arg);
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
 
 public:
 	NavRegion3D();
@@ -69,9 +78,7 @@ public:
 
 	uint32_t get_iteration_id() const { return iteration_id; }
 
-	void scratch_polygons() {
-		polygons_dirty = true;
-	}
+	void scratch_polygons();
 
 	void set_enabled(bool p_enabled);
 	bool get_enabled() const { return enabled; }
@@ -90,17 +97,16 @@ public:
 	}
 
 	void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
+	Ref<NavigationMesh> get_navigation_mesh() const { return navmesh; }
 
-	LocalVector<Nav3D::Polygon> const &get_polygons() const {
-		return navmesh_polygons;
-	}
+	LocalVector<Nav3D::Polygon> const &get_polygons() const;
 
 	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const;
 	Nav3D::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
 	Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
 
-	real_t get_surface_area() const { return surface_area; }
-	AABB get_bounds() const { return bounds; }
+	real_t get_surface_area() const;
+	AABB get_bounds() const;
 
 	// NavBase properties.
 	virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
@@ -112,8 +118,17 @@ public:
 	void request_sync();
 	void cancel_sync_request();
 
-	void get_iteration_update(NavRegionIteration3D &r_iteration);
+	void sync_async_tasks();
+	void request_async_thread_join();
+	void cancel_async_thread_join();
+
+	Ref<NavRegionIteration3D> get_iteration();
+
+	// Performance Monitor
+	int get_pm_polygon_count() const { return performance_data.pm_polygon_count; }
+	int get_pm_edge_count() const { return performance_data.pm_edge_count; }
+	int get_pm_edge_merge_count() const { return performance_data.pm_edge_merge_count; }
 
-private:
-	void update_polygons();
+	void set_use_async_iterations(bool p_enabled);
+	bool get_use_async_iterations() const;
 };

modules/navigation_3d/nav_utils_3d.h

@@ -31,12 +31,13 @@
 #pragma once
 
 #include "core/math/vector3.h"
+#include "core/object/ref_counted.h"
 #include "core/templates/hash_map.h"
 #include "core/templates/hashfuncs.h"
 #include "servers/navigation/nav_heap.h"
 #include "servers/navigation/navigation_utilities.h"
 
-struct NavBaseIteration3D;
+class NavBaseIteration3D;
 
 namespace Nav3D {
 struct Polygon;
@@ -72,28 +73,28 @@ struct EdgeKey {
 	}
 };
 
-struct Edge {
-	/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
-	struct Connection {
-		/// Polygon that this connection leads to.
-		Polygon *polygon = nullptr;
+struct ConnectableEdge {
+	EdgeKey ek;
+	uint32_t polygon_index;
+	Vector3 pathway_start;
+	Vector3 pathway_end;
+};
 
-		/// Edge of the source polygon where this connection starts from.
-		int edge = -1;
+struct Connection {
+	/// Polygon that this connection leads to.
+	Polygon *polygon = nullptr;
 
-		/// Point on the edge where the gateway leading to the poly starts.
-		Vector3 pathway_start;
+	/// Edge of the source polygon where this connection starts from.
+	int edge = -1;
 
-		/// Point on the edge where the gateway leading to the poly ends.
-		Vector3 pathway_end;
-	};
+	/// Point on the edge where the gateway leading to the poly starts.
+	Vector3 pathway_start;
 
-	/// Connections from this edge to other polygons.
-	LocalVector<Connection> connections;
+	/// Point on the edge where the gateway leading to the poly ends.
+	Vector3 pathway_end;
 };
 
 struct Polygon {
-	/// Id of the polygon in the map.
 	uint32_t id = UINT32_MAX;
 
 	/// Navigation region or link that contains this polygon.
@@ -101,9 +102,6 @@ struct Polygon {
 
 	LocalVector<Vector3> vertices;
 
-	/// The edges of this `Polygon`
-	LocalVector<Edge> edges;
-
 	real_t surface_area = 0.0;
 };
 
@@ -179,7 +177,7 @@ struct ClosestPointQueryResult {
 };
 
 struct EdgeConnectionPair {
-	Edge::Connection connections[2];
+	Connection connections[2];
 	int size = 0;
 };
 

servers/navigation_server_3d.cpp

@@ -84,6 +84,8 @@ void NavigationServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer3D::region_create);
 	ClassDB::bind_method(D_METHOD("region_get_iteration_id", "region"), &NavigationServer3D::region_get_iteration_id);
+	ClassDB::bind_method(D_METHOD("region_set_use_async_iterations", "region", "enabled"), &NavigationServer3D::region_set_use_async_iterations);
+	ClassDB::bind_method(D_METHOD("region_get_use_async_iterations", "region"), &NavigationServer3D::region_get_use_async_iterations);
 	ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer3D::region_set_enabled);
 	ClassDB::bind_method(D_METHOD("region_get_enabled", "region"), &NavigationServer3D::region_get_enabled);
 	ClassDB::bind_method(D_METHOD("region_set_use_edge_connections", "region", "enabled"), &NavigationServer3D::region_set_use_edge_connections);

servers/navigation_server_3d.h

@@ -109,6 +109,9 @@ public:
 	virtual RID region_create() = 0;
 	virtual uint32_t region_get_iteration_id(RID p_region) const = 0;
 
+	virtual void region_set_use_async_iterations(RID p_region, bool p_enabled) = 0;
+	virtual bool region_get_use_async_iterations(RID p_region) const = 0;
+
 	virtual void region_set_enabled(RID p_region, bool p_enabled) = 0;
 	virtual bool region_get_enabled(RID p_region) const = 0;
 

servers/navigation_server_3d_dummy.h

@@ -71,6 +71,8 @@ public:
 
 	RID region_create() override { return RID(); }
 	uint32_t region_get_iteration_id(RID p_region) const override { return 0; }
+	void region_set_use_async_iterations(RID p_region, bool p_enabled) override {}
+	bool region_get_use_async_iterations(RID p_region) const override { return false; }
 	void region_set_enabled(RID p_region, bool p_enabled) override {}
 	bool region_get_enabled(RID p_region) const override { return false; }
 	void region_set_use_edge_connections(RID p_region, bool p_enabled) override {}

tests/servers/test_navigation_server_3d.h

@@ -536,6 +536,7 @@ TEST_SUITE("[Navigation3D]") {
 		Ref<NavigationMesh> navigation_mesh = memnew(NavigationMesh);
 		navigation_server->map_set_use_async_iterations(map, false);
 		navigation_server->map_set_active(map, true);
+		navigation_server->region_set_use_async_iterations(region, false);
 		navigation_server->region_set_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
 		navigation_server->physics_process(0.0); // Give server some cycles to commit.
@@ -632,6 +633,7 @@ TEST_SUITE("[Navigation3D]") {
 		Ref<NavigationMesh> navigation_mesh = memnew(NavigationMesh);
 		navigation_server->map_set_use_async_iterations(map, false);
 		navigation_server->map_set_active(map, true);
+		navigation_server->region_set_use_async_iterations(region, false);
 		navigation_server->region_set_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
 		navigation_server->physics_process(0.0); // Give server some cycles to commit.
@@ -681,6 +683,7 @@ TEST_SUITE("[Navigation3D]") {
 		RID region = navigation_server->region_create();
 		navigation_server->map_set_active(map, true);
 		navigation_server->map_set_use_async_iterations(map, false);
+		navigation_server->region_set_use_async_iterations(region, false);
 		navigation_server->region_set_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
 		navigation_server->physics_process(0.0); // Give server some cycles to commit.