From c69408168c2db5cf7ccb1f005a9e96f8265ea1b0 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Thu, 26 Dec 2024 04:50:33 +0100 Subject: [PATCH] Patch navigation map async synchronization Patches navigation map async synchronization. --- modules/navigation/3d/nav_base_iteration_3d.h | 9 + modules/navigation/3d/nav_map_builder_3d.cpp | 279 ++++++------ modules/navigation/3d/nav_map_iteration_3d.h | 1 + modules/navigation/3d/nav_mesh_queries_3d.cpp | 408 ++++++++---------- modules/navigation/3d/nav_mesh_queries_3d.h | 16 +- .../navigation/3d/nav_region_iteration_3d.h | 5 + modules/navigation/nav_link.cpp | 8 +- modules/navigation/nav_link.h | 4 + modules/navigation/nav_map.cpp | 1 - modules/navigation/nav_region.cpp | 48 +-- .../navigation_path_query_result_3d.cpp | 41 ++ .../navigation_path_query_result_3d.h | 2 + 12 files changed, 410 insertions(+), 412 deletions(-) diff --git a/modules/navigation/3d/nav_base_iteration_3d.h b/modules/navigation/3d/nav_base_iteration_3d.h index 65e79af1a2ac..d289c5d533a8 100644 --- a/modules/navigation/3d/nav_base_iteration_3d.h +++ b/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 diff --git a/modules/navigation/3d/nav_map_builder_3d.cpp b/modules/navigation/3d/nav_map_builder_3d.cpp index bdac1b5c45a5..96cd7401bf18 100644 --- a/modules/navigation/3d/nav_map_builder_3d.cpp +++ b/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 &polygons = map_iteration->navmesh_polygons; + LocalVector ®ions = map_iteration->region_iterations; + HashMap> ®ion_external_connections = map_iteration->external_region_connections; // Remove regions connections. - map_iteration->external_region_connections.clear(); - for (const NavRegionIteration ®ion : map_iteration->region_iterations) { - map_iteration->external_region_connections[region.id] = LocalVector(); + region_external_connections.clear(); + for (const NavRegionIteration ®ion : regions) { + region_external_connections[region.id] = LocalVector(); } - polygon_count = 0; - navmesh_polygon_count = 0; - for (NavRegionIteration ®ion : map_iteration->region_iterations) { - for (gd::Polygon ®ion_polygon : region.navmesh_polygons) { - region_polygon.id = polygon_count; - region_polygon.owner = ®ion; + // 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) { + if (!region.get_enabled()) { + continue; + } + const LocalVector &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 &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 ®ion : map_iteration->region_iterations) { - for (gd::Polygon ®ion_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::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 = ®ion_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 &polygons = map_iteration->navmesh_polygons; + HashMap &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::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 &iter_connection_pairs_map = r_build.iter_connection_pairs_map; - LocalVector &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 &connection_pairs_map = r_build.iter_connection_pairs_map; + LocalVector &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 &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 &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 &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 &free_edges = r_build.iter_free_edges; + HashMap> ®ion_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 &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; - 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 ®ion : 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 &polygons = map_iteration->navmesh_polygons; + LocalVector &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 ®ion : map_iteration->region_iterations) { - for (gd::Polygon ®ion_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(); } diff --git a/modules/navigation/3d/nav_map_iteration_3d.h b/modules/navigation/3d/nav_map_iteration_3d.h index 74a94e2c2515..f58a52f0f6f9 100644 --- a/modules/navigation/3d/nav_map_iteration_3d.h +++ b/modules/navigation/3d/nav_map_iteration_3d.h @@ -79,6 +79,7 @@ struct NavMapIteration { Vector3 map_up; LocalVector navmesh_polygons; + LocalVector link_polygons; LocalVector region_iterations; LocalVector link_iterations; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp index 9b7ea5ddfea9..43b5e90c034a 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -129,41 +129,17 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVectorowner->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 Refquery_path(query_task); - const uint32_t path_point_size = query_task.path_points.size(); - - Vector path_points; - Vector path_meta_point_types; - TypedArray path_meta_point_rids; - Vector 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 &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 &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 + &traversable_polys = p_query_task.path_query_slot->traversable_polys; + traversable_polys.clear(); + LocalVector &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 - &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 &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 &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 &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 &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 &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 &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 &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 &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 &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 &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 &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; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h index 7d7202f2c4ec..dad06972dc6b 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -82,7 +82,6 @@ class NavMeshQueries3D { Vector3 map_up; NavMap *map = nullptr; PathQuerySlot *path_query_slot = nullptr; - uint32_t link_polygons_size = 0; // Path points. LocalVector path_points; @@ -94,6 +93,20 @@ class NavMeshQueries3D { Ref 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 @@ class NavMeshQueries3D { 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_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 &p_polygons); static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons); diff --git a/modules/navigation/3d/nav_region_iteration_3d.h b/modules/navigation/3d/nav_region_iteration_3d.h index 0535e64854ef..cee1a2d57758 100644 --- a/modules/navigation/3d/nav_region_iteration_3d.h +++ b/modules/navigation/3d/nav_region_iteration_3d.h @@ -41,6 +41,11 @@ struct NavRegionIteration : NavBaseIteration { LocalVector navmesh_polygons; real_t surface_area = 0.0; AABB bounds; + + const Transform3D &get_transform() const { return transform; } + const LocalVector &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 diff --git a/modules/navigation/nav_link.cpp b/modules/navigation/nav_link.cpp index bb32551a4b79..511edc719738 100644 --- a/modules/navigation/nav_link.cpp +++ b/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(); } diff --git a/modules/navigation/nav_link.h b/modules/navigation/nav_link.h index 623ff00bf661..e6fd7a0e57c8 100644 --- a/modules/navigation/nav_link.h +++ b/modules/navigation/nav_link.h @@ -40,6 +40,10 @@ struct NavLinkIteration : NavBaseIteration { Vector3 start_position; Vector3 end_position; LocalVector 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" diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index b7dbd62d5ff6..273b65da4332 100644 --- a/modules/navigation/nav_map.cpp +++ b/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); diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp index 04cfcbc19524..fbd98c129192 100644 --- a/modules/navigation/nav_region.cpp +++ b/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 &from_points = from_polygon.points; - LocalVector &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 &from_edges = from_polygon.edges; - LocalVector &to_edges = to_polygon.edges; - - to_edges.resize(from_edges.size()); - - for (uint32_t j = 0; j < from_edges.size(); j++) { - const LocalVector &from_connections = from_edges[j].connections; - LocalVector &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() { diff --git a/servers/navigation/navigation_path_query_result_3d.cpp b/servers/navigation/navigation_path_query_result_3d.cpp index 45d9b795359d..a205dec7e3b3 100644 --- a/servers/navigation/navigation_path_query_result_3d.cpp +++ b/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 &p_path, const LocalVector &p_path_types, const LocalVector &p_path_rids, const LocalVector &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); diff --git a/servers/navigation/navigation_path_query_result_3d.h b/servers/navigation/navigation_path_query_result_3d.h index 8c4e89b9b017..a2aa4ac4e06e 100644 --- a/servers/navigation/navigation_path_query_result_3d.h +++ b/servers/navigation/navigation_path_query_result_3d.h @@ -65,6 +65,8 @@ class NavigationPathQueryResult3D : public RefCounted { const Vector &get_path_owner_ids() const; void reset(); + + void set_data(const LocalVector &p_path, const LocalVector &p_path_types, const LocalVector &p_path_rids, const LocalVector &p_path_owner_ids); }; VARIANT_ENUM_CAST(NavigationPathQueryResult3D::PathSegmentType);