From 4184884ad1eab8f326bfedd6273e106c9e2e5fc6 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Thu, 2 Jan 2025 15:35:29 +0100 Subject: [PATCH] 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 | 135 +++--- modules/navigation/3d/nav_map_iteration_3d.h | 1 - modules/navigation/3d/nav_mesh_queries_3d.cpp | 415 ++++++++++++++---- modules/navigation/3d/nav_mesh_queries_3d.h | 14 +- modules/navigation/nav_map.cpp | 72 +-- 5 files changed, 407 insertions(+), 230 deletions(-) diff --git a/modules/navigation/3d/nav_map_builder_3d.cpp b/modules/navigation/3d/nav_map_builder_3d.cpp index 96cd7401bf..256d984c4d 100644 --- a/modules/navigation/3d/nav_map_builder_3d.cpp +++ b/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 &polygons = map_iteration->navmesh_polygons; LocalVector ®ions = map_iteration->region_iterations; HashMap> ®ion_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(); } - // Resize the polygon count. - int polygon_count = 0; - for (const NavRegionIteration ®ion : 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 ®ion : regions) { + int polygon_count = 0; + for (NavRegionIteration ®ion : regions) { if (!region.get_enabled()) { continue; } - const LocalVector &polygons_source = region.get_navmesh_polygons(); + LocalVector &polygons_source = region.navmesh_polygons; for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons[polygon_count] = polygons_source[n]; - polygons[polygon_count].id = polygon_count; + polygons_source[n].id = polygon_count; polygon_count++; } } @@ -118,44 +106,50 @@ 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 &polygons = map_iteration->navmesh_polygons; HashMap &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 (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); + for (NavRegionIteration ®ion : map_iteration->region_iterations) { + if (!region.get_enabled()) { + continue; + } - HashMap::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; + 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); - pair.connections[pair.size] = new_connection; - ++pair.size; - if (pair.size == 2) { - --free_edges_count; + HashMap::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; - } 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."); + 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."); + } } } } @@ -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 &polygons = map_iteration->navmesh_polygons; LocalVector &link_polygons = map_iteration->link_polygons; LocalVector &links = map_iteration->link_iterations; int polygon_count = r_build.polygon_count; @@ -301,31 +294,42 @@ 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 (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); + for (NavRegionIteration ®ion : 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; + } - { - 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); + 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); - // 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 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; + } } } } @@ -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 &polygons = map_iteration->navmesh_polygons; LocalVector &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(); diff --git a/modules/navigation/3d/nav_map_iteration_3d.h b/modules/navigation/3d/nav_map_iteration_3d.h index f58a52f0f6..47029f0f36 100644 --- a/modules/navigation/3d/nav_map_iteration_3d.h +++ b/modules/navigation/3d/nav_map_iteration_3d.h @@ -78,7 +78,6 @@ struct NavMapIteration { RWLock rwlock; Vector3 map_up; - LocalVector navmesh_polygons; LocalVector link_polygons; LocalVector region_iterations; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp index 43b5e90c03..953465e4a5 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -206,72 +206,49 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref &p_polygons) { - p_query_task.path_clear(); +void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) { + 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 ®ions = p_map_iteration.region_iterations; - // Check for trivial cases. - if (!p_query_task.begin_polygon || !p_query_task.end_polygon) { - p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; - return; + for (const NavRegionIteration ®ion : regions) { + if (!region.get_enabled()) { + continue; + } + + // Find the initial poly and the end poly on this map. + for (const gd::Polygon &p : region.get_navmesh_polygons()) { + // Only consider the polygon if it in a region with compatible layers. + if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) { + continue; + } + + // 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; + } + } + } } - 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); - - 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_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &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 &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 ®ions = p_map_iteration.region_iterations; + for (const NavRegionIteration ®ion : 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 ®ions = p_map_iteration.region_iterations; + for (const NavRegionIteration ®ion : 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 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 ®ion = 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 accessible_regions_area_map; + + for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) { + const NavRegionIteration ®ion = 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::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 &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) { bool use_collision = p_use_collision; Vector3 closest_point; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h index dad06972dc..3ef97c8786 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/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 &p_polygons, const Vector3 &p_point); static RID polygons_get_closest_point_owner(const LocalVector &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 &p_query_parameters, Ref p_query_result, const Callable &p_callback); - static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &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 &p_polygons); - static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &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); 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); diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 273b65da43..c5c6e334f3 100644 --- a/modules/navigation/nav_map.cpp +++ b/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 Vector3(); - } - - LocalVector accessible_regions; - accessible_regions.reserve(map_iteration.region_iterations.size()); - - for (uint32_t i = 0; i < map_iteration.region_iterations.size(); i++) { - const NavRegionIteration ®ion = 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 accessible_regions_area_map; - - for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) { - const NavRegionIteration ®ion = 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::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); - } + return NavMeshQueries3D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly); } void NavMap::_build_iteration() {