Merge pull request #100823 from smix8/patch_async_mapsync

Patch navigation map async synchronization

modules/navigation/3d/nav_base_iteration_3d.h

@@ -43,6 +43,15 @@ struct NavBaseIteration {
 	ObjectID owner_object_id;
 	RID owner_rid;
 	bool owner_use_edge_connections = false;
+
+	bool get_enabled() const { return enabled; }
+	NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
+	RID get_self() const { return owner_rid; }
+	ObjectID get_owner_id() const { return owner_object_id; }
+	uint32_t get_navigation_layers() const { return navigation_layers; }
+	real_t get_enter_cost() const { return enter_cost; }
+	real_t get_travel_cost() const { return travel_cost; }
+	bool get_use_edge_connections() const { return owner_use_edge_connections; }
 };
 
 #endif // NAV_BASE_ITERATION_3D_H

modules/navigation/3d/nav_map_builder_3d.cpp

@@ -52,6 +52,14 @@ gd::PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3
 }
 
 void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
+	gd::PerformanceData &performance_data = r_build.performance_data;
+
+	performance_data.pm_polygon_count = 0;
+	performance_data.pm_edge_count = 0;
+	performance_data.pm_edge_merge_count = 0;
+	performance_data.pm_edge_connection_count = 0;
+	performance_data.pm_edge_free_count = 0;
+
 	_build_step_gather_region_polygons(r_build);
 
 	_build_step_find_edge_connection_pairs(r_build);
@@ -66,89 +74,108 @@ void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &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;
+	NavMapIteration *map_iteration = r_build.map_iteration;
+
+	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
+	LocalVector<NavRegionIteration> &regions = map_iteration->region_iterations;
+	HashMap<uint32_t, LocalVector<gd::Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
 
 	// 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>();
+	region_external_connections.clear();
+	for (const NavRegionIteration &region : regions) {
+		region_external_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;
+	// Resize the polygon count.
+	int polygon_count = 0;
+	for (const NavRegionIteration &region : regions) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+		polygon_count += region.get_navmesh_polygons().size();
+	}
+	polygons.resize(polygon_count);
 
+	// Copy all region polygons in the map.
+	polygon_count = 0;
+	for (const NavRegionIteration &region : regions) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+		const LocalVector<gd::Polygon> &polygons_source = region.get_navmesh_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++;
-			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;
-					}
+	NavMapIteration *map_iteration = r_build.map_iteration;
+
+	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
+	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
 
-				} 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.");
+	// 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, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
+			if (!pair_it) {
+				pair_it = connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
+				performance_data.pm_edge_count += 1;
+				++free_edges_count;
+			}
+			gd::EdgeConnectionPair &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.");
 			}
 		}
 	}
-	r_build.free_edge_count = free_edge_count;
+
+	r_build.free_edge_count = free_edges_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;
+	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+	LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
+	int free_edges_count = r_build.free_edge_count;
+	bool use_edge_connections = r_build.use_edge_connections;
 
-	for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : iter_connection_pairs_map) {
-		const gd::EdgeConnectionPair &pair = pair_it.value;
+	free_edges.clear();
+	free_edges.reserve(free_edges_count);
 
+	for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : 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];
@@ -159,21 +186,21 @@ void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBui
 			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];
+			if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
+				free_edges.push_back(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;
+	NavMapIteration *map_iteration = r_build.map_iteration;
+
+	real_t edge_connection_margin = r_build.edge_connection_margin;
+	LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
+	HashMap<uint32_t, LocalVector<gd::Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
+
 	// Find the compatible near edges.
 	//
 	// Note:
@@ -181,26 +208,17 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 	// 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;
-	}
+	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 < iter_free_edges.size(); i++) {
-		const gd::Edge::Connection &free_edge = iter_free_edges[i];
-
+	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;
 
-		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];
+		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;
 			}
@@ -209,8 +227,9 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 			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);
+			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;
 			}
@@ -245,7 +264,7 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 			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);
+			region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
 			performance_data.pm_edge_connection_count += 1;
 		}
 	}
@@ -253,19 +272,26 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
 
 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;
+	Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
+
+	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
+	LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
+	LocalVector<NavLinkIteration> &links = map_iteration->link_iterations;
 	int polygon_count = r_build.polygon_count;
-	int link_polygon_count = r_build.link_polygon_count;
+
+	real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
+	uint32_t link_poly_idx = 0;
+	link_polygons.resize(links.size());
 
 	// Search for polygons within range of a nav link.
-	for (NavLinkIteration &link : map_iteration->link_iterations) {
-		if (!link.enabled) {
+	for (const NavLinkIteration &link : links) {
+		if (!link.get_enabled()) {
 			continue;
 		}
-		const Vector3 link_start_pos = link.start_position;
-		const Vector3 link_end_pos = link.end_position;
+		const Vector3 link_start_pos = link.get_start_position();
+		const Vector3 link_end_pos = link.get_end_position();
 
 		gd::Polygon *closest_start_polygon = nullptr;
 		real_t closest_start_sqr_dist = link_connection_radius_sqr;
@@ -275,37 +301,31 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 		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;
-						}
+		for (gd::Polygon &polyon : 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);
+				{
+					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;
-						}
+					// 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;
 					}
 				}
 			}
@@ -313,13 +333,10 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 
 		// 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];
+			gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
 			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);
@@ -348,7 +365,7 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 			}
 
 			// If the link is bi-directional, create connections from the end to the start.
-			if (link.bidirectional) {
+			if (link.is_bidirectional()) {
 				gd::Edge::Connection entry_connection;
 				entry_connection.polygon = &new_polygon;
 				entry_connection.edge = -1;
@@ -365,33 +382,23 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 			}
 		}
 	}
-
-	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;
+	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
+	LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
 
-	// 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->navmesh_polygon_count = polygons.size();
+	map_iteration->link_polygon_count = link_polygons.size();
 
 	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);
+		p_path_query_slot.path_corridor.clear();
+		p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
 	}
 	map_iteration->path_query_slots_mutex.unlock();
 }

modules/navigation/3d/nav_map_iteration_3d.h

@@ -79,6 +79,7 @@ struct NavMapIteration {
 
 	Vector3 map_up;
 	LocalVector<gd::Polygon> navmesh_polygons;
+	LocalVector<gd::Polygon> link_polygons;
 
 	LocalVector<NavRegionIteration> region_iterations;
 	LocalVector<NavLinkIteration> link_iterations;

modules/navigation/3d/nav_mesh_queries_3d.cpp

@@ -129,41 +129,17 @@ 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 *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] = 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] = 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] = 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] = 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, 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->owner_type);
+		p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_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->owner_rid);
+		p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
 	}
 
 	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->owner_object_id);
+		p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
 	}
 
 	p_query_task.path_points.push_back(p_point);
@@ -215,49 +191,11 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
 
 	map->query_path(query_task);
 
-	const uint32_t path_point_size = query_task.path_points.size();
-
-	Vector<Vector3> path_points;
-	Vector<int32_t> path_meta_point_types;
-	TypedArray<RID> path_meta_point_rids;
-	Vector<int64_t> path_meta_point_owners;
-
-	{
-		path_points.resize(path_point_size);
-		Vector3 *w = path_points.ptrw();
-		const Vector3 *r = query_task.path_points.ptr();
-		for (uint32_t i = 0; i < path_point_size; i++) {
-			w[i] = r[i];
-		}
-	}
-
-	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
-		path_meta_point_types.resize(path_point_size);
-		int32_t *w = path_meta_point_types.ptrw();
-		const int32_t *r = query_task.path_meta_point_types.ptr();
-		for (uint32_t i = 0; i < path_point_size; i++) {
-			w[i] = r[i];
-		}
-	}
-	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
-		path_meta_point_rids.resize(path_point_size);
-		for (uint32_t i = 0; i < path_point_size; i++) {
-			path_meta_point_rids[i] = query_task.path_meta_point_rids[i];
-		}
-	}
-	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
-		path_meta_point_owners.resize(path_point_size);
-		int64_t *w = path_meta_point_owners.ptrw();
-		const int64_t *r = query_task.path_meta_point_owners.ptr();
-		for (uint32_t i = 0; i < path_point_size; i++) {
-			w[i] = r[i];
-		}
-	}
-
-	p_query_result->set_path(path_points);
-	p_query_result->set_path_types(path_meta_point_types);
-	p_query_result->set_path_rids(path_meta_point_rids);
-	p_query_result->set_path_owner_ids(path_meta_point_owners);
+	p_query_result->set_data(
+			query_task.path_points,
+			query_task.path_meta_point_types,
+			query_task.path_meta_point_rids,
+			query_task.path_meta_point_owners);
 
 	if (query_task.callback.is_valid()) {
 		if (emit_callback(query_task.callback)) {
@@ -269,25 +207,23 @@ 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) {
-	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();
+	p_query_task.path_clear();
 
 	_query_task_find_start_end_positions(p_query_task, p_polygons);
 
-	// Check for trivial cases
+	// Check for trivial cases.
 	if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
-		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
+		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 		return;
 	}
-
 	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);
+		p_query_task.path_clear();
+		_query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon);
+		_query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon);
+		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 		return;
 	}
 
-	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) {
@@ -311,10 +247,7 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
 		} break;
 	}
 
-	p_query_task.path_points.invert();
-	p_query_task.path_meta_point_types.invert();
-	p_query_task.path_meta_point_rids.invert();
-	p_query_task.path_meta_point_owners.invert();
+	p_query_task.path_reverse();
 
 	if (p_query_task.simplify_path) {
 		_query_task_simplified_path_points(p_query_task);
@@ -339,33 +272,32 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
 }
 
 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.
+	const Vector3 p_target_position = p_query_task.target_position;
+	const uint32_t p_navigation_layers = p_query_task.navigation_layers;
+	const gd::Polygon *begin_poly = p_query_task.begin_polygon;
+	const gd::Polygon *end_poly = p_query_task.end_polygon;
+	Vector3 begin_point = p_query_task.begin_position;
+	Vector3 end_point = p_query_task.end_position;
+
+	// Heap of polygons to travel next.
+	gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
+			&traversable_polys = p_query_task.path_query_slot->traversable_polys;
+	traversable_polys.clear();
+
 	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_query_task.link_polygons_size);
-
 	// Initialize the matching navigation polygon.
-	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>
-			&traversable_polys = p_query_task.path_query_slot->traversable_polys;
-	traversable_polys.clear();
-	traversable_polys.reserve(p_polygons.size() * 0.25);
+	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;
 
 	// This is an implementation of the A* algorithm.
-	p_query_task.least_cost_id = begin_polygon->id;
+	int least_cost_id = begin_poly->id;
 	int prev_least_cost_id = -1;
 	bool found_route = false;
 
@@ -375,24 +307,24 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 
 	while (true) {
 		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
-		for (const gd::Edge &edge : navigation_polys[p_query_task.least_cost_id].poly->edges) {
+		for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
 			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
 			for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
 				const 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->navigation_layers) == 0) {
+				if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
 					continue;
 				}
 
-				const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id];
+				const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
 				real_t poly_enter_cost = 0.0;
-				real_t poly_travel_cost = least_cost_poly.poly->owner->travel_cost;
+				real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_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;
+				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();
 				}
-				prev_least_cost_id = p_query_task.least_cost_id;
+				prev_least_cost_id = least_cost_id;
 
 				Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
 				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
@@ -405,14 +337,14 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 					// it is shorter, update the polygon.
 					if (neighbor_poly.traversable_poly_index < traversable_polys.size() &&
 							new_traveled_distance < neighbor_poly.traveled_distance) {
-						neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
+						neighbor_poly.back_navigation_poly_id = least_cost_id;
 						neighbor_poly.back_navigation_edge = connection.edge;
 						neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
 						neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
 						neighbor_poly.traveled_distance = new_traveled_distance;
 						neighbor_poly.distance_to_destination =
-								new_entry.distance_to(end_position) *
-								neighbor_poly.poly->owner->travel_cost;
+								new_entry.distance_to(end_point) *
+								neighbor_poly.poly->owner->get_travel_cost();
 						neighbor_poly.entry = new_entry;
 
 						// Update the priority of the polygon in the heap.
@@ -421,14 +353,14 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 				} else {
 					// Initialize the matching navigation polygon.
 					neighbor_poly.poly = connection.polygon;
-					neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
+					neighbor_poly.back_navigation_poly_id = least_cost_id;
 					neighbor_poly.back_navigation_edge = connection.edge;
 					neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
 					neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
 					neighbor_poly.traveled_distance = new_traveled_distance;
 					neighbor_poly.distance_to_destination =
-							new_entry.distance_to(end_position) *
-							neighbor_poly.poly->owner->travel_cost;
+							new_entry.distance_to(end_point) *
+							neighbor_poly.poly->owner->get_travel_cost();
 					neighbor_poly.entry = new_entry;
 
 					// Add the polygon to the heap of polygons to traverse next.
@@ -449,33 +381,37 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			}
 
 			// Set as end point the furthest reachable point.
-			end_polygon = reachable_end;
+			end_poly = reachable_end;
 			real_t end_d = FLT_MAX;
-			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);
+			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);
+				Vector3 spoint = f.get_closest_point_to(p_target_position);
+				real_t dpoint = spoint.distance_to(p_target_position);
 				if (dpoint < end_d) {
-					end_position = spoint;
+					end_point = 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_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);
+			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);
+				Vector3 spoint = f.get_closest_point_to(p_target_position);
+				real_t dpoint = spoint.distance_to(p_target_position);
 				if (dpoint < end_d) {
-					end_position = spoint;
+					end_point = 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_polygon, end_polygon);
+				// No point to run PostProcessing when start and end convex polygon is the same.
+				p_query_task.path_clear();
+
+				_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+				_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
 				p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 				return;
 			}
@@ -483,9 +419,9 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 			for (gd::NavigationPoly &nav_poly : navigation_polys) {
 				nav_poly.poly = nullptr;
 			}
-			navigation_polys[begin_polygon->id].poly = begin_polygon;
+			navigation_polys[begin_poly->id].poly = begin_poly;
 
-			p_query_task.least_cost_id = begin_polygon->id;
+			least_cost_id = begin_poly->id;
 			prev_least_cost_id = -1;
 
 			reachable_end = nullptr;
@@ -494,19 +430,19 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 		}
 
 		// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
-		p_query_task.least_cost_id = traversable_polys.pop()->poly->id;
+		least_cost_id = traversable_polys.pop()->poly->id;
 
 		// Store the farthest reachable end polygon in case our goal is not reachable.
 		if (is_reachable) {
-			real_t distance = navigation_polys[p_query_task.least_cost_id].entry.distance_to(p_query_task.target_position);
+			real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_target_position);
 			if (distance_to_reachable_end > distance) {
 				distance_to_reachable_end = distance;
-				reachable_end = navigation_polys[p_query_task.least_cost_id].poly;
+				reachable_end = navigation_polys[least_cost_id].poly;
 			}
 		}
 
 		// Check if we reached the end
-		if (navigation_polys[p_query_task.least_cost_id].poly == end_polygon) {
+		if (navigation_polys[least_cost_id].poly == end_poly) {
 			found_route = true;
 			break;
 		}
@@ -517,19 +453,29 @@ 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_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);
+		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);
+			Vector3 spoint = f.get_closest_point_to(p_target_position);
+			real_t dpoint = spoint.distance_to(p_target_position);
 			if (dpoint < end_d) {
-				end_position = spoint;
+				end_point = spoint;
 				end_d = dpoint;
 			}
 		}
-		_query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, begin_polygon);
+
+		p_query_task.path_clear();
+
+		_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+		_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
 		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
 		return;
 	}
+
+	p_query_task.end_position = end_point;
+	p_query_task.end_polygon = end_poly;
+	p_query_task.begin_position = begin_point;
+	p_query_task.begin_polygon = begin_poly;
+	p_query_task.least_cost_id = least_cost_id;
 }
 
 void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
@@ -574,26 +520,63 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D
 	}
 }
 
+void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
+	real_t begin_d = FLT_MAX;
+	real_t end_d = FLT_MAX;
+
+	// Find the initial poly and the end poly on this map.
+	for (const gd::Polygon &p : 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) {
+			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);
+
+			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;
+				p_query_task.begin_polygon = &p;
+				p_query_task.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;
+				p_query_task.end_polygon = &p;
+				p_query_task.end_position = point;
+			}
+		}
+	}
+}
+
 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;
+	Vector3 end_point = p_query_task.end_position;
+	const gd::Polygon *end_poly = p_query_task.end_polygon;
+	Vector3 begin_point = p_query_task.begin_position;
+	const gd::Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+	Vector3 p_map_up = p_query_task.map_up;
 
 	// Set the apex poly/point to the end point
-	gd::NavigationPoly *apex_poly = &p_path_corridor[p_query_task.least_cost_id];
+	gd::NavigationPoly *apex_poly = &navigation_polys[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(end_position, back_pathway);
-	if (end_position.is_equal_approx(back_edge_closest_point)) {
+	const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
+	if (end_point.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) {
-			apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id];
+			apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
 		}
 	}
 
-	Vector3 apex_point = end_position;
+	Vector3 apex_point = end_point;
 
 	gd::NavigationPoly *left_poly = apex_poly;
 	Vector3 left_portal = apex_point;
@@ -602,20 +585,20 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
 
 	gd::NavigationPoly *p = apex_poly;
 
-	_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
 
 	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(map_up) < 0) {
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
 			SWAP(left, right);
 		}
 
 		bool skip = false;
-		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(map_up) >= 0) {
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
 			//process
-			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(map_up) > 0) {
+			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
 				left_poly = p;
 				left_portal = left;
 			} else {
@@ -629,14 +612,13 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
 				right_portal = apex_point;
 
 				_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
-
 				skip = true;
 			}
 		}
 
-		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(map_up) <= 0) {
+		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
 			//process
-			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(map_up) < 0) {
+			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
 				right_poly = p;
 				right_portal = right;
 			} else {
@@ -655,7 +637,7 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
 
 		// Go to the previous polygon.
 		if (p->back_navigation_poly_id != -1) {
-			p = &p_path_corridor[p->back_navigation_poly_id];
+			p = &navigation_polys[p->back_navigation_poly_id];
 		} else {
 			// The end
 			p = nullptr;
@@ -663,95 +645,59 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
 	}
 
 	// 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] != begin_position) {
-		_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
+	if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_point) {
+		_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
 	}
 }
 
 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;
+	Vector3 end_point = p_query_task.end_position;
+	const gd::Polygon *end_poly = p_query_task.end_polygon;
+	Vector3 begin_point = p_query_task.begin_position;
+	const gd::Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
 
-	_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
 
-	// Add mid points.
-	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;
-			int prev_n = (p_path_corridor[np_id].back_navigation_edge + 1) % p_path_corridor[np_id].poly->points.size();
-			Vector3 point = (p_path_corridor[np_id].poly->points[prev].pos + p_path_corridor[np_id].poly->points[prev_n].pos) * 0.5;
+	// Add mid points
+	int np_id = least_cost_id;
+	while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+		if (navigation_polys[np_id].back_navigation_edge != -1) {
+			int prev = navigation_polys[np_id].back_navigation_edge;
+			int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
+			Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
 
-			_query_task_push_back_point_with_metadata(p_query_task, point, p_path_corridor[np_id].poly);
+			_query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly);
 		} else {
-			_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
+			_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
 		}
 
-		np_id = p_path_corridor[np_id].back_navigation_poly_id;
+		np_id = navigation_polys[np_id].back_navigation_poly_id;
 	}
 
-	_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
+	_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
 }
 
 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, end_position, p_query_task.end_polygon);
-
-	// Add mid points.
-	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, 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) {
-	// 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 end_point = p_query_task.end_position;
+	const gd::Polygon *end_poly = p_query_task.end_polygon;
+	Vector3 begin_point = p_query_task.begin_position;
+	const gd::Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
 
-	Vector3 begin_position;
-	Vector3 end_position;
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
 
-	// Find the initial poly and the end poly on this map.
-	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 & polygon.owner->navigation_layers) == 0) {
-			continue;
-		}
-
-		// For each face check the distance between the origin/destination.
-		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);
+	// Add mid points
+	int np_id = least_cost_id;
+	while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+		_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
 
-			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;
-				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;
-				p_query_task.end_polygon = &polygon;
-				end_position = point;
-			}
-		}
+		np_id = navigation_polys[np_id].back_navigation_poly_id;
 	}
 
-	p_query_task.begin_position = begin_position;
-	p_query_task.end_position = end_position;
+	_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
 }
 
 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) {
@@ -878,7 +824,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->owner_rid;
+				result.owner = polygon.owner->get_self();
 
 				if (Math::is_zero_approx(distance)) {
 					break;
@@ -890,7 +836,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->owner_rid;
+				result.owner = polygon.owner->get_self();
 			}
 		}
 	}
@@ -904,16 +850,16 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Pol
 }
 
 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];
+	const LocalVector<gd::NavigationPoly> &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
+	const Vector3 p_map_up = p_query_task.map_up;
 
 	if (from.is_equal_approx(p_to_point)) {
 		return;
 	}
 
 	Plane cut_plane;
-	cut_plane.normal = (from - p_to_point).cross(map_up);
+	cut_plane.normal = (from - p_to_point).cross(p_map_up);
 	if (cut_plane.normal == Vector3()) {
 		return;
 	}
@@ -925,7 +871,7 @@ void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_tas
 		Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
 
 		ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
-		from_poly = &path_corridor[from_poly->back_navigation_poly_id];
+		from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
 
 		if (!pathway_start.is_equal_approx(pathway_end)) {
 			Vector3 inters;

modules/navigation/3d/nav_mesh_queries_3d.h

@@ -82,7 +82,6 @@ public:
 		Vector3 map_up;
 		NavMap *map = nullptr;
 		PathQuerySlot *path_query_slot = nullptr;
-		uint32_t link_polygons_size = 0;
 
 		// Path points.
 		LocalVector<Vector3> path_points;
@@ -94,6 +93,20 @@ public:
 		Ref<NavigationPathQueryResult3D> query_result;
 		Callable callback;
 		NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
+
+		void path_clear() {
+			path_points.clear();
+			path_meta_point_types.clear();
+			path_meta_point_rids.clear();
+			path_meta_point_owners.clear();
+		}
+
+		void path_reverse() {
+			path_points.invert();
+			path_meta_point_types.invert();
+			path_meta_point_rids.invert();
+			path_meta_point_owners.invert();
+		}
 	};
 
 	static bool emit_callback(const Callable &p_callback);
@@ -109,7 +122,6 @@ 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);
-	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);

modules/navigation/3d/nav_region_iteration_3d.h

@@ -41,6 +41,11 @@ struct NavRegionIteration : NavBaseIteration {
 	LocalVector<gd::Polygon> navmesh_polygons;
 	real_t surface_area = 0.0;
 	AABB bounds;
+
+	const Transform3D &get_transform() const { return transform; }
+	const LocalVector<gd::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
+	real_t get_surface_area() const { return surface_area; }
+	AABB get_bounds() const { return bounds; }
 };
 
 #endif // NAV_REGION_ITERATION_3D_H

modules/navigation/nav_link.cpp

@@ -129,8 +129,10 @@ void NavLink::get_iteration_update(NavLinkIteration &r_iteration) {
 	r_iteration.travel_cost = get_travel_cost();
 	r_iteration.owner_object_id = get_owner_id();
 	r_iteration.owner_type = get_type();
+	r_iteration.owner_rid = get_self();
 
-	r_iteration.enabled = enabled;
-	r_iteration.start_position = start_position;
-	r_iteration.end_position = end_position;
+	r_iteration.enabled = get_enabled();
+	r_iteration.start_position = get_start_position();
+	r_iteration.end_position = get_end_position();
+	r_iteration.bidirectional = is_bidirectional();
 }

modules/navigation/nav_link.h

@@ -40,6 +40,10 @@ struct NavLinkIteration : NavBaseIteration {
 	Vector3 start_position;
 	Vector3 end_position;
 	LocalVector<gd::Polygon> navmesh_polygons;
+
+	Vector3 get_start_position() const { return start_position; }
+	Vector3 get_end_position() const { return end_position; }
+	bool is_bidirectional() const { return bidirectional; }
 };
 
 #include "core/templates/self_list.h"

modules/navigation/nav_map.cpp

@@ -165,7 +165,6 @@ void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task)
 	}
 
 	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, map_iteration.navmesh_polygons);
 

modules/navigation/nav_region.cpp

@@ -250,51 +250,21 @@ void NavRegion::get_iteration_update(NavRegionIteration &r_iteration) {
 	r_iteration.travel_cost = get_travel_cost();
 	r_iteration.owner_object_id = get_owner_id();
 	r_iteration.owner_type = get_type();
+	r_iteration.owner_rid = get_self();
 
-	r_iteration.enabled = enabled;
-	r_iteration.transform = transform;
-	r_iteration.owner_use_edge_connections = use_edge_connections;
+	r_iteration.enabled = get_enabled();
+	r_iteration.transform = get_transform();
+	r_iteration.owner_use_edge_connections = get_use_edge_connections();
 	r_iteration.bounds = get_bounds();
+	r_iteration.surface_area = get_surface_area();
 
+	r_iteration.navmesh_polygons.clear();
 	r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
-
 	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
-		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];
-			}
-		}
+		gd::Polygon &navmesh_polygon = navmesh_polygons[i];
+		navmesh_polygon.owner = &r_iteration;
+		r_iteration.navmesh_polygons[i] = navmesh_polygon;
 	}
-
-	r_iteration.surface_area = surface_area;
-	r_iteration.owner_rid = get_self();
 }
 
 void NavRegion::request_sync() {

servers/navigation/navigation_path_query_result_3d.cpp

@@ -69,6 +69,47 @@ void NavigationPathQueryResult3D::reset() {
 	path_owner_ids.clear();
 }
 
+void NavigationPathQueryResult3D::set_data(const LocalVector<Vector3> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids) {
+	path.clear();
+	path_types.clear();
+	path_rids.clear();
+	path_owner_ids.clear();
+
+	{
+		path.resize(p_path.size());
+		Vector3 *w = path.ptrw();
+		const Vector3 *r = p_path.ptr();
+		for (uint32_t i = 0; i < p_path.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+
+	{
+		path_types.resize(p_path_types.size());
+		int32_t *w = path_types.ptrw();
+		const int32_t *r = p_path_types.ptr();
+		for (uint32_t i = 0; i < p_path_types.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+
+	{
+		path_rids.resize(p_path_rids.size());
+		for (uint32_t i = 0; i < p_path_rids.size(); i++) {
+			path_rids[i] = p_path_rids[i];
+		}
+	}
+
+	{
+		path_owner_ids.resize(p_path_owner_ids.size());
+		int64_t *w = path_owner_ids.ptrw();
+		const int64_t *r = p_path_owner_ids.ptr();
+		for (uint32_t i = 0; i < p_path_owner_ids.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+}
+
 void NavigationPathQueryResult3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_path", "path"), &NavigationPathQueryResult3D::set_path);
 	ClassDB::bind_method(D_METHOD("get_path"), &NavigationPathQueryResult3D::get_path);

servers/navigation/navigation_path_query_result_3d.h

@@ -65,6 +65,8 @@ public:
 	const Vector<int64_t> &get_path_owner_ids() const;
 
 	void reset();
+
+	void set_data(const LocalVector<Vector3> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids);
 };
 
 VARIANT_ENUM_CAST(NavigationPathQueryResult3D::PathSegmentType);