Change navigation map synchronization to an async process

Changes the navigation map synchronization to an async process to avoid stalling the main thread.

doc/classes/NavigationServer2D.xml

@@ -562,6 +562,13 @@
 				Returns all navigation regions [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
+		<method name="map_get_use_async_iterations" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="map" type="RID" />
+			<description>
+				Returns [code]true[/code] if the [param map] synchronization uses an async process that runs on a background thread.
+			</description>
+		</method>
 		<method name="map_get_use_edge_connections" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="map" type="RID" />
@@ -608,6 +615,14 @@
 				Set the map's link connection radius used to connect links to navigation polygons.
 			</description>
 		</method>
+		<method name="map_set_use_async_iterations">
+			<return type="void" />
+			<param index="0" name="map" type="RID" />
+			<param index="1" name="enabled" type="bool" />
+			<description>
+				If [param enabled] is [code]true[/code] the [param map] synchronization uses an async process that runs on a background thread.
+			</description>
+		</method>
 		<method name="map_set_use_edge_connections">
 			<return type="void" />
 			<param index="0" name="map" type="RID" />

doc/classes/NavigationServer3D.xml

@@ -641,6 +641,13 @@
 				Returns the map's up direction.
 			</description>
 		</method>
+		<method name="map_get_use_async_iterations" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="map" type="RID" />
+			<description>
+				Returns [code]true[/code] if the [param map] synchronization uses an async process that runs on a background thread.
+			</description>
+		</method>
 		<method name="map_get_use_edge_connections" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="map" type="RID" />
@@ -711,6 +718,14 @@
 				Sets the map up direction.
 			</description>
 		</method>
+		<method name="map_set_use_async_iterations">
+			<return type="void" />
+			<param index="0" name="map" type="RID" />
+			<param index="1" name="enabled" type="bool" />
+			<description>
+				If [param enabled] is [code]true[/code] the [param map] synchronization uses an async process that runs on a background thread.
+			</description>
+		</method>
 		<method name="map_set_use_edge_connections">
 			<return type="void" />
 			<param index="0" name="map" type="RID" />

doc/classes/ProjectSettings.xml

@@ -2165,6 +2165,9 @@
 		<member name="navigation/pathfinding/max_threads" type="int" setter="" getter="" default="4">
 			Maximum number of threads that can run pathfinding queries simultaneously on the same pathfinding graph, for example the same navigation map. Additional threads increase memory consumption and synchronization time due to the need for extra data copies prepared for each thread. A value of [code]-1[/code] means unlimited and the maximum available OS processor count is used. Defaults to [code]1[/code] when the OS does not support threads.
 		</member>
+		<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="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/2d/godot_navigation_server_2d.cpp

@@ -259,6 +259,14 @@ uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
 	return NavigationServer3D::get_singleton()->map_get_iteration_id(p_map);
 }
 
+void GodotNavigationServer2D::map_set_use_async_iterations(RID p_map, bool p_enabled) {
+	return NavigationServer3D::get_singleton()->map_set_use_async_iterations(p_map, p_enabled);
+}
+
+bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
+	return NavigationServer3D::get_singleton()->map_get_use_async_iterations(p_map);
+}
+
 void FORWARD_2(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real);
 real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid);
 

modules/navigation/2d/godot_navigation_server_2d.h

@@ -78,6 +78,8 @@ public:
 	virtual void map_force_update(RID p_map) override;
 	virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
 	virtual uint32_t map_get_iteration_id(RID p_map) const override;
+	virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) override;
+	virtual bool map_get_use_async_iterations(RID p_map) const override;
 
 	virtual RID region_create() override;
 	virtual void region_set_enabled(RID p_region, bool p_enabled) override;

modules/navigation/3d/godot_navigation_server_3d.cpp

@@ -364,6 +364,19 @@ RID GodotNavigationServer3D::agent_get_map(RID p_agent) const {
 	return RID();
 }
 
+COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled) {
+	NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+	map->set_use_async_iterations(p_enabled);
+}
+
+bool GodotNavigationServer3D::map_get_use_async_iterations(RID p_map) const {
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, false);
+
+	return map->get_use_async_iterations();
+}
+
 Vector3 GodotNavigationServer3D::map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const {
 	const NavMap *map = map_owner.get_or_null(p_map);
 	ERR_FAIL_NULL_V(map, Vector3());

modules/navigation/3d/godot_navigation_server_3d.h

@@ -147,6 +147,9 @@ public:
 	virtual void map_force_update(RID p_map) override;
 	virtual uint32_t map_get_iteration_id(RID p_map) const override;
 
+	COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled);
+	virtual bool map_get_use_async_iterations(RID p_map) const override;
+
 	virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
 
 	virtual RID region_create() override;

modules/navigation/3d/nav_base_iteration_3d.h

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  nav_base_iteration_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.                 */
+/**************************************************************************/
+
+#ifndef NAV_BASE_ITERATION_3D_H
+#define NAV_BASE_ITERATION_3D_H
+
+#include "servers/navigation/navigation_utilities.h"
+
+struct NavBaseIteration {
+	uint32_t id = UINT32_MAX;
+	bool enabled = true;
+	uint32_t navigation_layers = 1;
+	real_t enter_cost = 0.0;
+	real_t travel_cost = 1.0;
+	NavigationUtilities::PathSegmentType owner_type;
+	ObjectID owner_object_id;
+	RID owner_rid;
+	bool owner_use_edge_connections = false;
+};
+
+#endif // NAV_BASE_ITERATION_3D_H

modules/navigation/3d/nav_map_builder_3d.cpp

@@ -0,0 +1,399 @@
+/**************************************************************************/
+/*  nav_map_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.                 */
+/**************************************************************************/
+
+#ifndef _3D_DISABLED
+
+#include "nav_map_builder_3d.h"
+
+#include "../nav_link.h"
+#include "../nav_map.h"
+#include "../nav_region.h"
+#include "nav_map_iteration_3d.h"
+#include "nav_region_iteration_3d.h"
+
+gd::PointKey NavMapBuilder3D::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));
+
+	gd::PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	p.z = z;
+	return p;
+}
+
+void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
+	_build_step_gather_region_polygons(r_build);
+
+	_build_step_find_edge_connection_pairs(r_build);
+
+	_build_step_merge_edge_connection_pairs(r_build);
+
+	_build_step_edge_connection_margin_connections(r_build);
+
+	_build_step_navlink_connections(r_build);
+
+	_build_update_map_iteration(r_build);
+}
+
+void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r_build) {
+	NavMapIteration *map_iteration = r_build.map_iteration;
+	gd::PerformanceData &performance_data = r_build.performance_data;
+	int polygon_count = r_build.polygon_count;
+	int navmesh_polygon_count = r_build.navmesh_polygon_count;
+
+	// Remove regions connections.
+	map_iteration->external_region_connections.clear();
+	for (const NavRegionIteration &region : map_iteration->region_iterations) {
+		map_iteration->external_region_connections[region.id] = LocalVector<gd::Edge::Connection>();
+	}
+
+	polygon_count = 0;
+	navmesh_polygon_count = 0;
+	for (NavRegionIteration &region : map_iteration->region_iterations) {
+		for (gd::Polygon &region_polygon : region.navmesh_polygons) {
+			region_polygon.id = polygon_count;
+			region_polygon.owner = &region;
+
+			polygon_count++;
+			navmesh_polygon_count++;
+		}
+	}
+
+	performance_data.pm_polygon_count = polygon_count;
+	r_build.polygon_count = polygon_count;
+	r_build.navmesh_polygon_count = navmesh_polygon_count;
+}
+
+void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build) {
+	NavMapIteration *map_iteration = r_build.map_iteration;
+	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &iter_connection_pairs_map = r_build.iter_connection_pairs_map;
+	gd::PerformanceData &performance_data = r_build.performance_data;
+	int free_edge_count = r_build.free_edge_count;
+
+	iter_connection_pairs_map.clear();
+	iter_connection_pairs_map.reserve(map_iteration->region_iterations.size());
+
+	for (NavRegionIteration &region : map_iteration->region_iterations) {
+		for (gd::Polygon &region_polygon : region.navmesh_polygons) {
+			for (uint32_t p = 0; p < region_polygon.points.size(); p++) {
+				const int next_point = (p + 1) % region_polygon.points.size();
+				const gd::EdgeKey ek(region_polygon.points[p].key, region_polygon.points[next_point].key);
+
+				HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = iter_connection_pairs_map.find(ek);
+				if (!pair_it) {
+					pair_it = iter_connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
+					performance_data.pm_edge_count += 1;
+					++free_edge_count;
+				}
+				gd::EdgeConnectionPair &pair = pair_it->value;
+				if (pair.size < 2) {
+					pair.connections[pair.size].polygon = &region_polygon;
+					pair.connections[pair.size].edge = p;
+					pair.connections[pair.size].pathway_start = region_polygon.points[p].pos;
+					pair.connections[pair.size].pathway_end = region_polygon.points[next_point].pos;
+					++pair.size;
+					if (pair.size == 2) {
+						--free_edge_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.");
+				}
+			}
+		}
+	}
+	r_build.free_edge_count = free_edge_count;
+}
+
+void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build) {
+	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &iter_connection_pairs_map = r_build.iter_connection_pairs_map;
+	LocalVector<gd::Edge::Connection> &iter_free_edges = r_build.iter_free_edges;
+	bool use_edge_connections = r_build.use_edge_connections;
+	gd::PerformanceData &performance_data = r_build.performance_data;
+
+	iter_free_edges.clear();
+	iter_free_edges.resize(r_build.free_edge_count);
+	uint32_t iter_free_edge_index = 0;
+
+	for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : iter_connection_pairs_map) {
+		const gd::EdgeConnectionPair &pair = pair_it.value;
+
+		if (pair.size == 2) {
+			// Connect edge that are shared in different polygons.
+			const gd::Edge::Connection &c1 = pair.connections[0];
+			const gd::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;
+		} 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->owner_use_edge_connections) {
+				iter_free_edges[iter_free_edge_index++] = pair.connections[0];
+			}
+		}
+	}
+
+	iter_free_edges.resize(iter_free_edge_index);
+}
+
+void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build) {
+	NavMapIteration *map_iteration = r_build.map_iteration;
+	const LocalVector<gd::Edge::Connection> &iter_free_edges = r_build.iter_free_edges;
+	bool use_edge_connections = r_build.use_edge_connections;
+	gd::PerformanceData &performance_data = r_build.performance_data;
+	const real_t edge_connection_margin = r_build.edge_connection_margin;
+	// Find the compatible near edges.
+	//
+	// Note:
+	// Considering that the edges must be compatible (for obvious reasons)
+	// to be connected, create new polygons to remove that small gap is
+	// not really useful and would result in wasteful computation during
+	// connection, integration and path finding.
+
+	performance_data.pm_edge_free_count = iter_free_edges.size();
+
+	if (!use_edge_connections) {
+		return;
+	}
+
+	const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
+
+	for (uint32_t i = 0; i < iter_free_edges.size(); i++) {
+		const gd::Edge::Connection &free_edge = iter_free_edges[i];
+
+		Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+		Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+		Vector3 edge_vector = edge_p2 - edge_p1;
+		real_t edge_vector_length_squared = edge_vector.length_squared();
+
+		for (uint32_t j = 0; j < iter_free_edges.size(); j++) {
+			const gd::Edge::Connection &other_edge = iter_free_edges[j];
+			if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+				continue;
+			}
+
+			Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
+			Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
+
+			// Compute the projection of the opposite edge on the current one
+			real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector_length_squared);
+			real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector_length_squared);
+			if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
+				continue;
+			}
+
+			// Check if the two edges are close to each other enough and compute a pathway between the two regions.
+			Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+			Vector3 other1;
+			if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+				other1 = other_edge_p1;
+			} else {
+				other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+			}
+			if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
+				continue;
+			}
+
+			Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+			Vector3 other2;
+			if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+				other2 = other_edge_p2;
+			} else {
+				other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+			}
+			if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
+				continue;
+			}
+
+			// The edges can now be connected.
+			gd::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);
+
+			// Add the connection to the region_connection map.
+			map_iteration->external_region_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
+			performance_data.pm_edge_connection_count += 1;
+		}
+	}
+}
+
+void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_build) {
+	NavMapIteration *map_iteration = r_build.map_iteration;
+	const Vector3 &merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
+	real_t link_connection_radius = r_build.link_connection_radius;
+	real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
+	int polygon_count = r_build.polygon_count;
+	int link_polygon_count = r_build.link_polygon_count;
+
+	// Search for polygons within range of a nav link.
+	for (NavLinkIteration &link : map_iteration->link_iterations) {
+		if (!link.enabled) {
+			continue;
+		}
+		const Vector3 link_start_pos = link.start_position;
+		const Vector3 link_end_pos = link.end_position;
+
+		gd::Polygon *closest_start_polygon = nullptr;
+		real_t closest_start_sqr_dist = link_connection_radius_sqr;
+		Vector3 closest_start_point;
+
+		gd::Polygon *closest_end_polygon = nullptr;
+		real_t closest_end_sqr_dist = link_connection_radius_sqr;
+		Vector3 closest_end_point;
+
+		for (NavRegionIteration &region : map_iteration->region_iterations) {
+			AABB region_bounds = region.bounds.grow(link_connection_radius);
+			if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
+				continue;
+			}
+			for (gd::Polygon &polyon : region.navmesh_polygons) {
+				for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
+					const Face3 face(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
+
+					{
+						const Vector3 start_point = face.get_closest_point_to(link_start_pos);
+						const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
+
+						// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+						if (sqr_dist < closest_start_sqr_dist) {
+							closest_start_sqr_dist = sqr_dist;
+							closest_start_point = start_point;
+							closest_start_polygon = &polyon;
+						}
+					}
+
+					{
+						const Vector3 end_point = face.get_closest_point_to(link_end_pos);
+						const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
+
+						// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+						if (sqr_dist < closest_end_sqr_dist) {
+							closest_end_sqr_dist = sqr_dist;
+							closest_end_point = end_point;
+							closest_end_polygon = &polyon;
+						}
+					}
+				}
+			}
+		}
+
+		// 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.resize(1);
+			gd::Polygon &new_polygon = link.navmesh_polygons[0];
+			new_polygon.id = polygon_count++;
+			new_polygon.owner = &link;
+
+			link_polygon_count++;
+
+			new_polygon.edges.clear();
+			new_polygon.edges.resize(4);
+			new_polygon.points.resize(4);
+
+			// Build a set of vertices that create a thin polygon going from the start to the end point.
+			new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+			new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+			new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+			new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+
+			// Setup connections to go forward in the link.
+			{
+				gd::Edge::Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.points[0].pos;
+				entry_connection.pathway_end = new_polygon.points[1].pos;
+				closest_start_polygon->edges[0].connections.push_back(entry_connection);
+
+				gd::Edge::Connection exit_connection;
+				exit_connection.polygon = closest_end_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.points[2].pos;
+				exit_connection.pathway_end = new_polygon.points[3].pos;
+				new_polygon.edges[2].connections.push_back(exit_connection);
+			}
+
+			// If the link is bi-directional, create connections from the end to the start.
+			if (link.bidirectional) {
+				gd::Edge::Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.points[2].pos;
+				entry_connection.pathway_end = new_polygon.points[3].pos;
+				closest_end_polygon->edges[0].connections.push_back(entry_connection);
+
+				gd::Edge::Connection exit_connection;
+				exit_connection.polygon = closest_start_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.points[0].pos;
+				exit_connection.pathway_end = new_polygon.points[1].pos;
+				new_polygon.edges[0].connections.push_back(exit_connection);
+			}
+		}
+	}
+
+	r_build.polygon_count = polygon_count;
+	r_build.link_polygon_count = link_polygon_count;
+}
+
+void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) {
+	NavMapIteration *map_iteration = r_build.map_iteration;
+
+	map_iteration->navmesh_polygon_count = r_build.navmesh_polygon_count;
+	map_iteration->link_polygon_count = r_build.link_polygon_count;
+
+	// TODO: This copying is for compatibility with legacy functions that expect a big polygon soup array.
+	// Those functions should be changed to work hierarchical with the region iteration polygons directly.
+	map_iteration->navmesh_polygons.resize(map_iteration->navmesh_polygon_count);
+	uint32_t polygon_index = 0;
+	for (NavRegionIteration &region : map_iteration->region_iterations) {
+		for (gd::Polygon &region_polygon : region.navmesh_polygons) {
+			map_iteration->navmesh_polygons[polygon_index++] = region_polygon;
+		}
+	}
+
+	map_iteration->path_query_slots_mutex.lock();
+	for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
+		p_path_query_slot.path_corridor.clear();
+		p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
+		p_path_query_slot.traversable_polys.clear();
+		p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
+	}
+	map_iteration->path_query_slots_mutex.unlock();
+}
+
+#endif // _3D_DISABLED

modules/navigation/3d/nav_map_builder_3d.h

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  nav_map_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.                 */
+/**************************************************************************/
+
+#ifndef NAV_MAP_BUILDER_3D_H
+#define NAV_MAP_BUILDER_3D_H
+
+#include "../nav_utils.h"
+
+struct NavMapIterationBuild;
+
+class NavMapBuilder3D {
+	static void _build_step_gather_region_polygons(NavMapIterationBuild &r_build);
+	static void _build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build);
+	static void _build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build);
+	static void _build_step_navlink_connections(NavMapIterationBuild &r_build);
+	static void _build_update_map_iteration(NavMapIterationBuild &r_build);
+
+public:
+	static gd::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
+
+	static void build_navmap_iteration(NavMapIterationBuild &r_build);
+};
+
+#endif // NAV_MAP_BUILDER_3D_H

modules/navigation/3d/nav_map_iteration_3d.h

@@ -0,0 +1,114 @@
+/**************************************************************************/
+/*  nav_map_iteration_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.                 */
+/**************************************************************************/
+
+#ifndef NAV_MAP_ITERATION_3D_H
+#define NAV_MAP_ITERATION_3D_H
+
+#include "../nav_rid.h"
+#include "../nav_utils.h"
+#include "nav_mesh_queries_3d.h"
+
+#include "core/math/math_defs.h"
+#include "core/os/semaphore.h"
+
+struct NavLinkIteration;
+class NavRegion;
+struct NavRegionIteration;
+struct NavMapIteration;
+
+struct NavMapIterationBuild {
+	Vector3 merge_rasterizer_cell_size;
+	bool use_edge_connections = true;
+	real_t edge_connection_margin;
+	real_t link_connection_radius;
+	gd::PerformanceData performance_data;
+	int polygon_count = 0;
+	int free_edge_count = 0;
+
+	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> iter_connection_pairs_map;
+	LocalVector<gd::Edge::Connection> iter_free_edges;
+
+	NavMapIteration *map_iteration = nullptr;
+
+	int navmesh_polygon_count = 0;
+	int link_polygon_count = 0;
+
+	void reset() {
+		performance_data.reset();
+
+		iter_connection_pairs_map.clear();
+		iter_free_edges.clear();
+		polygon_count = 0;
+		free_edge_count = 0;
+
+		navmesh_polygon_count = 0;
+		link_polygon_count = 0;
+	}
+};
+
+struct NavMapIteration {
+	mutable SafeNumeric<uint32_t> users;
+	RWLock rwlock;
+
+	Vector3 map_up;
+	LocalVector<gd::Polygon> navmesh_polygons;
+
+	LocalVector<NavRegionIteration> region_iterations;
+	LocalVector<NavLinkIteration> link_iterations;
+
+	int navmesh_polygon_count = 0;
+	int link_polygon_count = 0;
+
+	// The edge connections that the map builds on top with the edge connection margin.
+	HashMap<uint32_t, LocalVector<gd::Edge::Connection>> external_region_connections;
+
+	HashMap<NavRegion *, uint32_t> region_ptr_to_region_id;
+
+	LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
+	Mutex path_query_slots_mutex;
+	Semaphore path_query_slots_semaphore;
+};
+
+class NavMapIterationRead {
+	const NavMapIteration &map_iteration;
+
+public:
+	_ALWAYS_INLINE_ NavMapIterationRead(const NavMapIteration &p_iteration) :
+			map_iteration(p_iteration) {
+		map_iteration.rwlock.read_lock();
+		map_iteration.users.increment();
+	}
+	_ALWAYS_INLINE_ ~NavMapIterationRead() {
+		map_iteration.users.decrement();
+		map_iteration.rwlock.read_unlock();
+	}
+};
+
+#endif // NAV_MAP_ITERATION_3D_H

modules/navigation/3d/nav_mesh_queries_3d.cpp

@@ -34,6 +34,7 @@
 
 #include "../nav_base.h"
 #include "../nav_map.h"
+#include "nav_region_iteration_3d.h"
 
 #include "core/math/geometry_3d.h"
 #include "servers/navigation/navigation_utilities.h"
@@ -128,41 +129,41 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
 	}
 }
 
-void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
+void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *p_begin_polygon, const gd::Polygon *p_end_polygon) {
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
 		p_query_task.path_meta_point_types.resize(2);
-		p_query_task.path_meta_point_types[0] = begin_poly->owner->get_type();
-		p_query_task.path_meta_point_types[1] = end_poly->owner->get_type();
+		p_query_task.path_meta_point_types[0] = p_begin_polygon->owner->owner_type;
+		p_query_task.path_meta_point_types[1] = p_end_polygon->owner->owner_type;
 	}
 
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
 		p_query_task.path_meta_point_rids.resize(2);
-		p_query_task.path_meta_point_rids[0] = begin_poly->owner->get_self();
-		p_query_task.path_meta_point_rids[1] = end_poly->owner->get_self();
+		p_query_task.path_meta_point_rids[0] = p_begin_polygon->owner->owner_rid;
+		p_query_task.path_meta_point_rids[1] = p_end_polygon->owner->owner_rid;
 	}
 
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
 		p_query_task.path_meta_point_owners.resize(2);
-		p_query_task.path_meta_point_owners[0] = begin_poly->owner->get_owner_id();
-		p_query_task.path_meta_point_owners[1] = end_poly->owner->get_owner_id();
+		p_query_task.path_meta_point_owners[0] = p_begin_polygon->owner->owner_object_id;
+		p_query_task.path_meta_point_owners[1] = p_end_polygon->owner->owner_object_id;
 	}
 
 	p_query_task.path_points.resize(2);
-	p_query_task.path_points[0] = begin_point;
-	p_query_task.path_points[1] = end_point;
+	p_query_task.path_points[0] = p_query_task.begin_position;
+	p_query_task.path_points[1] = p_query_task.end_position;
 }
 
-void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) {
+void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon) {
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
-		p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
+		p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->owner_type);
 	}
 
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
-		p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
+		p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->owner_rid);
 	}
 
 	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
-		p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
+		p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->owner_object_id);
 	}
 
 	p_query_task.path_points.push_back(p_point);
@@ -267,33 +268,27 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
 	}
 }
 
-void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
+void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
 	p_query_task.path_points.clear();
 	p_query_task.path_meta_point_types.clear();
 	p_query_task.path_meta_point_rids.clear();
 	p_query_task.path_meta_point_owners.clear();
 
-	// Find begin polyon and begin position closest to start position and
-	// end polyon and end position closest to target position on the map.
-	const gd::Polygon *begin_poly = nullptr;
-	const gd::Polygon *end_poly = nullptr;
-	Vector3 begin_point;
-	Vector3 end_point;
-
-	_query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point);
+	_query_task_find_start_end_positions(p_query_task, p_polygons);
 
 	// Check for trivial cases
-	if (!begin_poly || !end_poly) {
+	if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
 		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
 		return;
 	}
 
-	if (begin_poly == end_poly) {
-		_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
+	if (p_query_task.begin_polygon == p_query_task.end_polygon) {
+		_query_task_create_same_polygon_two_point_path(p_query_task, p_query_task.begin_polygon, p_query_task.end_polygon);
 		return;
 	}
 
-	_query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point);
+	DEV_ASSERT(p_query_task.path_query_slot->path_corridor.size() == p_polygons.size() + p_query_task.link_polygons_size);
+	_query_task_build_path_corridor(p_query_task, p_polygons);
 
 	if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
 		return;
@@ -302,17 +297,17 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
 	// Post-Process path.
 	switch (p_query_task.path_postprocessing) {
 		case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
-			_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
+			_query_task_post_process_corridorfunnel(p_query_task);
 		} break;
 		case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
-			_path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
+			_query_task_post_process_edgecentered(p_query_task);
 		} break;
 		case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
-			_path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
+			_query_task_post_process_nopostprocessing(p_query_task);
 		} break;
 		default: {
 			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
-			_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
+			_query_task_post_process_corridorfunnel(p_query_task);
 		} break;
 	}
 
@@ -343,21 +338,25 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
 	p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 }
 
-void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
+void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
+	const gd::Polygon *begin_polygon = p_query_task.begin_polygon;
+	const gd::Polygon *end_polygon = p_query_task.end_polygon;
+	const Vector3 &begin_position = p_query_task.begin_position;
+	Vector3 &end_position = p_query_task.end_position;
 	// List of all reachable navigation polys.
 	LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
 	for (gd::NavigationPoly &polygon : navigation_polys) {
 		polygon.reset();
 	}
 
-	DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_link_polygons_size);
+	DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_query_task.link_polygons_size);
 
 	// Initialize the matching navigation polygon.
-	gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
-	begin_navigation_poly.poly = begin_poly;
-	begin_navigation_poly.entry = begin_point;
-	begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
-	begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+	gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_polygon->id];
+	begin_navigation_poly.poly = begin_polygon;
+	begin_navigation_poly.entry = begin_position;
+	begin_navigation_poly.back_navigation_edge_pathway_start = begin_position;
+	begin_navigation_poly.back_navigation_edge_pathway_end = begin_position;
 
 	// Heap of polygons to travel next.
 	gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
@@ -366,7 +365,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	traversable_polys.reserve(p_polygons.size() * 0.25);
 
 	// This is an implementation of the A* algorithm.
-	p_query_task.least_cost_id = begin_poly->id;
+	p_query_task.least_cost_id = begin_polygon->id;
 	int prev_least_cost_id = -1;
 	bool found_route = false;
 
@@ -382,16 +381,16 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 				const gd::Edge::Connection &connection = edge.connections[connection_index];
 
 				// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
-				if ((p_query_task.navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
+				if ((p_query_task.navigation_layers & connection.polygon->owner->navigation_layers) == 0) {
 					continue;
 				}
 
 				const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id];
 				real_t poly_enter_cost = 0.0;
-				real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
+				real_t poly_travel_cost = least_cost_poly.poly->owner->travel_cost;
 
-				if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) {
-					poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
+				if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->owner_rid != least_cost_poly.poly->owner->owner_rid) {
+					poly_enter_cost = least_cost_poly.poly->owner->enter_cost;
 				}
 				prev_least_cost_id = p_query_task.least_cost_id;
 
@@ -412,8 +411,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 						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) *
-								neighbor_poly.poly->owner->get_travel_cost();
+								new_entry.distance_to(end_position) *
+								neighbor_poly.poly->owner->travel_cost;
 						neighbor_poly.entry = new_entry;
 
 						// Update the priority of the polygon in the heap.
@@ -428,8 +427,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 					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) *
-							neighbor_poly.poly->owner->get_travel_cost();
+							new_entry.distance_to(end_position) *
+							neighbor_poly.poly->owner->travel_cost;
 					neighbor_poly.entry = new_entry;
 
 					// Add the polygon to the heap of polygons to traverse next.
@@ -450,33 +449,33 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			}
 
 			// Set as end point the furthest reachable point.
-			end_poly = reachable_end;
+			end_polygon = reachable_end;
 			real_t end_d = FLT_MAX;
-			for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
-				Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+			for (size_t point_id = 2; point_id < end_polygon->points.size(); point_id++) {
+				Face3 f(end_polygon->points[0].pos, end_polygon->points[point_id - 1].pos, end_polygon->points[point_id].pos);
 				Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
 				real_t dpoint = spoint.distance_to(p_query_task.target_position);
 				if (dpoint < end_d) {
-					end_point = spoint;
+					end_position = spoint;
 					end_d = dpoint;
 				}
 			}
 
 			// Search all faces of start polygon as well.
 			bool closest_point_on_start_poly = false;
-			for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
-				Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+			for (size_t point_id = 2; point_id < begin_polygon->points.size(); point_id++) {
+				Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos);
 				Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
 				real_t dpoint = spoint.distance_to(p_query_task.target_position);
 				if (dpoint < end_d) {
-					end_point = spoint;
+					end_position = spoint;
 					end_d = dpoint;
 					closest_point_on_start_poly = true;
 				}
 			}
 
 			if (closest_point_on_start_poly) {
-				_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
+				_query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, end_polygon);
 				p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 				return;
 			}
@@ -484,9 +483,9 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			for (gd::NavigationPoly &nav_poly : navigation_polys) {
 				nav_poly.poly = nullptr;
 			}
-			navigation_polys[begin_poly->id].poly = begin_poly;
+			navigation_polys[begin_polygon->id].poly = begin_polygon;
 
-			p_query_task.least_cost_id = begin_poly->id;
+			p_query_task.least_cost_id = begin_polygon->id;
 			prev_least_cost_id = -1;
 
 			reachable_end = nullptr;
@@ -507,7 +506,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 		}
 
 		// Check if we reached the end
-		if (navigation_polys[p_query_task.least_cost_id].poly == end_poly) {
+		if (navigation_polys[p_query_task.least_cost_id].poly == end_polygon) {
 			found_route = true;
 			break;
 		}
@@ -518,16 +517,16 @@ 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 (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
-			Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+		for (size_t point_id = 2; point_id < begin_polygon->points.size(); point_id++) {
+			Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos);
 			Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
 			real_t dpoint = spoint.distance_to(p_query_task.target_position);
 			if (dpoint < end_d) {
-				end_point = spoint;
+				end_position = spoint;
 				end_d = dpoint;
 			}
 		}
-		_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point);
+		_query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, begin_polygon);
 		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 		return;
 	}
@@ -575,15 +574,18 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D
 	}
 }
 
-void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) {
+void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task) {
+	const Vector3 &begin_position = p_query_task.begin_position;
+	const Vector3 &end_position = p_query_task.end_position;
+	const Vector3 &map_up = p_query_task.map_up;
 	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
 
 	// Set the apex poly/point to the end point
-	gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id];
+	gd::NavigationPoly *apex_poly = &p_path_corridor[p_query_task.least_cost_id];
 
 	Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
-	const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway);
-	if (p_end_point.is_equal_approx(back_edge_closest_point)) {
+	const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_position, back_pathway);
+	if (end_position.is_equal_approx(back_edge_closest_point)) {
 		// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
 		// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
 		if (apex_poly->back_navigation_poly_id != -1) {
@@ -591,7 +593,7 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
 		}
 	}
 
-	Vector3 apex_point = p_end_point;
+	Vector3 apex_point = end_position;
 
 	gd::NavigationPoly *left_poly = apex_poly;
 	Vector3 left_portal = apex_point;
@@ -600,24 +602,24 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
 
 	gd::NavigationPoly *p = apex_poly;
 
-	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
 
 	while (p) {
 		// Set left and right points of the pathway between polygons.
 		Vector3 left = p->back_navigation_edge_pathway_start;
 		Vector3 right = p->back_navigation_edge_pathway_end;
-		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(map_up) < 0) {
 			SWAP(left, right);
 		}
 
 		bool skip = false;
-		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(map_up) >= 0) {
 			//process
-			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
+			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(map_up) > 0) {
 				left_poly = p;
 				left_portal = left;
 			} else {
-				clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up);
+				_query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly);
 
 				apex_point = right_portal;
 				p = right_poly;
@@ -632,13 +634,13 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
 			}
 		}
 
-		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
+		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(map_up) <= 0) {
 			//process
-			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
+			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(map_up) < 0) {
 				right_poly = p;
 				right_portal = right;
 			} else {
-				clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up);
+				_query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly);
 
 				apex_point = left_portal;
 				p = left_poly;
@@ -661,18 +663,20 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
 	}
 
 	// If the last point is not the begin point, add it to the list.
-	if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) {
-		_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+	if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_position) {
+		_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
 	}
 }
 
-void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
+void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task) {
+	const Vector3 &begin_position = p_query_task.begin_position;
+	const Vector3 &end_position = p_query_task.end_position;
 	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
 
-	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
 
 	// Add mid points.
-	int np_id = p_least_cost_id;
+	int np_id = p_query_task.least_cost_id;
 	while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
 		if (p_path_corridor[np_id].back_navigation_edge != -1) {
 			int prev = p_path_corridor[np_id].back_navigation_edge;
@@ -687,57 +691,67 @@ void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQuery
 		np_id = p_path_corridor[np_id].back_navigation_poly_id;
 	}
 
-	_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+	_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
 }
 
-void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
+void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task) {
+	const Vector3 &begin_position = p_query_task.begin_position;
+	const Vector3 &end_position = p_query_task.end_position;
 	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
 
-	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
 
 	// Add mid points.
-	int np_id = p_least_cost_id;
+	int np_id = p_query_task.least_cost_id;
 	while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
 		_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
 
 		np_id = p_path_corridor[np_id].back_navigation_poly_id;
 	}
 
-	_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+	_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
 }
 
-void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point) {
+void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
+	// Find begin polyon and begin position closest to start position and
+	// end polyon and end position closest to target position on the map.
 	real_t begin_d = FLT_MAX;
 	real_t end_d = FLT_MAX;
 
+	Vector3 begin_position;
+	Vector3 end_position;
+
 	// Find the initial poly and the end poly on this map.
-	for (const gd::Polygon &p : p_polygons) {
+	for (const gd::Polygon &polygon : p_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) {
+		if ((p_query_task.navigation_layers & polygon.owner->navigation_layers) == 0) {
 			continue;
 		}
 
 		// For each face check the distance between the origin/destination.
-		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
-			const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+		for (size_t point_id = 2; point_id < polygon.points.size(); point_id++) {
+			const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
 
 			Vector3 point = face.get_closest_point_to(p_query_task.start_position);
 			real_t distance_to_point = point.distance_to(p_query_task.start_position);
 			if (distance_to_point < begin_d) {
 				begin_d = distance_to_point;
-				*r_begin_poly = &p;
-				r_begin_point = point;
+				p_query_task.begin_polygon = &polygon;
+				begin_position = point;
 			}
 
 			point = face.get_closest_point_to(p_query_task.target_position);
 			distance_to_point = point.distance_to(p_query_task.target_position);
 			if (distance_to_point < end_d) {
 				end_d = distance_to_point;
-				*r_end_poly = &p;
-				r_end_point = point;
+				p_query_task.end_polygon = &polygon;
+				end_position = point;
 			}
 		}
 	}
+
+	p_query_task.begin_position = begin_position;
+	p_query_task.end_position = end_position;
 }
 
 Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
@@ -864,7 +878,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co
 				closest_point_distance_squared = distance_squared;
 				result.point = p_point - plane_normalized * distance;
 				result.normal = plane_normal;
-				result.owner = polygon.owner->get_self();
+				result.owner = polygon.owner->owner_rid;
 
 				if (Math::is_zero_approx(distance)) {
 					break;
@@ -876,7 +890,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co
 				closest_point_distance_squared = distance;
 				result.point = closest_on_polygon;
 				result.normal = plane_normal;
-				result.owner = polygon.owner->get_self();
+				result.owner = polygon.owner->owner_rid;
 			}
 		}
 	}
@@ -889,7 +903,9 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Pol
 	return cp.owner;
 }
 
-void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up) {
+void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) {
+	const Vector3 &map_up = p_query_task.map_up;
+	LocalVector<gd::NavigationPoly> &path_corridor = p_query_task.path_query_slot->path_corridor;
 	Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
 
 	if (from.is_equal_approx(p_to_point)) {
@@ -897,7 +913,7 @@ void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const Loc
 	}
 
 	Plane cut_plane;
-	cut_plane.normal = (from - p_to_point).cross(p_map_up);
+	cut_plane.normal = (from - p_to_point).cross(map_up);
 	if (cut_plane.normal == Vector3()) {
 		return;
 	}
@@ -909,7 +925,7 @@ void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const Loc
 		Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
 
 		ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
-		from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
+		from_poly = &path_corridor[from_poly->back_navigation_poly_id];
 
 		if (!pathway_start.is_equal_approx(pathway_end)) {
 			Vector3 inters;

modules/navigation/3d/nav_mesh_queries_3d.h

@@ -74,10 +74,15 @@ public:
 		// Path building.
 		Vector3 begin_position;
 		Vector3 end_position;
+		const gd::Polygon *begin_polygon = nullptr;
+		const gd::Polygon *end_polygon = nullptr;
 		uint32_t least_cost_id = 0;
+
+		// Map.
 		Vector3 map_up;
 		NavMap *map = nullptr;
 		PathQuerySlot *path_query_slot = nullptr;
+		uint32_t link_polygons_size = 0;
 
 		// Path points.
 		LocalVector<Vector3> path_points;
@@ -103,17 +108,15 @@ public:
 
 	static void map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
 
-	static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size);
-
-	static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point);
-	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon);
-	static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point);
-	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_polygon, Vector3 end_point);
-	static void _path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up);
-	static void _path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point);
-	static void _path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point);
-
-	static void clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up);
+	static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
+	static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *p_begin_polygon, const gd::Polygon *p_end_polygon);
+	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon);
+	static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
+	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);
+	static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly);
 	static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
 
 	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);

modules/navigation/3d/nav_region_iteration_3d.h

@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  nav_region_iteration_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.                 */
+/**************************************************************************/
+
+#ifndef NAV_REGION_ITERATION_3D_H
+#define NAV_REGION_ITERATION_3D_H
+
+#include "../nav_utils.h"
+#include "nav_base_iteration_3d.h"
+
+#include "core/math/aabb.h"
+
+struct NavRegionIteration : NavBaseIteration {
+	Transform3D transform;
+	LocalVector<gd::Polygon> navmesh_polygons;
+	real_t surface_area = 0.0;
+	AABB bounds;
+};
+
+#endif // NAV_REGION_ITERATION_3D_H

modules/navigation/nav_link.cpp

@@ -122,3 +122,15 @@ NavLink::NavLink() :
 NavLink::~NavLink() {
 	cancel_sync_request();
 }
+
+void NavLink::get_iteration_update(NavLinkIteration &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.enabled = enabled;
+	r_iteration.start_position = start_position;
+	r_iteration.end_position = end_position;
+}

modules/navigation/nav_link.h

@@ -31,9 +31,17 @@
 #ifndef NAV_LINK_H
 #define NAV_LINK_H
 
+#include "3d/nav_base_iteration_3d.h"
 #include "nav_base.h"
 #include "nav_utils.h"
 
+struct NavLinkIteration : NavBaseIteration {
+	bool bidirectional = true;
+	Vector3 start_position;
+	Vector3 end_position;
+	LocalVector<gd::Polygon> navmesh_polygons;
+};
+
 #include "core/templates/self_list.h"
 
 class NavLink : public NavBase {
@@ -78,6 +86,8 @@ public:
 	void sync();
 	void request_sync();
 	void cancel_sync_request();
+
+	void get_iteration_update(NavLinkIteration &r_iteration);
 };
 
 #endif // NAV_LINK_H

modules/navigation/nav_map.cpp

@@ -30,6 +30,9 @@
 
 #include "nav_map.h"
 
+#include "3d/nav_map_builder_3d.h"
+#include "3d/nav_mesh_queries_3d.h"
+#include "3d/nav_region_iteration_3d.h"
 #include "nav_agent.h"
 #include "nav_link.h"
 #include "nav_obstacle.h"
@@ -49,6 +52,18 @@
 #define NAVMAP_ITERATION_ZERO_ERROR_MSG()
 #endif // DEBUG_ENABLED
 
+#define GET_MAP_ITERATION()                                                 \
+	iteration_slot_rwlock.read_lock();                                      \
+	NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead iteration_read_lock(map_iteration);                 \
+	iteration_slot_rwlock.read_unlock();
+
+#define GET_MAP_ITERATION_CONST()                                                 \
+	iteration_slot_rwlock.read_lock();                                            \
+	const NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead iteration_read_lock(map_iteration);                       \
+	iteration_slot_rwlock.read_unlock();
+
 void NavMap::set_up(Vector3 p_up) {
 	if (up == p_up) {
 		return;
@@ -61,7 +76,7 @@ void NavMap::set_cell_size(real_t p_cell_size) {
 	if (cell_size == p_cell_size) {
 		return;
 	}
-	cell_size = p_cell_size;
+	cell_size = MAX(p_cell_size, NavigationDefaults3D::navmesh_cell_size_min);
 	_update_merge_rasterizer_cell_dimensions();
 	map_settings_dirty = true;
 }
@@ -70,7 +85,7 @@ void NavMap::set_cell_height(real_t p_cell_height) {
 	if (cell_height == p_cell_height) {
 		return;
 	}
-	cell_height = p_cell_height;
+	cell_height = MAX(p_cell_height, NavigationDefaults3D::navmesh_cell_size_min);
 	_update_merge_rasterizer_cell_dimensions();
 	map_settings_dirty = true;
 }
@@ -79,7 +94,7 @@ void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
 	if (merge_rasterizer_cell_scale == p_value) {
 		return;
 	}
-	merge_rasterizer_cell_scale = p_value;
+	merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults3D::navmesh_cell_size_min);
 	_update_merge_rasterizer_cell_dimensions();
 	map_settings_dirty = true;
 }
@@ -108,10 +123,14 @@ void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
 	iteration_dirty = true;
 }
 
+const Vector3 &NavMap::get_merge_rasterizer_cell_size() const {
+	return merge_rasterizer_cell_size;
+}
+
 gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
-	const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size));
-	const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_height));
-	const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size));
+	const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size.x));
+	const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_size.y));
+	const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size.z));
 
 	gd::PointKey p;
 	p.key = 0;
@@ -122,85 +141,91 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 }
 
 void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) {
-	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
 		return;
 	}
 
-	path_query_slots_semaphore.wait();
+	GET_MAP_ITERATION();
 
-	path_query_slots_mutex.lock();
-	for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) {
+	map_iteration.path_query_slots_semaphore.wait();
+
+	map_iteration.path_query_slots_mutex.lock();
+	for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) {
 		if (!p_path_query_slot.in_use) {
 			p_path_query_slot.in_use = true;
 			p_query_task.path_query_slot = &p_path_query_slot;
 			break;
 		}
 	}
-	path_query_slots_mutex.unlock();
+	map_iteration.path_query_slots_mutex.unlock();
 
 	if (p_query_task.path_query_slot == nullptr) {
-		path_query_slots_semaphore.post();
+		map_iteration.path_query_slots_semaphore.post();
 		ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(.");
 	}
 
-	p_query_task.map_up = get_up();
+	p_query_task.map_up = map_iteration.map_up;
+	p_query_task.link_polygons_size = map_iteration.link_polygon_count;
 
-	NavMeshQueries3D::query_task_polygons_get_path(p_query_task, polygons, up, link_polygons.size());
+	NavMeshQueries3D::query_task_polygons_get_path(p_query_task, map_iteration.navmesh_polygons);
 
-	path_query_slots_mutex.lock();
+	map_iteration.path_query_slots_mutex.lock();
 	uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
-	path_query_slots[used_slot_index].in_use = false;
+	map_iteration.path_query_slots[used_slot_index].in_use = false;
 	p_query_task.path_query_slot = nullptr;
-	path_query_slots_mutex.unlock();
+	map_iteration.path_query_slots_mutex.unlock();
 
-	path_query_slots_semaphore.post();
+	map_iteration.path_query_slots_semaphore.post();
 }
 
 Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
-	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
 		NAVMAP_ITERATION_ZERO_ERROR_MSG();
 		return Vector3();
 	}
 
-	return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision);
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::polygons_get_closest_point_to_segment(map_iteration.navmesh_polygons, p_from, p_to, p_use_collision);
 }
 
 Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
-	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
 		NAVMAP_ITERATION_ZERO_ERROR_MSG();
 		return Vector3();
 	}
 
-	return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point);
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::polygons_get_closest_point(map_iteration.navmesh_polygons, p_point);
 }
 
 Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
-	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
 		NAVMAP_ITERATION_ZERO_ERROR_MSG();
 		return Vector3();
 	}
 
-	return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point);
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::polygons_get_closest_point_normal(map_iteration.navmesh_polygons, p_point);
 }
 
 RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
-	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
 		NAVMAP_ITERATION_ZERO_ERROR_MSG();
 		return RID();
 	}
 
-	return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point);
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::polygons_get_closest_point_owner(map_iteration.navmesh_polygons, p_point);
 }
 
 gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
-	RWLockRead read_lock(map_rwlock);
+	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point);
+	return NavMeshQueries3D::polygons_get_closest_point_info(map_iteration.navmesh_polygons, p_point);
 }
 
 void NavMap::add_region(NavRegion *p_region) {
@@ -310,21 +335,21 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) {
 }
 
 Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
-	RWLockRead read_lock(map_rwlock);
-
-	const LocalVector<NavRegion *> map_regions = get_regions();
+	GET_MAP_ITERATION_CONST();
 
-	if (map_regions.is_empty()) {
+	if (map_iteration.region_iterations.is_empty()) {
 		return Vector3();
 	}
 
-	LocalVector<const NavRegion *> accessible_regions;
+	LocalVector<uint32_t> accessible_regions;
+	accessible_regions.reserve(map_iteration.region_iterations.size());
 
-	for (const NavRegion *region : map_regions) {
-		if (!region->get_enabled() || (p_navigation_layers & region->get_navigation_layers()) == 0) {
+	for (uint32_t i = 0; i < map_iteration.region_iterations.size(); i++) {
+		const NavRegionIteration &region = map_iteration.region_iterations[i];
+		if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
 			continue;
 		}
-		accessible_regions.push_back(region);
+		accessible_regions.push_back(i);
 	}
 
 	if (accessible_regions.is_empty()) {
@@ -337,9 +362,9 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly)
 		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 NavRegion *region = accessible_regions[accessible_region_index];
+			const NavRegionIteration &region = map_iteration.region_iterations[accessible_regions[accessible_region_index]];
 
-			real_t region_surface_area = region->get_surface_area();
+			real_t region_surface_area = region.surface_area;
 
 			if (region_surface_area == 0.0f) {
 				continue;
@@ -360,320 +385,167 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly)
 		uint32_t random_region_index = E->value;
 		ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
 
-		const NavRegion *random_region = accessible_regions[random_region_index];
-		ERR_FAIL_NULL_V(random_region, Vector3());
+		const NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]];
 
-		return random_region->get_random_point(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 NavRegion *random_region = accessible_regions[random_region_index];
-		ERR_FAIL_NULL_V(random_region, Vector3());
+		const NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]];
 
-		return random_region->get_random_point(p_navigation_layers, p_uniformly);
+		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
 	}
 }
 
-void NavMap::sync() {
-	RWLockWrite write_lock(map_rwlock);
+void NavMap::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
+	}
 
-	performance_data.pm_region_count = regions.size();
-	performance_data.pm_agent_count = agents.size();
-	performance_data.pm_link_count = links.size();
-	performance_data.pm_obstacle_count = obstacles.size();
+	// Get the next free iteration slot that should be potentially unused.
+	iteration_slot_rwlock.read_lock();
+	NavMapIteration &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
+	// Check if the iteration slot is truly free or still used by an external thread.
+	bool iteration_is_free = next_map_iteration.users.get() == 0;
+	iteration_slot_rwlock.read_unlock();
 
-	_sync_dirty_map_update_requests();
+	if (!iteration_is_free) {
+		// A long running pathfinding thread or something is still reading
+		// from this older iteration and needs to finish first.
+		// Return and wait for the next sync cycle to check again.
+		return;
+	}
 
-	if (iteration_dirty) {
-		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;
+	// Iteration slot is free and no longer used by anything, let's build.
 
-		// Remove regions connections.
-		region_external_connections.clear();
-		for (NavRegion *region : regions) {
-			region_external_connections[region] = LocalVector<gd::Edge::Connection>();
-		}
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
 
-		// Resize the polygon count.
-		int polygon_count = 0;
-		for (const NavRegion *region : regions) {
-			if (!region->get_enabled()) {
-				continue;
-			}
-			polygon_count += region->get_polygons().size();
-		}
-		polygons.resize(polygon_count);
+	// We don't need to hold any lock because at this point nothing else can touch it.
+	// All new queries are already forwarded to the other iteration slot.
 
-		// Copy all region polygons in the map.
-		polygon_count = 0;
-		for (const NavRegion *region : regions) {
-			if (!region->get_enabled()) {
-				continue;
-			}
-			const LocalVector<gd::Polygon> &polygons_source = region->get_polygons();
-			for (uint32_t n = 0; n < polygons_source.size(); n++) {
-				polygons[polygon_count] = polygons_source[n];
-				polygons[polygon_count].id = polygon_count;
-				polygon_count++;
-			}
-		}
+	iteration_build.reset();
 
-		performance_data.pm_polygon_count = polygon_count;
-
-		// Group all edges per key.
-		connection_pairs_map.clear();
-		connection_pairs_map.reserve(polygons.size());
-		int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
-
-		for (gd::Polygon &poly : polygons) {
-			for (uint32_t p = 0; p < poly.points.size(); p++) {
-				const int next_point = (p + 1) % poly.points.size();
-				const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
-
-				HashMap<gd::EdgeKey, ConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
-				if (!pair_it) {
-					pair_it = connection_pairs_map.insert(ek, ConnectionPair());
-					performance_data.pm_edge_count += 1;
-					++free_edges_count;
-				}
-				ConnectionPair &pair = pair_it->value;
-				if (pair.size < 2) {
-					// Add the polygon/edge tuple to this key.
-					gd::Edge::Connection new_connection;
-					new_connection.polygon = &poly;
-					new_connection.edge = p;
-					new_connection.pathway_start = poly.points[p].pos;
-					new_connection.pathway_end = poly.points[next_point].pos;
-
-					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.");
-				}
-			}
-		}
+	iteration_build.merge_rasterizer_cell_size = get_merge_rasterizer_cell_size();
+	iteration_build.use_edge_connections = get_use_edge_connections();
+	iteration_build.edge_connection_margin = get_edge_connection_margin();
+	iteration_build.link_connection_radius = get_link_connection_radius();
 
-		free_edges.clear();
-		free_edges.reserve(free_edges_count);
-
-		for (const KeyValue<gd::EdgeKey, ConnectionPair> &pair_it : connection_pairs_map) {
-			const ConnectionPair &pair = pair_it.value;
-			if (pair.size == 2) {
-				// Connect edge that are shared in different polygons.
-				const gd::Edge::Connection &c1 = pair.connections[0];
-				const gd::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;
-			} 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()) {
-					free_edges.push_back(pair.connections[0]);
-				}
-			}
-		}
+	uint32_t enabled_region_count = 0;
+	uint32_t enabled_link_count = 0;
 
-		// Find the compatible near edges.
-		//
-		// Note:
-		// Considering that the edges must be compatible (for obvious reasons)
-		// to be connected, create new polygons to remove that small gap is
-		// not really useful and would result in wasteful computation during
-		// connection, integration and path finding.
-		performance_data.pm_edge_free_count = free_edges.size();
-
-		const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
-
-		for (uint32_t i = 0; i < free_edges.size(); i++) {
-			const gd::Edge::Connection &free_edge = free_edges[i];
-			Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
-			Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
-
-			for (uint32_t j = 0; j < free_edges.size(); j++) {
-				const gd::Edge::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->points[other_edge.edge].pos;
-				Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
-
-				// Compute the projection of the opposite edge on the current one
-				Vector3 edge_vector = edge_p2 - edge_p1;
-				real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
-				real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
-				if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
-					continue;
-				}
-
-				// Check if the two edges are close to each other enough and compute a pathway between the two regions.
-				Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
-				Vector3 other1;
-				if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
-					other1 = other_edge_p1;
-				} else {
-					other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
-				}
-				if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
-					continue;
-				}
-
-				Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
-				Vector3 other2;
-				if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
-					other2 = other_edge_p2;
-				} else {
-					other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
-				}
-				if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
-					continue;
-				}
-
-				// The edges can now be connected.
-				gd::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);
-
-				// Add the connection to the region_connection map.
-				region_external_connections[(NavRegion *)free_edge.polygon->owner].push_back(new_connection);
-				performance_data.pm_edge_connection_count += 1;
-			}
+	for (NavRegion *region : regions) {
+		if (!region->get_enabled()) {
+			continue;
 		}
+		enabled_region_count++;
+	}
+	for (NavLink *link : links) {
+		if (!link->get_enabled()) {
+			continue;
+		}
+		enabled_link_count++;
+	}
 
-		uint32_t link_poly_idx = 0;
-		link_polygons.resize(links.size());
+	next_map_iteration.region_ptr_to_region_id.clear();
 
-		// Search for polygons within range of a nav link.
-		for (const NavLink *link : links) {
-			if (!link->get_enabled()) {
-				continue;
-			}
-			const Vector3 start = link->get_start_position();
-			const Vector3 end = link->get_end_position();
-
-			gd::Polygon *closest_start_polygon = nullptr;
-			real_t closest_start_sqr_dist = link_connection_radius * link_connection_radius;
-			Vector3 closest_start_point;
-
-			gd::Polygon *closest_end_polygon = nullptr;
-			real_t closest_end_sqr_dist = link_connection_radius * link_connection_radius;
-			Vector3 closest_end_point;
-
-			// Create link to any polygons within the search radius of the start point.
-			for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) {
-				gd::Polygon &start_poly = polygons[start_index];
-
-				// For each face check the distance to the start
-				for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) {
-					const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos);
-					const Vector3 start_point = start_face.get_closest_point_to(start);
-					const real_t sqr_dist = start_point.distance_squared_to(start);
-
-					// Pick the polygon that is within our radius and is closer than anything we've seen yet.
-					if (sqr_dist < closest_start_sqr_dist) {
-						closest_start_sqr_dist = sqr_dist;
-						closest_start_point = start_point;
-						closest_start_polygon = &start_poly;
-					}
-				}
-			}
+	next_map_iteration.region_iterations.clear();
+	next_map_iteration.link_iterations.clear();
 
-			// Find any polygons within the search radius of the end point.
-			for (gd::Polygon &end_poly : polygons) {
-				// For each face check the distance to the end
-				for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) {
-					const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos);
-					const Vector3 end_point = end_face.get_closest_point_to(end);
-					const real_t sqr_dist = end_point.distance_squared_to(end);
-
-					// Pick the polygon that is within our radius and is closer than anything we've seen yet.
-					if (sqr_dist < closest_end_sqr_dist) {
-						closest_end_sqr_dist = sqr_dist;
-						closest_end_point = end_point;
-						closest_end_polygon = &end_poly;
-					}
-				}
-			}
+	next_map_iteration.region_iterations.resize(enabled_region_count);
+	next_map_iteration.link_iterations.resize(enabled_link_count);
 
-			// If we have both a start and end point, then create a synthetic polygon to route through.
-			if (closest_start_polygon && closest_end_polygon) {
-				gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
-				new_polygon.id = polygon_count++;
-				new_polygon.owner = link;
-
-				new_polygon.edges.clear();
-				new_polygon.edges.resize(4);
-				new_polygon.points.clear();
-				new_polygon.points.reserve(4);
-
-				// Build a set of vertices that create a thin polygon going from the start to the end point.
-				new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
-				new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
-				new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
-				new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
-
-				// Setup connections to go forward in the link.
-				{
-					gd::Edge::Connection entry_connection;
-					entry_connection.polygon = &new_polygon;
-					entry_connection.edge = -1;
-					entry_connection.pathway_start = new_polygon.points[0].pos;
-					entry_connection.pathway_end = new_polygon.points[1].pos;
-					closest_start_polygon->edges[0].connections.push_back(entry_connection);
-
-					gd::Edge::Connection exit_connection;
-					exit_connection.polygon = closest_end_polygon;
-					exit_connection.edge = -1;
-					exit_connection.pathway_start = new_polygon.points[2].pos;
-					exit_connection.pathway_end = new_polygon.points[3].pos;
-					new_polygon.edges[2].connections.push_back(exit_connection);
-				}
-
-				// If the link is bi-directional, create connections from the end to the start.
-				if (link->is_bidirectional()) {
-					gd::Edge::Connection entry_connection;
-					entry_connection.polygon = &new_polygon;
-					entry_connection.edge = -1;
-					entry_connection.pathway_start = new_polygon.points[2].pos;
-					entry_connection.pathway_end = new_polygon.points[3].pos;
-					closest_end_polygon->edges[0].connections.push_back(entry_connection);
-
-					gd::Edge::Connection exit_connection;
-					exit_connection.polygon = closest_start_polygon;
-					exit_connection.edge = -1;
-					exit_connection.pathway_start = new_polygon.points[0].pos;
-					exit_connection.pathway_end = new_polygon.points[1].pos;
-					new_polygon.edges[0].connections.push_back(exit_connection);
-				}
-			}
+	uint32_t region_id_count = 0;
+	uint32_t link_id_count = 0;
+
+	for (NavRegion *region : regions) {
+		if (!region->get_enabled()) {
+			continue;
+		}
+		NavRegionIteration &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;
+	}
+	for (NavLink *link : links) {
+		if (!link->get_enabled()) {
+			continue;
 		}
+		NavLinkIteration &link_iteration = next_map_iteration.link_iterations[link_id_count];
+		link_iteration.id = link_id_count++;
+		link->get_iteration_update(link_iteration);
+	}
+
+	next_map_iteration.map_up = get_up();
+
+	iteration_build.map_iteration = &next_map_iteration;
+
+	if (use_async_iterations) {
+		iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D"));
+	} else {
+		NavMapBuilder3D::build_navmap_iteration(iteration_build);
+
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
+
+void NavMap::_build_iteration_threaded(void *p_arg) {
+	NavMapIterationBuild *_iteration_build = static_cast<NavMapIterationBuild *>(p_arg);
+
+	NavMapBuilder3D::build_navmap_iteration(*_iteration_build);
+}
+
+void NavMap::_sync_iteration() {
+	if (iteration_building || !iteration_ready) {
+		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;
+
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	// Finally ping-pong switch the iteration slot.
+	iteration_slot_rwlock.write_lock();
+	uint32_t next_iteration_slot_index = (iteration_slot_index + 1) % 2;
+	iteration_slot_index = next_iteration_slot_index;
+	iteration_slot_rwlock.write_unlock();
+
+	iteration_ready = false;
+}
+
+void NavMap::sync() {
+	// Performance Monitor.
+	performance_data.pm_region_count = regions.size();
+	performance_data.pm_agent_count = agents.size();
+	performance_data.pm_link_count = links.size();
+	performance_data.pm_obstacle_count = obstacles.size();
+
+	_sync_dirty_map_update_requests();
 
-		// Some code treats 0 as a failure case, so we avoid returning 0 and modulo wrap UINT32_MAX manually.
-		iteration_id = iteration_id % UINT32_MAX + 1;
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
+	}
+	if (use_async_iterations && 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);
 
-		path_query_slots_mutex.lock();
-		for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) {
-			p_path_query_slot.path_corridor.clear();
-			p_path_query_slot.path_corridor.resize(polygons.size() + link_polygons.size());
-			p_path_query_slot.traversable_polys.clear();
-			p_path_query_slot.traversable_polys.reserve(polygons.size() * 0.25);
+			iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+			iteration_building = false;
+			iteration_ready = true;
 		}
-		path_query_slots_mutex.unlock();
+	}
+	if (iteration_ready) {
+		_sync_iteration();
 	}
 
 	map_settings_dirty = false;
-	iteration_dirty = false;
 
 	_sync_avoidance();
 }
@@ -857,27 +729,39 @@ void NavMap::dispatch_callbacks() {
 }
 
 void NavMap::_update_merge_rasterizer_cell_dimensions() {
-	merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale;
-	merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.y = cell_height * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.z = cell_size * merge_rasterizer_cell_scale;
 }
 
 int NavMap::get_region_connections_count(NavRegion *p_region) const {
 	ERR_FAIL_NULL_V(p_region, 0);
 
-	HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
-	if (found_connections) {
-		return found_connections->value.size();
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		if (found_connections) {
+			return found_connections->value.size();
+		}
 	}
+
 	return 0;
 }
 
 Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const {
 	ERR_FAIL_NULL_V(p_region, Vector3());
 
-	HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
-	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;
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		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;
+		}
 	}
 
 	return Vector3();
@@ -886,10 +770,15 @@ Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_c
 Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const {
 	ERR_FAIL_NULL_V(p_region, Vector3());
 
-	HashMap<NavRegion *, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = region_external_connections.find(p_region);
-	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;
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		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;
+		}
 	}
 
 	return Vector3();
@@ -997,6 +886,19 @@ void NavMap::_sync_dirty_avoidance_update_requests() {
 	sync_dirty_requests.obstacles.clear();
 }
 
+void NavMap::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavMap::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
 NavMap::NavMap() {
 	avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
 	avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
@@ -1014,13 +916,26 @@ NavMap::NavMap() {
 		path_query_slots_max = 1;
 	}
 
-	path_query_slots.resize(path_query_slots_max);
-	for (uint32_t i = 0; i < path_query_slots.size(); i++) {
-		path_query_slots[i].slot_index = i;
+	iteration_slots.resize(2);
+
+	for (NavMapIteration &iteration_slot : iteration_slots) {
+		iteration_slot.path_query_slots.resize(path_query_slots_max);
+		for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) {
+			iteration_slot.path_query_slots[i].slot_index = i;
+		}
+		iteration_slot.path_query_slots_semaphore.post(path_query_slots_max);
 	}
 
-	path_query_slots_semaphore.post(path_query_slots_max);
+#ifdef THREADS_ENABLED
+	use_async_iterations = GLOBAL_GET("navigation/world/map_use_async_iterations");
+#else
+	use_async_iterations = false;
+#endif
 }
 
 NavMap::~NavMap() {
+	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;
+	}
 }

modules/navigation/nav_map.h

@@ -31,6 +31,7 @@
 #ifndef NAV_MAP_H
 #define NAV_MAP_H
 
+#include "3d/nav_map_iteration_3d.h"
 #include "3d/nav_mesh_queries_3d.h"
 #include "nav_rid.h"
 #include "nav_utils.h"
@@ -50,8 +51,6 @@ class NavAgent;
 class NavObstacle;
 
 class NavMap : public NavRid {
-	RWLock map_rwlock;
-
 	/// Map Up
 	Vector3 up = Vector3(0, 1, 0);
 
@@ -61,8 +60,8 @@ class NavMap : public NavRid {
 	real_t cell_height = NavigationDefaults3D::navmesh_cell_height;
 
 	// For the inter-region merging to work, internal rasterization is performed.
-	float merge_rasterizer_cell_size = NavigationDefaults3D::navmesh_cell_size;
-	float merge_rasterizer_cell_height = NavigationDefaults3D::navmesh_cell_height;
+	Vector3 merge_rasterizer_cell_size = Vector3(cell_size, cell_height, cell_size);
+
 	// This value is used to control sensitivity of internal rasterizer.
 	float merge_rasterizer_cell_scale = 1.0;
 
@@ -74,17 +73,12 @@ class NavMap : public NavRid {
 	real_t link_connection_radius = NavigationDefaults3D::link_connection_radius;
 
 	bool map_settings_dirty = true;
-	bool iteration_dirty = true;
 
 	/// Map regions
 	LocalVector<NavRegion *> regions;
 
 	/// Map links
 	LocalVector<NavLink *> links;
-	LocalVector<gd::Polygon> link_polygons;
-
-	/// Map polygons
-	LocalVector<gd::Polygon> polygons;
 
 	/// RVO avoidance worlds
 	RVO2D::RVOSimulator2D rvo_simulation_2d;
@@ -119,16 +113,6 @@ class NavMap : public NavRid {
 	// Performance Monitor
 	gd::PerformanceData performance_data;
 
-	HashMap<NavRegion *, LocalVector<gd::Edge::Connection>> region_external_connections;
-
-	struct ConnectionPair {
-		gd::Edge::Connection connections[2];
-		int size = 0;
-	};
-
-	HashMap<gd::EdgeKey, ConnectionPair, gd::EdgeKey> connection_pairs_map;
-	LocalVector<gd::Edge::Connection> free_edges;
-
 	struct {
 		SelfList<NavRegion>::List regions;
 		SelfList<NavLink>::List links;
@@ -136,10 +120,25 @@ class NavMap : public NavRid {
 		SelfList<NavObstacle>::List obstacles;
 	} sync_dirty_requests;
 
-	LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
 	int path_query_slots_max = 4;
-	Mutex path_query_slots_mutex;
-	Semaphore path_query_slots_semaphore;
+
+	bool use_async_iterations = true;
+
+	uint32_t iteration_slot_index = 0;
+	LocalVector<NavMapIteration> iteration_slots;
+	mutable RWLock iteration_slot_rwlock;
+
+	NavMapIterationBuild 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);
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
 
 public:
 	NavMap();
@@ -181,6 +180,7 @@ public:
 	}
 
 	gd::PointKey get_point_key(const Vector3 &p_pos) const;
+	const Vector3 &get_merge_rasterizer_cell_size() const;
 
 	void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task);
 
@@ -250,6 +250,9 @@ public:
 	void remove_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request);
 	void remove_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request);
 
+	void set_use_async_iterations(bool p_enabled);
+	bool get_use_async_iterations() const;
+
 private:
 	void _sync_dirty_map_update_requests();
 	void _sync_dirty_avoidance_update_requests();

modules/navigation/nav_region.cpp

@@ -32,7 +32,9 @@
 
 #include "nav_map.h"
 
+#include "3d/nav_map_builder_3d.h"
 #include "3d/nav_mesh_queries_3d.h"
+#include "3d/nav_region_iteration_3d.h"
 
 void NavRegion::set_map(NavMap *p_map) {
 	if (map == p_map) {
@@ -153,8 +155,9 @@ void NavRegion::update_polygons() {
 	if (!polygons_dirty) {
 		return;
 	}
-	polygons.clear();
+	navmesh_polygons.clear();
 	surface_area = 0.0;
+	bounds = AABB();
 	polygons_dirty = false;
 
 	if (map == nullptr) {
@@ -174,14 +177,15 @@ void NavRegion::update_polygons() {
 
 	const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
 
-	polygons.resize(pending_navmesh_polygons.size());
+	navmesh_polygons.resize(pending_navmesh_polygons.size());
 
 	real_t _new_region_surface_area = 0.0;
+	AABB _new_bounds;
 
-	// Build
+	bool first_vertex = true;
 	int navigation_mesh_polygon_index = 0;
-	for (gd::Polygon &polygon : polygons) {
-		polygon.owner = this;
+
+	for (gd::Polygon &polygon : navmesh_polygons) {
 		polygon.surface_area = 0.0;
 
 		Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
@@ -221,7 +225,14 @@ void NavRegion::update_polygons() {
 
 			Vector3 point_position = transform.xform(vertices_r[idx]);
 			polygon.points[j].pos = point_position;
-			polygon.points[j].key = map->get_point_key(point_position);
+			polygon.points[j].key = NavMapBuilder3D::get_point_key(point_position, map->get_merge_rasterizer_cell_size());
+
+			if (first_vertex) {
+				first_vertex = false;
+				_new_bounds.position = point_position;
+			} else {
+				_new_bounds.expand_to(point_position);
+			}
 		}
 
 		if (!valid) {
@@ -230,6 +241,60 @@ void NavRegion::update_polygons() {
 	}
 
 	surface_area = _new_region_surface_area;
+	bounds = _new_bounds;
+}
+
+void NavRegion::get_iteration_update(NavRegionIteration &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.enabled = enabled;
+	r_iteration.transform = transform;
+	r_iteration.owner_use_edge_connections = use_edge_connections;
+	r_iteration.bounds = get_bounds();
+
+	r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
+
+	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
+		const gd::Polygon &from_polygon = navmesh_polygons[i];
+		gd::Polygon &to_polygon = r_iteration.navmesh_polygons[i];
+
+		to_polygon.surface_area = from_polygon.surface_area;
+		to_polygon.owner = &r_iteration;
+		to_polygon.points.resize(from_polygon.points.size());
+
+		const LocalVector<gd::Point> &from_points = from_polygon.points;
+		LocalVector<gd::Point> &to_points = to_polygon.points;
+
+		to_points.resize(from_points.size());
+
+		for (uint32_t j = 0; j < from_points.size(); j++) {
+			to_points[j].pos = from_points[j].pos;
+			to_points[j].key = from_points[j].key;
+		}
+
+		const LocalVector<gd::Edge> &from_edges = from_polygon.edges;
+		LocalVector<gd::Edge> &to_edges = to_polygon.edges;
+
+		to_edges.resize(from_edges.size());
+
+		for (uint32_t j = 0; j < from_edges.size(); j++) {
+			const LocalVector<gd::Edge::Connection> &from_connections = from_edges[j].connections;
+			LocalVector<gd::Edge::Connection> &to_connections = to_edges[j].connections;
+
+			to_connections.resize(from_connections.size());
+
+			for (uint32_t k = 0; k < from_connections.size(); k++) {
+				to_connections[k] = from_connections[k];
+			}
+		}
+	}
+
+	r_iteration.surface_area = surface_area;
+	r_iteration.owner_rid = get_self();
 }
 
 void NavRegion::request_sync() {

modules/navigation/nav_region.h

@@ -37,6 +37,8 @@
 #include "core/os/rw_lock.h"
 #include "scene/resources/navigation_mesh.h"
 
+struct NavRegionIteration;
+
 class NavRegion : public NavBase {
 	RWLock region_rwlock;
 
@@ -48,10 +50,10 @@ class NavRegion : public NavBase {
 
 	bool polygons_dirty = true;
 
-	/// Cache
-	LocalVector<gd::Polygon> polygons;
+	LocalVector<gd::Polygon> navmesh_polygons;
 
 	real_t surface_area = 0.0;
+	AABB bounds;
 
 	RWLock navmesh_rwlock;
 	Vector<Vector3> pending_navmesh_vertices;
@@ -88,7 +90,7 @@ public:
 	void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
 
 	LocalVector<gd::Polygon> const &get_polygons() const {
-		return polygons;
+		return navmesh_polygons;
 	}
 
 	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const;
@@ -96,11 +98,14 @@ public:
 	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; }
 
 	bool sync();
 	void request_sync();
 	void cancel_sync_request();
 
+	void get_iteration_update(NavRegionIteration &r_iteration);
+
 private:
 	void update_polygons();
 };

modules/navigation/nav_utils.h

@@ -35,8 +35,9 @@
 #include "core/templates/hash_map.h"
 #include "core/templates/hashfuncs.h"
 #include "core/templates/local_vector.h"
+#include "servers/navigation/navigation_utilities.h"
 
-class NavBase;
+struct NavBaseIteration;
 
 namespace gd {
 struct Polygon;
@@ -102,7 +103,7 @@ struct Polygon {
 	uint32_t id = UINT32_MAX;
 
 	/// Navigation region or link that contains this polygon.
-	const NavBase *owner = nullptr;
+	const NavBaseIteration *owner = nullptr;
 
 	/// The points of this `Polygon`
 	LocalVector<Point> points;
@@ -308,6 +309,11 @@ private:
 	}
 };
 
+struct EdgeConnectionPair {
+	gd::Edge::Connection connections[2];
+	int size = 0;
+};
+
 struct PerformanceData {
 	int pm_region_count = 0;
 	int pm_agent_count = 0;
@@ -318,6 +324,18 @@ struct PerformanceData {
 	int pm_edge_connection_count = 0;
 	int pm_edge_free_count = 0;
 	int pm_obstacle_count = 0;
+
+	void reset() {
+		pm_region_count = 0;
+		pm_agent_count = 0;
+		pm_link_count = 0;
+		pm_polygon_count = 0;
+		pm_edge_count = 0;
+		pm_edge_merge_count = 0;
+		pm_edge_connection_count = 0;
+		pm_edge_free_count = 0;
+		pm_obstacle_count = 0;
+	}
 };
 
 } // namespace gd

servers/navigation/navigation_globals.h

@@ -39,6 +39,7 @@ namespace NavigationDefaults3D {
 // each cell has the following cell_size and cell_height.
 constexpr float navmesh_cell_size{ 0.25f }; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size.
 constexpr float navmesh_cell_height{ 0.25f }; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height.
+constexpr float navmesh_cell_size_min{ 0.01f };
 constexpr auto navmesh_cell_size_hint{ "0.001,100,0.001,or_greater" };
 
 // Map.

servers/navigation_server_2d.cpp

@@ -60,6 +60,8 @@ void NavigationServer2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer2D::map_force_update);
 	ClassDB::bind_method(D_METHOD("map_get_iteration_id", "map"), &NavigationServer2D::map_get_iteration_id);
+	ClassDB::bind_method(D_METHOD("map_set_use_async_iterations", "map", "enabled"), &NavigationServer2D::map_set_use_async_iterations);
+	ClassDB::bind_method(D_METHOD("map_get_use_async_iterations", "map"), &NavigationServer2D::map_get_use_async_iterations);
 
 	ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer2D::map_get_random_point);
 

servers/navigation_server_2d.h

@@ -104,6 +104,9 @@ public:
 	virtual void map_force_update(RID p_map) = 0;
 	virtual uint32_t map_get_iteration_id(RID p_map) const = 0;
 
+	virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) = 0;
+	virtual bool map_get_use_async_iterations(RID p_map) const = 0;
+
 	virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const = 0;
 
 	/// Creates a new region.

servers/navigation_server_2d_dummy.h

@@ -60,6 +60,8 @@ public:
 	void map_force_update(RID p_map) override {}
 	Vector2 map_get_random_point(RID p_map, uint32_t p_naviation_layers, bool p_uniformly) const override { return Vector2(); }
 	uint32_t map_get_iteration_id(RID p_map) const override { return 0; }
+	void map_set_use_async_iterations(RID p_map, bool p_enabled) override {}
+	bool map_get_use_async_iterations(RID p_map) const override { return false; }
 
 	RID region_create() override { return RID(); }
 	void region_set_enabled(RID p_region, bool p_enabled) override {}

servers/navigation_server_3d.cpp

@@ -70,6 +70,8 @@ void NavigationServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer3D::map_force_update);
 	ClassDB::bind_method(D_METHOD("map_get_iteration_id", "map"), &NavigationServer3D::map_get_iteration_id);
+	ClassDB::bind_method(D_METHOD("map_set_use_async_iterations", "map", "enabled"), &NavigationServer3D::map_set_use_async_iterations);
+	ClassDB::bind_method(D_METHOD("map_get_use_async_iterations", "map"), &NavigationServer3D::map_get_use_async_iterations);
 
 	ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer3D::map_get_random_point);
 
@@ -245,6 +247,8 @@ NavigationServer3D::NavigationServer3D() {
 	GLOBAL_DEF_BASIC("navigation/3d/default_edge_connection_margin", NavigationDefaults3D::edge_connection_margin);
 	GLOBAL_DEF_BASIC("navigation/3d/default_link_connection_radius", NavigationDefaults3D::link_connection_radius);
 
+	GLOBAL_DEF("navigation/world/map_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);
 

servers/navigation_server_3d.h

@@ -118,6 +118,9 @@ public:
 	virtual void map_force_update(RID p_map) = 0;
 	virtual uint32_t map_get_iteration_id(RID p_map) const = 0;
 
+	virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) = 0;
+	virtual bool map_get_use_async_iterations(RID p_map) const = 0;
+
 	virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const = 0;
 
 	/// Creates a new region.

servers/navigation_server_3d_dummy.h

@@ -67,6 +67,8 @@ public:
 	TypedArray<RID> map_get_obstacles(RID p_map) const override { return TypedArray<RID>(); }
 	void map_force_update(RID p_map) override {}
 	uint32_t map_get_iteration_id(RID p_map) const override { return 0; }
+	void map_set_use_async_iterations(RID p_map, bool p_enabled) override {}
+	bool map_get_use_async_iterations(RID p_map) const override { return false; }
 
 	RID region_create() override { return RID(); }
 	void region_set_enabled(RID p_region, bool p_enabled) override {}

tests/servers/test_navigation_server_3d.h

@@ -572,6 +572,7 @@ TEST_SUITE("[Navigation]") {
 		RID map = navigation_server->map_create();
 		RID region = navigation_server->region_create();
 		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_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
@@ -667,6 +668,7 @@ TEST_SUITE("[Navigation]") {
 		RID map = navigation_server->map_create();
 		RID region = navigation_server->region_create();
 		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_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
@@ -716,6 +718,7 @@ TEST_SUITE("[Navigation]") {
 		RID map = navigation_server->map_create();
 		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_map(region, map);
 		navigation_server->region_set_navigation_mesh(region, navigation_mesh);
 		navigation_server->process(0.0); // Give server some cycles to commit.