Make NavMeshQueries use NavRegionIteration polygons directly

Removes the duplicated NavMap polygon soup. All navmesh queries now use the NavRegionIteration polygons directly.

modules/navigation/3d/nav_map_builder_3d.cpp

@@ -77,7 +77,6 @@ void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r
 	gd::PerformanceData &performance_data = r_build.performance_data;
 	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;
 
@@ -87,26 +86,15 @@ void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r
 		region_external_connections[region.id] = LocalVector<gd::Edge::Connection>();
 	}
 
-	// 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;
+	int polygon_count = 0;
-	for (const NavRegionIteration &region : regions) {
+	for (NavRegionIteration &region : regions) {
 		if (!region.get_enabled()) {
 			continue;
 		}
-		const LocalVector<gd::Polygon> &polygons_source = region.get_navmesh_polygons();
+		LocalVector<gd::Polygon> &polygons_source = region.navmesh_polygons;
 		for (uint32_t n = 0; n < polygons_source.size(); n++) {
-			polygons[polygon_count] = polygons_source[n];
+			polygons_source[n].id = polygon_count;
-			polygons[polygon_count].id = polygon_count;
 			polygon_count++;
 		}
 	}
@@ -118,16 +106,21 @@ void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r
 void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build) {
 	gd::PerformanceData &performance_data = r_build.performance_data;
 	NavMapIteration *map_iteration = r_build.map_iteration;
+	int polygon_count = r_build.polygon_count;
 
-	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
 	HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
 
 	// Group all edges per key.
 	connection_pairs_map.clear();
-	connection_pairs_map.reserve(polygons.size());
+	connection_pairs_map.reserve(polygon_count);
 	int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
 
-	for (gd::Polygon &poly : polygons) {
+	for (NavRegionIteration &region : map_iteration->region_iterations) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+
+		for (gd::Polygon &poly : region.navmesh_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);
@@ -159,6 +152,7 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
 				}
 			}
 		}
+	}
 
 	r_build.free_edge_count = free_edges_count;
 }
@@ -276,7 +270,6 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 	real_t link_connection_radius = r_build.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;
@@ -301,7 +294,17 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 		real_t closest_end_sqr_dist = link_connection_radius_sqr;
 		Vector3 closest_end_point;
 
-		for (gd::Polygon &polyon : polygons) {
+		for (NavRegionIteration &region : map_iteration->region_iterations) {
+			if (!region.get_enabled()) {
+				continue;
+			}
+			AABB region_bounds = region.get_bounds().grow(link_connection_radius);
+			if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
+				continue;
+			}
+
+			for (gd::Polygon &polyon : region.navmesh_polygons) {
+				//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);
 
@@ -330,6 +333,7 @@ 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) {
@@ -387,10 +391,9 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
 void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) {
 	NavMapIteration *map_iteration = r_build.map_iteration;
 
-	LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
 	LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
 
-	map_iteration->navmesh_polygon_count = polygons.size();
+	map_iteration->navmesh_polygon_count = r_build.polygon_count;
 	map_iteration->link_polygon_count = link_polygons.size();
 
 	map_iteration->path_query_slots_mutex.lock();

modules/navigation/3d/nav_map_iteration_3d.h

@@ -78,7 +78,6 @@ struct NavMapIteration {
 	RWLock rwlock;
 
 	Vector3 map_up;
-	LocalVector<gd::Polygon> navmesh_polygons;
 	LocalVector<gd::Polygon> link_polygons;
 
 	LocalVector<NavRegionIteration> region_iterations;

modules/navigation/3d/nav_mesh_queries_3d.cpp

@@ -206,72 +206,49 @@ 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) {
+void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
-	p_query_task.path_clear();
+	real_t begin_d = FLT_MAX;
+	real_t end_d = FLT_MAX;
 
-	_query_task_find_start_end_positions(p_query_task, p_polygons);
+	const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
 
-	// Check for trivial cases.
+	for (const NavRegionIteration &region : regions) {
-	if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
+		if (!region.get_enabled()) {
-		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
+			continue;
-		return;
-	}
-	if (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;
 		}
 
-	_query_task_build_path_corridor(p_query_task, p_polygons);
+		// Find the initial poly and the end poly on this map.
-
+		for (const gd::Polygon &p : region.get_navmesh_polygons()) {
-	if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
+			// Only consider the polygon if it in a region with compatible layers.
-		return;
+			if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
+				continue;
 			}
 
-	// Post-Process path.
+			// For each face check the distance between the origin/destination.
-	switch (p_query_task.path_postprocessing) {
+			for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
-		case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+				const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
-			_query_task_post_process_corridorfunnel(p_query_task);
+
-		} break;
+				Vector3 point = face.get_closest_point_to(p_query_task.start_position);
-		case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+				real_t distance_to_point = point.distance_to(p_query_task.start_position);
-			_query_task_post_process_edgecentered(p_query_task);
+				if (distance_to_point < begin_d) {
-		} break;
+					begin_d = distance_to_point;
-		case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+					p_query_task.begin_polygon = &p;
-			_query_task_post_process_nopostprocessing(p_query_task);
+					p_query_task.begin_position = point;
-		} break;
-		default: {
-			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
-			_query_task_post_process_corridorfunnel(p_query_task);
-		} break;
 				}
 
-	p_query_task.path_reverse();
+				point = face.get_closest_point_to(p_query_task.target_position);
-
+				distance_to_point = point.distance_to(p_query_task.target_position);
-	if (p_query_task.simplify_path) {
+				if (distance_to_point < end_d) {
-		_query_task_simplified_path_points(p_query_task);
+					end_d = distance_to_point;
+					p_query_task.end_polygon = &p;
+					p_query_task.end_position = point;
 				}
-
-#ifdef DEBUG_ENABLED
-	// Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
-	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
-		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
 			}
-
-	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
-		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
 		}
-
-	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
-		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
 	}
-#endif // DEBUG_ENABLED
-
-	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) {
+void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task) {
 	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;
@@ -478,6 +455,71 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
 	p_query_task.least_cost_id = least_cost_id;
 }
 
+void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
+	p_query_task.path_clear();
+
+	_query_task_find_start_end_positions(p_query_task, p_map_iteration);
+
+	// Check for trivial cases.
+	if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
+		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
+		return;
+	}
+	if (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;
+	}
+
+	_query_task_build_path_corridor(p_query_task);
+
+	if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
+		return;
+	}
+
+	// Post-Process path.
+	switch (p_query_task.path_postprocessing) {
+		case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			_query_task_post_process_corridorfunnel(p_query_task);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			_query_task_post_process_edgecentered(p_query_task);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			_query_task_post_process_nopostprocessing(p_query_task);
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			_query_task_post_process_corridorfunnel(p_query_task);
+		} break;
+	}
+
+	p_query_task.path_reverse();
+
+	if (p_query_task.simplify_path) {
+		_query_task_simplified_path_points(p_query_task);
+	}
+
+#ifdef DEBUG_ENABLED
+	// Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
+	}
+#endif // DEBUG_ENABLED
+
+	p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
+}
+
 void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
 	if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
 		return;
@@ -520,40 +562,6 @@ 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) {
 	Vector3 end_point = p_query_task.end_position;
 	const gd::Polygon *end_poly = p_query_task.end_polygon;
@@ -700,6 +708,223 @@ void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQuer
 	_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
 }
 
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMapIteration &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
+	bool use_collision = p_use_collision;
+	Vector3 closest_point;
+	real_t closest_point_distance = FLT_MAX;
+
+	const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
+	for (const NavRegionIteration &region : regions) {
+		for (const gd::Polygon &polygon : region.get_navmesh_polygons()) {
+			// For each face check the distance to the segment.
+			for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) {
+				const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
+				Vector3 intersection_point;
+				if (face.intersects_segment(p_from, p_to, &intersection_point)) {
+					const real_t d = p_from.distance_to(intersection_point);
+					if (!use_collision) {
+						closest_point = intersection_point;
+						use_collision = true;
+						closest_point_distance = d;
+					} else if (closest_point_distance > d) {
+						closest_point = intersection_point;
+						closest_point_distance = d;
+					}
+				}
+				// If segment does not itersect face, check the distance from segment's endpoints.
+				else if (!use_collision) {
+					const Vector3 p_from_closest = face.get_closest_point_to(p_from);
+					const real_t d_p_from = p_from.distance_to(p_from_closest);
+					if (closest_point_distance > d_p_from) {
+						closest_point = p_from_closest;
+						closest_point_distance = d_p_from;
+					}
+
+					const Vector3 p_to_closest = face.get_closest_point_to(p_to);
+					const real_t d_p_to = p_to.distance_to(p_to_closest);
+					if (closest_point_distance > d_p_to) {
+						closest_point = p_to_closest;
+						closest_point_distance = d_p_to;
+					}
+				}
+			}
+			// Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment.
+			if (!use_collision) {
+				for (size_t point_id = 0; point_id < polygon.points.size(); point_id += 1) {
+					Vector3 a, b;
+
+					Geometry3D::get_closest_points_between_segments(
+							p_from,
+							p_to,
+							polygon.points[point_id].pos,
+							polygon.points[(point_id + 1) % polygon.points.size()].pos,
+							a,
+							b);
+
+					const real_t d = a.distance_to(b);
+					if (d < closest_point_distance) {
+						closest_point_distance = d;
+						closest_point = b;
+					}
+				}
+			}
+		}
+	}
+
+	return closest_point;
+}
+
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
+	gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+	return cp.point;
+}
+
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point_normal(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
+	gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+	return cp.normal;
+}
+
+RID NavMeshQueries3D::map_iteration_get_closest_point_owner(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
+	gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+	return cp.owner;
+}
+
+gd::ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
+	gd::ClosestPointQueryResult result;
+	real_t closest_point_distance_squared = FLT_MAX;
+
+	const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
+	for (const NavRegionIteration &region : regions) {
+		for (const gd::Polygon &polygon : region.get_navmesh_polygons()) {
+			Vector3 plane_normal = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
+			Vector3 closest_on_polygon;
+			real_t closest = FLT_MAX;
+			bool inside = true;
+			Vector3 previous = polygon.points[polygon.points.size() - 1].pos;
+			for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
+				Vector3 edge = polygon.points[point_id].pos - previous;
+				Vector3 to_point = p_point - previous;
+				Vector3 edge_to_point_pormal = edge.cross(to_point);
+				bool clockwise = edge_to_point_pormal.dot(plane_normal) > 0;
+				// If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
+				if (!clockwise) {
+					inside = false;
+					real_t point_projected_on_edge = edge.dot(to_point);
+					real_t edge_square = edge.length_squared();
+
+					if (point_projected_on_edge > edge_square) {
+						real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
+						if (distance < closest) {
+							closest_on_polygon = polygon.points[point_id].pos;
+							closest = distance;
+						}
+					} else if (point_projected_on_edge < 0.f) {
+						real_t distance = previous.distance_squared_to(p_point);
+						if (distance < closest) {
+							closest_on_polygon = previous;
+							closest = distance;
+						}
+					} else {
+						// If we project on this edge, this will be the closest point.
+						real_t percent = point_projected_on_edge / edge_square;
+						closest_on_polygon = previous + percent * edge;
+						break;
+					}
+				}
+				previous = polygon.points[point_id].pos;
+			}
+
+			if (inside) {
+				Vector3 plane_normalized = plane_normal.normalized();
+				real_t distance = plane_normalized.dot(p_point - polygon.points[0].pos);
+				real_t distance_squared = distance * distance;
+				if (distance_squared < closest_point_distance_squared) {
+					closest_point_distance_squared = distance_squared;
+					result.point = p_point - plane_normalized * distance;
+					result.normal = plane_normal;
+					result.owner = polygon.owner->get_self();
+
+					if (Math::is_zero_approx(distance)) {
+						break;
+					}
+				}
+			} else {
+				real_t distance = closest_on_polygon.distance_squared_to(p_point);
+				if (distance < closest_point_distance_squared) {
+					closest_point_distance_squared = distance;
+					result.point = closest_on_polygon;
+					result.normal = plane_normal;
+					result.owner = polygon.owner->get_self();
+				}
+			}
+		}
+	}
+
+	return result;
+}
+
+Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
+	if (p_map_iteration.region_iterations.is_empty()) {
+		return Vector3();
+	}
+
+	LocalVector<uint32_t> accessible_regions;
+	accessible_regions.reserve(p_map_iteration.region_iterations.size());
+
+	for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
+		const NavRegionIteration &region = p_map_iteration.region_iterations[i];
+		if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
+			continue;
+		}
+		accessible_regions.push_back(i);
+	}
+
+	if (accessible_regions.is_empty()) {
+		// All existing region polygons are disabled.
+		return Vector3();
+	}
+
+	if (p_uniformly) {
+		real_t accumulated_region_surface_area = 0;
+		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 NavRegionIteration &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
+
+			real_t region_surface_area = region.surface_area;
+
+			if (region_surface_area == 0.0f) {
+				continue;
+			}
+
+			accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
+			accumulated_region_surface_area += region_surface_area;
+		}
+		if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
+			// All faces have no real surface / no area.
+			return Vector3();
+		}
+
+		real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
+
+		RBMap<real_t, uint32_t>::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
+		ERR_FAIL_COND_V(!E, Vector3());
+		uint32_t random_region_index = E->value;
+		ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
+
+		const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+
+	} else {
+		uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
+
+		const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+	}
+}
+
 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) {
 	bool use_collision = p_use_collision;
 	Vector3 closest_point;

modules/navigation/3d/nav_mesh_queries_3d.h

@@ -42,6 +42,7 @@
 using namespace NavigationUtilities;
 
 class NavMap;
+struct NavMapIteration;
 
 class NavMeshQueries3D {
 public:
@@ -119,12 +120,19 @@ public:
 	static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 	static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 
+	static Vector3 map_iteration_get_closest_point_to_segment(const NavMapIteration &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
+	static Vector3 map_iteration_get_closest_point(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
+	static Vector3 map_iteration_get_closest_point_normal(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
+	static RID map_iteration_get_closest_point_owner(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
+	static gd::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
+	static Vector3 map_iteration_get_random_point(const NavMapIteration &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
+
 	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_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration);
 	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_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration);
-	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task);
 	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);

modules/navigation/nav_map.cpp

@@ -166,7 +166,7 @@ void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task)
 
 	p_query_task.map_up = map_iteration.map_up;
 
-	NavMeshQueries3D::query_task_polygons_get_path(p_query_task, map_iteration.navmesh_polygons);
+	NavMeshQueries3D::query_task_map_iteration_get_path(p_query_task, map_iteration);
 
 	map_iteration.path_query_slots_mutex.lock();
 	uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
@@ -185,7 +185,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
 
 	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point_to_segment(map_iteration.navmesh_polygons, p_from, p_to, p_use_collision);
+	return NavMeshQueries3D::map_iteration_get_closest_point_to_segment(map_iteration, p_from, p_to, p_use_collision);
 }
 
 Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
@@ -196,7 +196,7 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
 
 	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point(map_iteration.navmesh_polygons, p_point);
+	return NavMeshQueries3D::map_iteration_get_closest_point(map_iteration, p_point);
 }
 
 Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
@@ -207,7 +207,7 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
 
 	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point_normal(map_iteration.navmesh_polygons, p_point);
+	return NavMeshQueries3D::map_iteration_get_closest_point_normal(map_iteration, p_point);
 }
 
 RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
@@ -218,13 +218,13 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
 
 	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point_owner(map_iteration.navmesh_polygons, p_point);
+	return NavMeshQueries3D::map_iteration_get_closest_point_owner(map_iteration, p_point);
 }
 
 gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
 	GET_MAP_ITERATION_CONST();
 
-	return NavMeshQueries3D::polygons_get_closest_point_info(map_iteration.navmesh_polygons, p_point);
+	return NavMeshQueries3D::map_iteration_get_closest_point_info(map_iteration, p_point);
 }
 
 void NavMap::add_region(NavRegion *p_region) {
@@ -336,65 +336,7 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) {
 Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
 	GET_MAP_ITERATION_CONST();
 
-	if (map_iteration.region_iterations.is_empty()) {
+	return NavMeshQueries3D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
-		return Vector3();
-	}
-
-	LocalVector<uint32_t> accessible_regions;
-	accessible_regions.reserve(map_iteration.region_iterations.size());
-
-	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(i);
-	}
-
-	if (accessible_regions.is_empty()) {
-		// All existing region polygons are disabled.
-		return Vector3();
-	}
-
-	if (p_uniformly) {
-		real_t accumulated_region_surface_area = 0;
-		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 NavRegionIteration &region = map_iteration.region_iterations[accessible_regions[accessible_region_index]];
-
-			real_t region_surface_area = region.surface_area;
-
-			if (region_surface_area == 0.0f) {
-				continue;
-			}
-
-			accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
-			accumulated_region_surface_area += region_surface_area;
-		}
-		if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
-			// All faces have no real surface / no area.
-			return Vector3();
-		}
-
-		real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
-
-		RBMap<real_t, uint32_t>::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
-		ERR_FAIL_COND_V(!E, Vector3());
-		uint32_t random_region_index = E->value;
-		ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
-
-		const NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]];
-
-		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 NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]];
-
-		return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
-	}
 }
 
 void NavMap::_build_iteration() {