diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index 0b6d21a58c7..96935595e4d 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -905,6 +905,13 @@ Returns the travel cost of this [param region]. + + + + + Returns [code]true[/code] if the [param region] uses an async synchronization process that runs on a background thread. + + @@ -986,6 +993,14 @@ Sets the [param travel_cost] for this [param region]. + + + + + + If [param enabled] is [code]true[/code] the [param region] uses an async synchronization process that runs on a background thread. + + diff --git a/modules/navigation_2d/2d/godot_navigation_server_2d.cpp b/modules/navigation_2d/2d/godot_navigation_server_2d.cpp index 498b27ed35e..75c82434c28 100644 --- a/modules/navigation_2d/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation_2d/2d/godot_navigation_server_2d.cpp @@ -457,6 +457,19 @@ uint32_t GodotNavigationServer2D::region_get_iteration_id(RID p_region) const { return region->get_iteration_id(); } +COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled) { + NavRegion2D *region = region_owner.get_or_null(p_region); + ERR_FAIL_NULL(region); + region->set_use_async_iterations(p_enabled); +} + +bool GodotNavigationServer2D::region_get_use_async_iterations(RID p_region) const { + NavRegion2D *region = region_owner.get_or_null(p_region); + ERR_FAIL_NULL_V(region, false); + + return region->get_use_async_iterations(); +} + COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled) { NavRegion2D *region = region_owner.get_or_null(p_region); ERR_FAIL_NULL(region); @@ -581,7 +594,7 @@ void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Refset_navigation_polygon(p_navigation_polygon); + region->set_navigation_mesh(p_navigation_polygon); } int GodotNavigationServer2D::region_get_connections_count(RID p_region) const { diff --git a/modules/navigation_2d/2d/godot_navigation_server_2d.h b/modules/navigation_2d/2d/godot_navigation_server_2d.h index f2505de95a7..d04649245f2 100644 --- a/modules/navigation_2d/2d/godot_navigation_server_2d.h +++ b/modules/navigation_2d/2d/godot_navigation_server_2d.h @@ -146,6 +146,9 @@ public: virtual RID region_create() override; virtual uint32_t region_get_iteration_id(RID p_region) const override; + COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled); + virtual bool region_get_use_async_iterations(RID p_region) const override; + COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled); virtual bool region_get_enabled(RID p_region) const override; diff --git a/modules/navigation_2d/2d/nav_base_iteration_2d.h b/modules/navigation_2d/2d/nav_base_iteration_2d.h index 3a3e438ce81..262bef21b71 100644 --- a/modules/navigation_2d/2d/nav_base_iteration_2d.h +++ b/modules/navigation_2d/2d/nav_base_iteration_2d.h @@ -32,10 +32,13 @@ #include "../nav_utils_2d.h" +#include "core/object/ref_counted.h" #include "servers/navigation/navigation_utilities.h" -struct NavBaseIteration2D { - uint32_t id = UINT32_MAX; +class NavBaseIteration2D : public RefCounted { + GDCLASS(NavBaseIteration2D, RefCounted); + +public: bool enabled = true; uint32_t navigation_layers = 1; real_t enter_cost = 0.0; @@ -45,6 +48,7 @@ struct NavBaseIteration2D { RID owner_rid; bool owner_use_edge_connections = false; LocalVector navmesh_polygons; + LocalVector> internal_connections; bool get_enabled() const { return enabled; } NavigationUtilities::PathSegmentType get_type() const { return owner_type; } @@ -55,4 +59,10 @@ struct NavBaseIteration2D { real_t get_travel_cost() const { return travel_cost; } bool get_use_edge_connections() const { return owner_use_edge_connections; } const LocalVector &get_navmesh_polygons() const { return navmesh_polygons; } + const LocalVector> &get_internal_connections() const { return internal_connections; } + + virtual ~NavBaseIteration2D() { + navmesh_polygons.clear(); + internal_connections.clear(); + } }; diff --git a/modules/navigation_2d/2d/nav_map_builder_2d.cpp b/modules/navigation_2d/2d/nav_map_builder_2d.cpp index 5a22da7df6b..6c8cb8586c5 100644 --- a/modules/navigation_2d/2d/nav_map_builder_2d.cpp +++ b/modules/navigation_2d/2d/nav_map_builder_2d.cpp @@ -76,26 +76,23 @@ void NavMapBuilder2D::_build_step_gather_region_polygons(NavMapIterationBuild2D PerformanceData &performance_data = r_build.performance_data; NavMapIteration2D *map_iteration = r_build.map_iteration; - LocalVector ®ions = map_iteration->region_iterations; - HashMap> ®ion_external_connections = map_iteration->external_region_connections; + const LocalVector> ®ions = map_iteration->region_iterations; + HashMap> ®ion_external_connections = map_iteration->external_region_connections; + + map_iteration->navbases_polygons_external_connections.clear(); // Remove regions connections. region_external_connections.clear(); - for (const NavRegionIteration2D ®ion : regions) { - region_external_connections[region.id] = LocalVector(); - } // Copy all region polygons in the map. int polygon_count = 0; - for (NavRegionIteration2D ®ion : regions) { - if (!region.get_enabled()) { - continue; - } - LocalVector &polygons_source = region.navmesh_polygons; - for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons_source[n].id = polygon_count; - polygon_count++; - } + for (const Ref ®ion : regions) { + const uint32_t polygons_size = region->navmesh_polygons.size(); + polygon_count += polygons_size; + + region_external_connections[region.ptr()] = LocalVector(); + map_iteration->navbases_polygons_external_connections[region.ptr()] = LocalVector>(); + map_iteration->navbases_polygons_external_connections[region.ptr()].resize(polygons_size); } performance_data.pm_polygon_count = polygon_count; @@ -106,7 +103,7 @@ void NavMapBuilder2D::_build_step_find_edge_connection_pairs(NavMapIterationBuil PerformanceData &performance_data = r_build.performance_data; NavMapIteration2D *map_iteration = r_build.map_iteration; int polygon_count = r_build.polygon_count; - const Vector2 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size; + HashMap &connection_pairs_map = r_build.iter_connection_pairs_map; // Group all edges per key. @@ -114,41 +111,33 @@ void NavMapBuilder2D::_build_step_find_edge_connection_pairs(NavMapIterationBuil connection_pairs_map.reserve(polygon_count); int free_edges_count = 0; // How many ConnectionPairs have only one Connection. - for (NavRegionIteration2D ®ion : map_iteration->region_iterations) { - if (!region.get_enabled()) { - continue; - } + for (const Ref ®ion : map_iteration->region_iterations) { + for (const ConnectableEdge &connectable_edge : region->get_external_edges()) { + const EdgeKey &ek = connectable_edge.ek; - for (Polygon &poly : region.navmesh_polygons) { - for (uint32_t p = 0; p < poly.vertices.size(); p++) { - const int next_point = (p + 1) % poly.vertices.size(); - const EdgeKey ek(get_point_key(poly.vertices[p], merge_rasterizer_cell_size), get_point_key(poly.vertices[next_point], merge_rasterizer_cell_size)); + HashMap::Iterator pair_it = connection_pairs_map.find(ek); + if (!pair_it) { + pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair()); + performance_data.pm_edge_count += 1; + ++free_edges_count; + } + EdgeConnectionPair &pair = pair_it->value; + if (pair.size < 2) { + // Add the polygon/edge tuple to this key. + Connection new_connection; + new_connection.polygon = ®ion->navmesh_polygons[connectable_edge.polygon_index]; + new_connection.pathway_start = connectable_edge.pathway_start; + new_connection.pathway_end = connectable_edge.pathway_end; - HashMap::Iterator pair_it = connection_pairs_map.find(ek); - if (!pair_it) { - pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair()); - performance_data.pm_edge_count += 1; - ++free_edges_count; + pair.connections[pair.size] = new_connection; + ++pair.size; + if (pair.size == 2) { + --free_edges_count; } - EdgeConnectionPair &pair = pair_it->value; - if (pair.size < 2) { - // Add the polygon/edge tuple to this key. - Edge::Connection new_connection; - new_connection.polygon = &poly; - new_connection.edge = p; - new_connection.pathway_start = poly.vertices[p]; - new_connection.pathway_end = poly.vertices[next_point]; - 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."); - } + } 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."); } } } @@ -160,23 +149,28 @@ void NavMapBuilder2D::_build_step_merge_edge_connection_pairs(NavMapIterationBui PerformanceData &performance_data = r_build.performance_data; HashMap &connection_pairs_map = r_build.iter_connection_pairs_map; - LocalVector &free_edges = r_build.iter_free_edges; + 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; free_edges.clear(); free_edges.reserve(free_edges_count); + NavMapIteration2D *map_iteration = r_build.map_iteration; + + HashMap>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections; + for (const KeyValue &pair_it : connection_pairs_map) { const EdgeConnectionPair &pair = pair_it.value; if (pair.size == 2) { // Connect edge that are shared in different polygons. - const Edge::Connection &c1 = pair.connections[0]; - const Edge::Connection &c2 = pair.connections[1]; - c1.polygon->edges[c1.edge].connections.push_back(c2); - c2.polygon->edges[c2.edge].connections.push_back(c1); - // Note: The pathway_start/end are full for those connection and do not need to be modified. - performance_data.pm_edge_merge_count += 1; + const Connection &c1 = pair.connections[0]; + const Connection &c2 = pair.connections[1]; + + navbases_polygons_external_connections[c1.polygon->owner][c1.polygon->id].push_back(c2); + navbases_polygons_external_connections[c2.polygon->owner][c2.polygon->id].push_back(c1); + performance_data.pm_edge_connection_count += 1; + } else { CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size)); if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) { @@ -191,8 +185,11 @@ void NavMapBuilder2D::_build_step_edge_connection_margin_connections(NavMapItera NavMapIteration2D *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; + + LocalVector &free_edges = r_build.iter_free_edges; + HashMap> ®ion_external_connections = map_iteration->external_region_connections; + + HashMap>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections; // Find the compatible near edges. // @@ -206,23 +203,23 @@ void NavMapBuilder2D::_build_step_edge_connection_margin_connections(NavMapItera const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin; for (uint32_t i = 0; i < free_edges.size(); i++) { - const Edge::Connection &free_edge = free_edges[i]; - Vector2 edge_p1 = free_edge.polygon->vertices[free_edge.edge]; - Vector2 edge_p2 = free_edge.polygon->vertices[(free_edge.edge + 1) % free_edge.polygon->vertices.size()]; + const Connection &free_edge = free_edges[i]; + const Vector2 &edge_p1 = free_edge.pathway_start; + const Vector2 &edge_p2 = free_edge.pathway_end; for (uint32_t j = 0; j < free_edges.size(); j++) { - const Edge::Connection &other_edge = free_edges[j]; + const Connection &other_edge = free_edges[j]; if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) { continue; } - Vector2 other_edge_p1 = other_edge.polygon->vertices[other_edge.edge]; - Vector2 other_edge_p2 = other_edge.polygon->vertices[(other_edge.edge + 1) % other_edge.polygon->vertices.size()]; + const Vector2 &other_edge_p1 = other_edge.pathway_start; + const Vector2 &other_edge_p2 = other_edge.pathway_end; - // Compute the projection of the opposite edge on the current one. + // Compute the projection of the opposite edge on the current one Vector2 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(); + 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; } @@ -251,13 +248,14 @@ void NavMapBuilder2D::_build_step_edge_connection_margin_connections(NavMapItera } // The edges can now be connected. - Edge::Connection new_connection = other_edge; + Connection new_connection = other_edge; new_connection.pathway_start = (self1 + other1) / 2.0; new_connection.pathway_end = (self2 + other2) / 2.0; - free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection); + //free_edge.polygon->connections.push_back(new_connection); // Add the connection to the region_connection map. - region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection); + region_external_connections[free_edge.polygon->owner].push_back(new_connection); + navbases_polygons_external_connections[free_edge.polygon->owner][free_edge.polygon->id].push_back(new_connection); performance_data.pm_edge_connection_count += 1; } } @@ -268,18 +266,28 @@ void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_ real_t link_connection_radius = r_build.link_connection_radius; - LocalVector &links = map_iteration->link_iterations; + const LocalVector> &links = map_iteration->link_iterations; + int polygon_count = r_build.polygon_count; real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius; + HashMap>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections; + LocalVector &navlink_polygons = map_iteration->navlink_polygons; + navlink_polygons.clear(); + navlink_polygons.resize(links.size()); + uint32_t navlink_index = 0; + // Search for polygons within range of a nav link. - for (NavLinkIteration2D &link : links) { - if (!link.get_enabled()) { - continue; - } - const Vector2 link_start_pos = link.get_start_position(); - const Vector2 link_end_pos = link.get_end_position(); + for (const Ref &link : links) { + polygon_count++; + Polygon &new_polygon = navlink_polygons[navlink_index++]; + + new_polygon.id = 0; + new_polygon.owner = link.ptr(); + + const Vector2 link_start_pos = link->get_start_position(); + const Vector2 link_end_pos = link->get_end_position(); Polygon *closest_start_polygon = nullptr; real_t closest_start_sqr_dist = link_connection_radius_sqr; @@ -289,16 +297,13 @@ void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_ real_t closest_end_sqr_dist = link_connection_radius_sqr; Vector2 closest_end_point; - for (NavRegionIteration2D ®ion : map_iteration->region_iterations) { - if (!region.get_enabled()) { - continue; - } - Rect2 region_bounds = region.get_bounds().grow(link_connection_radius); + for (const Ref ®ion : map_iteration->region_iterations) { + Rect2 region_bounds = region->get_bounds().grow(link_connection_radius); if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) { continue; } - for (Polygon &polyon : region.navmesh_polygons) { + for (Polygon &polyon : region->navmesh_polygons) { for (uint32_t point_id = 2; point_id < polyon.vertices.size(); point_id += 1) { const Triangle2 triangle(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]); @@ -331,14 +336,6 @@ void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_ // 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.clear(); - link.navmesh_polygons.resize(1); - Polygon &new_polygon = link.navmesh_polygons[0]; - new_polygon.id = polygon_count++; - new_polygon.owner = &link; - - new_polygon.edges.clear(); - new_polygon.edges.resize(4); new_polygon.vertices.resize(4); // Build a set of vertices that create a thin polygon going from the start to the end point. @@ -349,36 +346,38 @@ void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_ // Setup connections to go forward in the link. { - Edge::Connection entry_connection; + Connection entry_connection; entry_connection.polygon = &new_polygon; entry_connection.edge = -1; entry_connection.pathway_start = new_polygon.vertices[0]; entry_connection.pathway_end = new_polygon.vertices[1]; - closest_start_polygon->edges[0].connections.push_back(entry_connection); + navbases_polygons_external_connections[closest_start_polygon->owner][closest_start_polygon->id].push_back(entry_connection); - Edge::Connection exit_connection; + Connection exit_connection; exit_connection.polygon = closest_end_polygon; exit_connection.edge = -1; exit_connection.pathway_start = new_polygon.vertices[2]; exit_connection.pathway_end = new_polygon.vertices[3]; - new_polygon.edges[2].connections.push_back(exit_connection); + navbases_polygons_external_connections[link.ptr()].push_back(LocalVector()); + navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection); } // If the link is bi-directional, create connections from the end to the start. - if (link.is_bidirectional()) { - Edge::Connection entry_connection; + if (link->is_bidirectional()) { + Connection entry_connection; entry_connection.polygon = &new_polygon; entry_connection.edge = -1; entry_connection.pathway_start = new_polygon.vertices[2]; entry_connection.pathway_end = new_polygon.vertices[3]; - closest_end_polygon->edges[0].connections.push_back(entry_connection); + navbases_polygons_external_connections[closest_end_polygon->owner][closest_end_polygon->id].push_back(entry_connection); - Edge::Connection exit_connection; + Connection exit_connection; exit_connection.polygon = closest_start_polygon; exit_connection.edge = -1; exit_connection.pathway_start = new_polygon.vertices[0]; exit_connection.pathway_end = new_polygon.vertices[1]; - new_polygon.edges[0].connections.push_back(exit_connection); + navbases_polygons_external_connections[link.ptr()].push_back(LocalVector()); + navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection); } } } @@ -391,12 +390,35 @@ void NavMapBuilder2D::_build_update_map_iteration(NavMapIterationBuild2D &r_buil map_iteration->navmesh_polygon_count = r_build.polygon_count; + uint32_t navmesh_polygon_count = r_build.polygon_count; + uint32_t total_polygon_count = navmesh_polygon_count; + map_iteration->path_query_slots_mutex.lock(); for (NavMeshQueries2D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) { 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.traversable_polys.reserve(navmesh_polygon_count * 0.25); p_path_query_slot.path_corridor.clear(); - p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count); + + p_path_query_slot.path_corridor.resize(total_polygon_count); + + p_path_query_slot.poly_to_id.clear(); + p_path_query_slot.poly_to_id.reserve(total_polygon_count); + + int polygon_id = 0; + for (Ref ®ion : map_iteration->region_iterations) { + for (const Polygon &polygon : region->navmesh_polygons) { + p_path_query_slot.poly_to_id[&polygon] = polygon_id; + polygon_id++; + } + } + + for (const Polygon &polygon : map_iteration->navlink_polygons) { + p_path_query_slot.poly_to_id[&polygon] = polygon_id; + polygon_id++; + } + + DEV_ASSERT(p_path_query_slot.path_corridor.size() == p_path_query_slot.poly_to_id.size()); } + map_iteration->path_query_slots_mutex.unlock(); } diff --git a/modules/navigation_2d/2d/nav_map_iteration_2d.h b/modules/navigation_2d/2d/nav_map_iteration_2d.h index 34f93ef1bb2..313374b2152 100644 --- a/modules/navigation_2d/2d/nav_map_iteration_2d.h +++ b/modules/navigation_2d/2d/nav_map_iteration_2d.h @@ -37,9 +37,9 @@ #include "core/math/math_defs.h" #include "core/os/semaphore.h" -struct NavLinkIteration2D; +class NavLinkIteration2D; class NavRegion2D; -struct NavRegionIteration2D; +class NavRegionIteration2D; struct NavMapIteration2D; struct NavMapIterationBuild2D { @@ -52,7 +52,7 @@ struct NavMapIterationBuild2D { int free_edge_count = 0; HashMap iter_connection_pairs_map; - LocalVector iter_free_edges; + LocalVector iter_free_edges; NavMapIteration2D *map_iteration = nullptr; @@ -74,19 +74,33 @@ struct NavMapIteration2D { mutable SafeNumeric users; RWLock rwlock; - LocalVector region_iterations; - LocalVector link_iterations; + LocalVector> region_iterations; + LocalVector> link_iterations; int navmesh_polygon_count = 0; // The edge connections that the map builds on top with the edge connection margin. - HashMap> external_region_connections; + HashMap> external_region_connections; + HashMap>> navbases_polygons_external_connections; - HashMap region_ptr_to_region_id; + LocalVector navlink_polygons; + + HashMap> region_ptr_to_region_iteration; LocalVector path_query_slots; Mutex path_query_slots_mutex; Semaphore path_query_slots_semaphore; + + void clear() { + navmesh_polygon_count = 0; + + region_iterations.clear(); + link_iterations.clear(); + external_region_connections.clear(); + navbases_polygons_external_connections.clear(); + navlink_polygons.clear(); + region_ptr_to_region_iteration.clear(); + } }; class NavMapIterationRead2D { diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp index 92d6eb16f2d..e686482cec8 100644 --- a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp +++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp @@ -234,22 +234,15 @@ void NavMeshQueries2D::_query_task_find_start_end_positions(NavMeshPathQueryTask real_t begin_d = FLT_MAX; real_t end_d = FLT_MAX; - const LocalVector ®ions = p_map_iteration.region_iterations; + const LocalVector> ®ions = p_map_iteration.region_iterations; - for (const NavRegionIteration2D ®ion : regions) { - if (!region.get_enabled()) { - continue; - } - - if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) { - continue; - } - if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) { + for (const Ref ®ion : regions) { + if (!_query_task_is_connection_owner_usable(p_query_task, region.ptr())) { continue; } // Find the initial poly and the end poly on this map. - for (const Polygon &p : region.get_navmesh_polygons()) { + for (const 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; @@ -279,7 +272,47 @@ void NavMeshQueries2D::_query_task_find_start_end_positions(NavMeshPathQueryTask } } -void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task) { +void NavMeshQueries2D::_query_task_search_polygon_connections(NavMeshPathQueryTask2D &p_query_task, const Connection &p_connection, uint32_t p_least_cost_id, const NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector2 &p_end_point) { + const NavBaseIteration2D *connection_owner = p_connection.polygon->owner; + ERR_FAIL_NULL(connection_owner); + const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner); + if (!owner_is_usable) { + return; + } + + Heap + &traversable_polys = p_query_task.path_query_slot->traversable_polys; + LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; + + real_t poly_travel_cost = p_least_cost_poly.poly->owner->get_travel_cost(); + + Vector2 new_entry = Geometry2D::get_closest_point_to_segment(p_least_cost_poly.entry, p_connection.pathway_start, p_connection.pathway_end); + real_t new_traveled_distance = p_least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + p_poly_enter_cost + p_least_cost_poly.traveled_distance; + + // Check if the neighbor polygon has already been processed. + NavigationPoly &neighbor_poly = navigation_polys[p_query_task.path_query_slot->poly_to_id[p_connection.polygon]]; + if (new_traveled_distance < neighbor_poly.traveled_distance) { + // Add the polygon to the heap of polygons to traverse next. + neighbor_poly.back_navigation_poly_id = p_least_cost_id; + neighbor_poly.back_navigation_edge = p_connection.edge; + neighbor_poly.back_navigation_edge_pathway_start = p_connection.pathway_start; + neighbor_poly.back_navigation_edge_pathway_end = p_connection.pathway_end; + neighbor_poly.traveled_distance = new_traveled_distance; + neighbor_poly.distance_to_destination = + new_entry.distance_to(p_end_point) * + connection_owner->get_travel_cost(); + neighbor_poly.entry = new_entry; + + if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) { + traversable_polys.shift(neighbor_poly.traversable_poly_index); + } else { + neighbor_poly.poly = p_connection.polygon; + traversable_polys.push(&neighbor_poly); + } + } +} + +void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) { const Vector2 p_target_position = p_query_task.target_position; const Polygon *begin_poly = p_query_task.begin_polygon; const Polygon *end_poly = p_query_task.end_polygon; @@ -297,15 +330,15 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p } // Initialize the matching navigation polygon. - NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; + NavigationPoly &begin_navigation_poly = navigation_polys[p_query_task.path_query_slot->poly_to_id[begin_poly]]; 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; - begin_navigation_poly.traveled_distance = 0.0; + begin_navigation_poly.traveled_distance = 0.f; // This is an implementation of the A* algorithm. - uint32_t least_cost_id = begin_poly->id; + uint32_t least_cost_id = p_query_task.path_query_slot->poly_to_id[begin_poly]; bool found_route = false; const Polygon *reachable_end = nullptr; @@ -313,49 +346,29 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p bool is_reachable = true; real_t poly_enter_cost = 0.0; + const HashMap>> &navbases_polygons_external_connections = p_map_iteration.navbases_polygons_external_connections; + while (true) { const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; - real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); - // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. - for (const Edge &edge : least_cost_poly.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 Edge::Connection &connection = edge.connections[connection_index]; + const NavBaseIteration2D *least_cost_navbase = least_cost_poly.poly->owner; - const NavBaseIteration2D *connection_owner = connection.polygon->owner; - const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner); - if (!owner_is_usable) { - continue; - } + const uint32_t navbase_local_polygon_id = least_cost_poly.poly->id; + const LocalVector> &navbase_polygons_to_connections = least_cost_poly.poly->owner->get_internal_connections(); - const Vector2 new_entry = Geometry2D::get_closest_point_to_segment(least_cost_poly.entry, connection.pathway_start, connection.pathway_end); - const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance; + if (navbase_polygons_to_connections.size() > 0) { + const LocalVector &polygon_connections = navbase_polygons_to_connections[navbase_local_polygon_id]; - // Check if the neighbor polygon has already been processed. - NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; - if (new_traveled_distance < neighbor_poly.traveled_distance) { - // Add the polygon to the heap of polygons to traverse next. - 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_point) * - connection_owner->get_travel_cost(); - neighbor_poly.entry = new_entry; - - if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) { - traversable_polys.shift(neighbor_poly.traversable_poly_index); - } else { - neighbor_poly.poly = connection.polygon; - traversable_polys.push(&neighbor_poly); - } - } + for (const Connection &connection : polygon_connections) { + _query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point); } } + // Search region external navmesh polygon connections, aka connections to other regions created by outline edge merge or links. + for (const Connection &connection : navbases_polygons_external_connections[least_cost_navbase][navbase_local_polygon_id]) { + _query_task_search_polygon_connections(p_query_task, connection, least_cost_id, least_cost_poly, poly_enter_cost, end_point); + } + poly_enter_cost = 0; // When the heap of traversable polygons is empty at this point it means the end polygon is // unreachable. @@ -371,6 +384,7 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p // Set as end point the furthest reachable point. end_poly = reachable_end; real_t end_d = FLT_MAX; + for (uint32_t point_id = 2; point_id < end_poly->vertices.size(); point_id++) { Triangle2 t(end_poly->vertices[0], end_poly->vertices[point_id - 1], end_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); @@ -383,6 +397,7 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p // Search all faces of start polygon as well. bool closest_point_on_start_poly = false; + for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) { Triangle2 t(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); @@ -408,13 +423,14 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p nav_poly.poly = nullptr; nav_poly.traveled_distance = FLT_MAX; } - navigation_polys[begin_poly->id].poly = begin_poly; - navigation_polys[begin_poly->id].traveled_distance = 0; - least_cost_id = begin_poly->id; + uint32_t _bp_id = p_query_task.path_query_slot->poly_to_id[begin_poly]; + navigation_polys[_bp_id].poly = begin_poly; + navigation_polys[_bp_id].traveled_distance = 0; + least_cost_id = _bp_id; reachable_end = nullptr; } else { // Pop the polygon with the lowest travel cost from the heap of traversable polygons. - least_cost_id = traversable_polys.pop()->poly->id; + least_cost_id = p_query_task.path_query_slot->poly_to_id[traversable_polys.pop()->poly]; // Store the farthest reachable end polygon in case our goal is not reachable. if (is_reachable) { @@ -432,6 +448,7 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p } if (navigation_polys[least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { + ERR_FAIL_NULL(least_cost_poly.poly->owner); poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); } } @@ -442,6 +459,7 @@ void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &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 (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) { Triangle2 t(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); @@ -484,7 +502,7 @@ void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D return; } - _query_task_build_path_corridor(p_query_task); + _query_task_build_path_corridor(p_query_task, p_map_iteration); if (p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FAILED) { return; @@ -733,9 +751,9 @@ ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(c // TODO: Check for further 2D improvements. - const LocalVector ®ions = p_map_iteration.region_iterations; - for (const NavRegionIteration2D ®ion : regions) { - for (const Polygon &polygon : region.get_navmesh_polygons()) { + const LocalVector> ®ions = p_map_iteration.region_iterations; + for (const Ref ®ion : regions) { + for (const Polygon &polygon : region->get_navmesh_polygons()) { real_t cross = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); Vector2 closest_on_polygon; real_t closest = FLT_MAX; @@ -803,8 +821,8 @@ Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D accessible_regions.reserve(p_map_iteration.region_iterations.size()); for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) { - const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[i]; - if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) { + const Ref ®ion = p_map_iteration.region_iterations[i]; + if (!region->get_enabled() || (p_navigation_layers & region->get_navigation_layers()) == 0) { continue; } accessible_regions.push_back(i); @@ -820,9 +838,9 @@ Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D RBMap accessible_regions_area_map; for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) { - const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]]; + const Ref ®ion = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]]; - real_t region_surface_area = region.surface_area; + real_t region_surface_area = region->surface_area; if (region_surface_area == 0.0f) { continue; @@ -843,16 +861,16 @@ Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D uint32_t random_region_index = E->value; ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector2()); - const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; + const Ref &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; - return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); + return NavMeshQueries2D::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 NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; + const Ref &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; - return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); + return NavMeshQueries2D::polygons_get_random_point(random_region->navmesh_polygons, p_navigation_layers, p_uniformly); } } @@ -978,8 +996,15 @@ void NavMeshQueries2D::_query_task_clip_path(NavMeshPathQueryTask2D &p_query_tas } bool NavMeshQueries2D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner) { + ERR_FAIL_NULL_V(p_owner, false); + bool owner_usable = true; + if (!p_owner->get_enabled()) { + owner_usable = false; + return owner_usable; + } + if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) { // Not usable. No matching bit between task filter bitmask and owner bitmask. owner_usable = false; diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.h b/modules/navigation_2d/2d/nav_mesh_queries_2d.h index 54e22083196..2bfa4471c07 100644 --- a/modules/navigation_2d/2d/nav_mesh_queries_2d.h +++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.h @@ -32,6 +32,8 @@ #include "../nav_utils_2d.h" +#include "core/templates/a_hash_map.h" + #include "servers/navigation/navigation_path_query_parameters_2d.h" #include "servers/navigation/navigation_path_query_result_2d.h" #include "servers/navigation/navigation_utilities.h" @@ -48,6 +50,7 @@ public: Heap traversable_polys; bool in_use = false; uint32_t slot_index = 0; + AHashMap poly_to_id; }; struct NavMeshPathQueryTask2D { @@ -128,7 +131,7 @@ public: static void query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration); static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const Nav2D::Polygon *p_point_polygon); static void _query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration); - static void _query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task); + static void _query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration); static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task); static void _query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task); static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task); @@ -136,6 +139,8 @@ public: static void _query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task); static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner); + static void _query_task_search_polygon_connections(NavMeshPathQueryTask2D &p_query_task, const Nav2D::Connection &p_connection, uint32_t p_least_cost_id, const Nav2D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector2 &p_end_point); + static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices); static LocalVector get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon); }; diff --git a/modules/navigation_2d/2d/nav_region_builder_2d.cpp b/modules/navigation_2d/2d/nav_region_builder_2d.cpp new file mode 100644 index 00000000000..53bb025dd98 --- /dev/null +++ b/modules/navigation_2d/2d/nav_region_builder_2d.cpp @@ -0,0 +1,255 @@ +/**************************************************************************/ +/* nav_region_builder_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "nav_region_builder_2d.h" + +#include "../nav_map_2d.h" +#include "../nav_region_2d.h" +#include "../triangle2.h" +#include "nav_region_iteration_2d.h" + +using namespace Nav2D; + +void NavRegionBuilder2D::build_iteration(NavRegionIterationBuild2D &r_build) { + 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_process_navmesh_data(r_build); + + _build_step_find_edge_connection_pairs(r_build); + + _build_step_merge_edge_connection_pairs(r_build); + + _build_update_iteration(r_build); +} + +void NavRegionBuilder2D::_build_step_process_navmesh_data(NavRegionIterationBuild2D &r_build) { + Vector _navmesh_vertices = r_build.navmesh_data.vertices; + Vector> _navmesh_polygons = r_build.navmesh_data.polygons; + + if (_navmesh_vertices.is_empty() || _navmesh_polygons.is_empty()) { + return; + } + + PerformanceData &performance_data = r_build.performance_data; + Ref region_iteration = r_build.region_iteration; + + const Transform2D ®ion_transform = region_iteration->transform; + LocalVector &navmesh_polygons = region_iteration->navmesh_polygons; + + const int vertex_count = _navmesh_vertices.size(); + + const Vector2 *vertices_ptr = _navmesh_vertices.ptr(); + const Vector *polygons_ptr = _navmesh_polygons.ptr(); + + navmesh_polygons.resize(_navmesh_polygons.size()); + + real_t _new_region_surface_area = 0.0; + Rect2 _new_region_bounds; + + bool first_vertex = true; + + for (uint32_t i = 0; i < navmesh_polygons.size(); i++) { + Polygon &polygon = navmesh_polygons[i]; + polygon.id = i; + polygon.owner = region_iteration.ptr(); + polygon.surface_area = 0.0; + + Vector polygon_indices = polygons_ptr[i]; + + int polygon_size = polygon_indices.size(); + if (polygon_size < 3) { + continue; + } + + const int *indices_ptr = polygon_indices.ptr(); + + bool polygon_valid = true; + + polygon.vertices.resize(polygon_size); + + { + real_t _new_polygon_surface_area = 0.0; + + for (int j(2); j < polygon_size; j++) { + const Triangle2 triangle = Triangle2( + region_transform.xform(vertices_ptr[indices_ptr[0]]), + region_transform.xform(vertices_ptr[indices_ptr[j - 1]]), + region_transform.xform(vertices_ptr[indices_ptr[j]])); + + _new_polygon_surface_area += triangle.get_area(); + } + + polygon.surface_area = _new_polygon_surface_area; + _new_region_surface_area += _new_polygon_surface_area; + } + + for (int j(0); j < polygon_size; j++) { + int vertex_index = indices_ptr[j]; + if (vertex_index < 0 || vertex_index >= vertex_count) { + polygon_valid = false; + break; + } + + const Vector2 point_position = region_transform.xform(vertices_ptr[vertex_index]); + polygon.vertices[j] = point_position; + + if (first_vertex) { + first_vertex = false; + _new_region_bounds.position = point_position; + } else { + _new_region_bounds.expand_to(point_position); + } + } + + if (!polygon_valid) { + polygon.surface_area = 0.0; + polygon.vertices.clear(); + ERR_FAIL_COND_MSG(!polygon_valid, "Corrupted navigation mesh set on region. The indices of a polygon are out of range."); + } + } + + region_iteration->surface_area = _new_region_surface_area; + region_iteration->bounds = _new_region_bounds; + + performance_data.pm_polygon_count = navmesh_polygons.size(); +} + +Nav2D::PointKey NavRegionBuilder2D::get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size) { + const int x = static_cast(Math::floor(p_pos.x / p_cell_size.x)); + const int y = static_cast(Math::floor(p_pos.y / p_cell_size.y)); + + PointKey p; + p.key = 0; + p.x = x; + p.y = y; + return p; +} + +Nav2D::EdgeKey NavRegionBuilder2D::get_edge_key(const Vector2 &p_vertex1, const Vector2 &p_vertex2, const Vector2 &p_cell_size) { + EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size)); + return ek; +} + +void NavRegionBuilder2D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild2D &r_build) { + PerformanceData &performance_data = r_build.performance_data; + + const Vector2 &map_cell_size = r_build.map_cell_size; + Ref region_iteration = r_build.region_iteration; + LocalVector &navmesh_polygons = region_iteration->navmesh_polygons; + + HashMap &connection_pairs_map = r_build.iter_connection_pairs_map; + connection_pairs_map.clear(); + + region_iteration->internal_connections.clear(); + region_iteration->internal_connections.resize(navmesh_polygons.size()); + + region_iteration->external_edges.clear(); + + int free_edges_count = 0; + + for (Polygon &poly : region_iteration->navmesh_polygons) { + for (uint32_t p = 0; p < poly.vertices.size(); p++) { + const int next_point = (p + 1) % poly.vertices.size(); + const EdgeKey ek = get_edge_key(poly.vertices[p], poly.vertices[next_point], map_cell_size); + + HashMap::Iterator pair_it = connection_pairs_map.find(ek); + if (!pair_it) { + pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair()); + performance_data.pm_edge_count += 1; + ++free_edges_count; + } + EdgeConnectionPair &pair = pair_it->value; + if (pair.size < 2) { + // Add the polygon/edge tuple to this key. + Connection new_connection; + new_connection.polygon = &poly; + new_connection.edge = p; + new_connection.pathway_start = poly.vertices[p]; + new_connection.pathway_end = poly.vertices[next_point]; + + 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_FAIL_COND_MSG(pair.size >= 2, "Navigation region synchronization error. More than 2 edges tried to occupy the same map rasterization space. This is a logical error in the navigation mesh caused by overlap or too densely placed edges."); + } + } + } + + performance_data.pm_edge_free_count = free_edges_count; +} + +void NavRegionBuilder2D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild2D &r_build) { + PerformanceData &performance_data = r_build.performance_data; + + Ref region_iteration = r_build.region_iteration; + + HashMap &connection_pairs_map = r_build.iter_connection_pairs_map; + + for (const KeyValue &pair_it : connection_pairs_map) { + const EdgeConnectionPair &pair = pair_it.value; + if (pair.size == 2) { + // Connect edge that are shared in different polygons. + const Connection &c1 = pair.connections[0]; + const Connection &c2 = pair.connections[1]; + region_iteration->internal_connections[c1.polygon->id].push_back(c2); + region_iteration->internal_connections[c2.polygon->id].push_back(c1); + performance_data.pm_edge_merge_count += 1; + + } else { + ERR_FAIL_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size)); + + const Connection &connection = pair.connections[0]; + + ConnectableEdge ce; + ce.ek = pair_it.key; + ce.polygon_index = connection.polygon->id; + ce.pathway_start = connection.pathway_start; + ce.pathway_end = connection.pathway_end; + + region_iteration->external_edges.push_back(ce); + } + } +} + +void NavRegionBuilder2D::_build_update_iteration(NavRegionIterationBuild2D &r_build) { + ERR_FAIL_NULL(r_build.region); + // Stub. End of the build. +} diff --git a/modules/navigation_2d/2d/nav_region_builder_2d.h b/modules/navigation_2d/2d/nav_region_builder_2d.h new file mode 100644 index 00000000000..a5583cc9773 --- /dev/null +++ b/modules/navigation_2d/2d/nav_region_builder_2d.h @@ -0,0 +1,48 @@ +/**************************************************************************/ +/* nav_region_builder_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#pragma once + +#include "../nav_utils_2d.h" + +struct NavRegionIterationBuild2D; + +class NavRegionBuilder2D { + static void _build_step_process_navmesh_data(NavRegionIterationBuild2D &r_build); + static void _build_step_find_edge_connection_pairs(NavRegionIterationBuild2D &r_build); + static void _build_step_merge_edge_connection_pairs(NavRegionIterationBuild2D &r_build); + static void _build_update_iteration(NavRegionIterationBuild2D &r_build); + +public: + static Nav2D::PointKey get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size); + static Nav2D::EdgeKey get_edge_key(const Vector2 &p_vertex1, const Vector2 &p_vertex2, const Vector2 &p_cell_size); + + static void build_iteration(NavRegionIterationBuild2D &r_build); +}; diff --git a/modules/navigation_2d/2d/nav_region_iteration_2d.h b/modules/navigation_2d/2d/nav_region_iteration_2d.h index 4d50b368566..329f4a673cb 100644 --- a/modules/navigation_2d/2d/nav_region_iteration_2d.h +++ b/modules/navigation_2d/2d/nav_region_iteration_2d.h @@ -32,15 +32,61 @@ #include "../nav_utils_2d.h" #include "nav_base_iteration_2d.h" +#include "scene/resources/2d/navigation_polygon.h" #include "core/math/rect2.h" -struct NavRegionIteration2D : NavBaseIteration2D { +class NavRegion2D; +class NavRegionIteration2D; + +struct NavRegionIterationBuild2D { + Nav2D::PerformanceData performance_data; + + NavRegion2D *region = nullptr; + + Vector2 map_cell_size; + Transform2D region_transform; + + struct NavMeshData { + Vector vertices; + Vector> polygons; + + void clear() { + vertices.clear(); + polygons.clear(); + } + } navmesh_data; + + Ref region_iteration; + + HashMap iter_connection_pairs_map; + + void reset() { + performance_data.reset(); + + navmesh_data.clear(); + region_iteration = Ref(); + iter_connection_pairs_map.clear(); + } +}; + +class NavRegionIteration2D : public NavBaseIteration2D { + GDCLASS(NavRegionIteration2D, NavBaseIteration2D); + +public: Transform2D transform; real_t surface_area = 0.0; Rect2 bounds; + LocalVector external_edges; const Transform2D &get_transform() const { return transform; } real_t get_surface_area() const { return surface_area; } Rect2 get_bounds() const { return bounds; } + const LocalVector &get_external_edges() const { return external_edges; } + + virtual ~NavRegionIteration2D() override { + external_edges.clear(); + navmesh_polygons.clear(); + internal_connections.clear(); + } }; diff --git a/modules/navigation_2d/nav_link_2d.cpp b/modules/navigation_2d/nav_link_2d.cpp index fcf4afeccbd..d4baa339b40 100644 --- a/modules/navigation_2d/nav_link_2d.cpp +++ b/modules/navigation_2d/nav_link_2d.cpp @@ -44,7 +44,7 @@ void NavLink2D::set_map(NavMap2D *p_map) { } map = p_map; - link_dirty = true; + iteration_dirty = true; if (map) { map->add_link(this); @@ -57,9 +57,7 @@ void NavLink2D::set_enabled(bool p_enabled) { return; } enabled = p_enabled; - - // TODO: This should not require a full rebuild as the link has not really changed. - link_dirty = true; + iteration_dirty = true; request_sync(); } @@ -69,27 +67,27 @@ void NavLink2D::set_bidirectional(bool p_bidirectional) { return; } bidirectional = p_bidirectional; - link_dirty = true; + iteration_dirty = true; request_sync(); } -void NavLink2D::set_start_position(const Vector2 &p_position) { +void NavLink2D::set_start_position(const Vector2 p_position) { if (start_position == p_position) { return; } start_position = p_position; - link_dirty = true; + iteration_dirty = true; request_sync(); } -void NavLink2D::set_end_position(const Vector2 &p_position) { +void NavLink2D::set_end_position(const Vector2 p_position) { if (end_position == p_position) { return; } end_position = p_position; - link_dirty = true; + iteration_dirty = true; request_sync(); } @@ -99,7 +97,7 @@ void NavLink2D::set_navigation_layers(uint32_t p_navigation_layers) { return; } navigation_layers = p_navigation_layers; - link_dirty = true; + iteration_dirty = true; request_sync(); } @@ -110,7 +108,7 @@ void NavLink2D::set_enter_cost(real_t p_enter_cost) { return; } enter_cost = new_enter_cost; - link_dirty = true; + iteration_dirty = true; request_sync(); } @@ -121,7 +119,7 @@ void NavLink2D::set_travel_cost(real_t p_travel_cost) { return; } travel_cost = new_travel_cost; - link_dirty = true; + iteration_dirty = true; request_sync(); } @@ -131,21 +129,59 @@ void NavLink2D::set_owner_id(ObjectID p_owner_id) { return; } owner_id = p_owner_id; - link_dirty = true; + iteration_dirty = true; request_sync(); } -bool NavLink2D::is_dirty() const { - return link_dirty; -} - -void NavLink2D::sync() { - if (link_dirty) { - iteration_id = iteration_id % UINT32_MAX + 1; +bool NavLink2D::sync() { + bool requires_map_update = false; + if (!map) { + return requires_map_update; } - link_dirty = false; + if (iteration_dirty && !iteration_building && !iteration_ready) { + _build_iteration(); + iteration_ready = false; + requires_map_update = true; + } + + return requires_map_update; +} + +void NavLink2D::_build_iteration() { + if (!iteration_dirty || iteration_building || iteration_ready) { + return; + } + + iteration_dirty = false; + iteration_building = true; + iteration_ready = false; + + Ref new_iteration; + new_iteration.instantiate(); + + new_iteration->navigation_layers = get_navigation_layers(); + new_iteration->enter_cost = get_enter_cost(); + new_iteration->travel_cost = get_travel_cost(); + new_iteration->owner_object_id = get_owner_id(); + new_iteration->owner_type = get_type(); + new_iteration->owner_rid = get_self(); + + new_iteration->enabled = get_enabled(); + new_iteration->start_position = get_start_position(); + new_iteration->end_position = get_end_position(); + new_iteration->bidirectional = is_bidirectional(); + + RWLockWrite write_lock(iteration_rwlock); + ERR_FAIL_COND(iteration.is_null()); + iteration = Ref(); + DEV_ASSERT(iteration.is_null()); + iteration = new_iteration; + iteration_id = iteration_id % UINT32_MAX + 1; + + iteration_building = false; + iteration_ready = true; } void NavLink2D::request_sync() { @@ -160,27 +196,19 @@ void NavLink2D::cancel_sync_request() { } } +Ref NavLink2D::get_iteration() { + RWLockRead read_lock(iteration_rwlock); + return iteration; +} + NavLink2D::NavLink2D() : sync_dirty_request_list_element(this) { type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK; + iteration.instantiate(); } NavLink2D::~NavLink2D() { cancel_sync_request(); -} - -void NavLink2D::get_iteration_update(NavLinkIteration2D &r_iteration) { - r_iteration.navigation_layers = get_navigation_layers(); - r_iteration.enter_cost = get_enter_cost(); - r_iteration.travel_cost = get_travel_cost(); - r_iteration.owner_object_id = get_owner_id(); - r_iteration.owner_type = get_type(); - r_iteration.owner_rid = get_self(); - - r_iteration.enabled = get_enabled(); - r_iteration.start_position = get_start_position(); - r_iteration.end_position = get_end_position(); - r_iteration.bidirectional = is_bidirectional(); - - r_iteration.navmesh_polygons.clear(); + + iteration = Ref(); } diff --git a/modules/navigation_2d/nav_link_2d.h b/modules/navigation_2d/nav_link_2d.h index 954308c825d..91d0f208e58 100644 --- a/modules/navigation_2d/nav_link_2d.h +++ b/modules/navigation_2d/nav_link_2d.h @@ -34,7 +34,10 @@ #include "nav_base_2d.h" #include "nav_utils_2d.h" -struct NavLinkIteration2D : NavBaseIteration2D { +class NavLinkIteration2D : public NavBaseIteration2D { + GDCLASS(NavLinkIteration2D, NavBaseIteration2D); + +public: bool bidirectional = true; Vector2 start_position; Vector2 end_position; @@ -42,6 +45,11 @@ struct NavLinkIteration2D : NavBaseIteration2D { Vector2 get_start_position() const { return start_position; } Vector2 get_end_position() const { return end_position; } bool is_bidirectional() const { return bidirectional; } + + virtual ~NavLinkIteration2D() override { + navmesh_polygons.clear(); + internal_connections.clear(); + } }; #include "core/templates/self_list.h" @@ -53,12 +61,20 @@ class NavLink2D : public NavBase2D { Vector2 end_position; bool enabled = true; - bool link_dirty = true; - SelfList sync_dirty_request_list_element; uint32_t iteration_id = 0; + mutable RWLock iteration_rwlock; + Ref iteration; + + bool iteration_dirty = true; + bool iteration_building = false; + bool iteration_ready = false; + + void _build_iteration(); + void _sync_iteration(); + public: NavLink2D(); ~NavLink2D(); @@ -78,12 +94,12 @@ public: return bidirectional; } - void set_start_position(const Vector2 &p_position); + void set_start_position(Vector2 p_position); Vector2 get_start_position() const { return start_position; } - void set_end_position(const Vector2 &p_position); + void set_end_position(Vector2 p_position); Vector2 get_end_position() const { return end_position; } @@ -94,10 +110,9 @@ public: virtual void set_travel_cost(real_t p_travel_cost) override; virtual void set_owner_id(ObjectID p_owner_id) override; - bool is_dirty() const; - void sync(); + bool sync(); void request_sync(); void cancel_sync_request(); - void get_iteration_update(NavLinkIteration2D &r_iteration); + Ref get_iteration(); }; diff --git a/modules/navigation_2d/nav_map_2d.cpp b/modules/navigation_2d/nav_map_2d.cpp index 7de7ab48ca6..259b70dd9da 100644 --- a/modules/navigation_2d/nav_map_2d.cpp +++ b/modules/navigation_2d/nav_map_2d.cpp @@ -109,7 +109,7 @@ void NavMap2D::set_link_connection_radius(real_t p_link_connection_radius) { iteration_dirty = true; } -Vector2 NavMap2D::get_merge_rasterizer_cell_size() const { +const Vector2 &NavMap2D::get_merge_rasterizer_cell_size() const { return merge_rasterizer_cell_size; } @@ -188,6 +188,8 @@ ClosestPointQueryResult NavMap2D::get_closest_point_info(const Vector2 &p_point) } void NavMap2D::add_region(NavRegion2D *p_region) { + DEV_ASSERT(!regions.has(p_region)); + regions.push_back(p_region); iteration_dirty = true; } @@ -199,6 +201,8 @@ void NavMap2D::remove_region(NavRegion2D *p_region) { } void NavMap2D::add_link(NavLink2D *p_link) { + DEV_ASSERT(!links.has(p_link)); + links.push_back(p_link); iteration_dirty = true; } @@ -311,49 +315,22 @@ void NavMap2D::_build_iteration() { iteration_build.edge_connection_margin = get_edge_connection_margin(); iteration_build.link_connection_radius = get_link_connection_radius(); - uint32_t enabled_region_count = 0; - uint32_t enabled_link_count = 0; + next_map_iteration.clear(); - for (NavRegion2D *region : regions) { - if (!region->get_enabled()) { - continue; - } - enabled_region_count++; - } - for (NavLink2D *link : links) { - if (!link->get_enabled()) { - continue; - } - enabled_link_count++; - } - - next_map_iteration.region_ptr_to_region_id.clear(); - - next_map_iteration.region_iterations.clear(); - next_map_iteration.link_iterations.clear(); - - next_map_iteration.region_iterations.resize(enabled_region_count); - next_map_iteration.link_iterations.resize(enabled_link_count); + next_map_iteration.region_iterations.resize(regions.size()); + next_map_iteration.link_iterations.resize(links.size()); uint32_t region_id_count = 0; uint32_t link_id_count = 0; for (NavRegion2D *region : regions) { - if (!region->get_enabled()) { - continue; - } - NavRegionIteration2D ®ion_iteration = next_map_iteration.region_iterations[region_id_count]; - region_iteration.id = region_id_count++; - region->get_iteration_update(region_iteration); - next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id; + const Ref region_iteration = region->get_iteration(); + next_map_iteration.region_iterations[region_id_count++] = region_iteration; + next_map_iteration.region_ptr_to_region_iteration[region] = region_iteration; } for (NavLink2D *link : links) { - if (!link->get_enabled()) { - continue; - } - NavLinkIteration2D &link_iteration = next_map_iteration.link_iterations[link_id_count]; - link_iteration.id = link_id_count++; - link->get_iteration_update(link_iteration); + const Ref link_iteration = link->get_iteration(); + next_map_iteration.link_iterations[link_id_count++] = link_iteration; } iteration_build.map_iteration = &next_map_iteration; @@ -379,9 +356,6 @@ void NavMap2D::_sync_iteration() { return; } - performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count; - performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count; - performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count; performance_data.pm_edge_connection_count = iteration_build.performance_data.pm_edge_connection_count; performance_data.pm_edge_free_count = iteration_build.performance_data.pm_edge_free_count; @@ -403,6 +377,8 @@ void NavMap2D::sync() { performance_data.pm_link_count = links.size(); performance_data.pm_obstacle_count = obstacles.size(); + _sync_async_tasks(); + _sync_dirty_map_update_requests(); if (iteration_dirty && !iteration_building && !iteration_ready) { @@ -426,6 +402,16 @@ void NavMap2D::sync() { map_settings_dirty = false; _sync_avoidance(); + + performance_data.pm_polygon_count = 0; + performance_data.pm_edge_count = 0; + performance_data.pm_edge_merge_count = 0; + + for (NavRegion2D *region : regions) { + performance_data.pm_polygon_count += region->get_pm_polygon_count(); + performance_data.pm_edge_count += region->get_pm_edge_count(); + performance_data.pm_edge_merge_count += region->get_pm_edge_merge_count(); + } } void NavMap2D::_sync_avoidance() { @@ -569,9 +555,9 @@ int NavMap2D::get_region_connections_count(NavRegion2D *p_region) const { GET_MAP_ITERATION_CONST(); - HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + HashMap>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region); if (found_id) { - HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr()); if (found_connections) { return found_connections->value.size(); } @@ -585,9 +571,9 @@ Vector2 NavMap2D::get_region_connection_pathway_start(NavRegion2D *p_region, int GET_MAP_ITERATION_CONST(); - HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + HashMap>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region); if (found_id) { - HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr()); if (found_connections) { ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2()); return found_connections->value[p_connection_id].pathway_start; @@ -602,9 +588,9 @@ Vector2 NavMap2D::get_region_connection_pathway_end(NavRegion2D *p_region, int p GET_MAP_ITERATION_CONST(); - HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + HashMap>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region); if (found_id) { - HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr()); if (found_connections) { ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2()); return found_connections->value[p_connection_id].pathway_end; @@ -618,56 +604,60 @@ void NavMap2D::add_region_sync_dirty_request(SelfList *p_sync_reque if (p_sync_request->in_list()) { return; } - sync_dirty_requests.regions.add(p_sync_request); + RWLockWrite write_lock(sync_dirty_requests.regions.rwlock); + sync_dirty_requests.regions.list.add(p_sync_request); } void NavMap2D::add_link_sync_dirty_request(SelfList *p_sync_request) { if (p_sync_request->in_list()) { return; } - sync_dirty_requests.links.add(p_sync_request); + RWLockWrite write_lock(sync_dirty_requests.links.rwlock); + sync_dirty_requests.links.list.add(p_sync_request); } void NavMap2D::add_agent_sync_dirty_request(SelfList *p_sync_request) { if (p_sync_request->in_list()) { return; } - sync_dirty_requests.agents.add(p_sync_request); + sync_dirty_requests.agents.list.add(p_sync_request); } void NavMap2D::add_obstacle_sync_dirty_request(SelfList *p_sync_request) { if (p_sync_request->in_list()) { return; } - sync_dirty_requests.obstacles.add(p_sync_request); + sync_dirty_requests.obstacles.list.add(p_sync_request); } void NavMap2D::remove_region_sync_dirty_request(SelfList *p_sync_request) { if (!p_sync_request->in_list()) { return; } - sync_dirty_requests.regions.remove(p_sync_request); + RWLockWrite write_lock(sync_dirty_requests.regions.rwlock); + sync_dirty_requests.regions.list.remove(p_sync_request); } void NavMap2D::remove_link_sync_dirty_request(SelfList *p_sync_request) { if (!p_sync_request->in_list()) { return; } - sync_dirty_requests.links.remove(p_sync_request); + RWLockWrite write_lock(sync_dirty_requests.links.rwlock); + sync_dirty_requests.links.list.remove(p_sync_request); } void NavMap2D::remove_agent_sync_dirty_request(SelfList *p_sync_request) { if (!p_sync_request->in_list()) { return; } - sync_dirty_requests.agents.remove(p_sync_request); + sync_dirty_requests.agents.list.remove(p_sync_request); } void NavMap2D::remove_obstacle_sync_dirty_request(SelfList *p_sync_request) { if (!p_sync_request->in_list()) { return; } - sync_dirty_requests.obstacles.remove(p_sync_request); + sync_dirty_requests.obstacles.list.remove(p_sync_request); } void NavMap2D::_sync_dirty_map_update_requests() { @@ -679,41 +669,69 @@ void NavMap2D::_sync_dirty_map_update_requests() { iteration_dirty = true; } - if (!iteration_dirty) { - iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first(); - } - // Sync NavRegions. - for (SelfList *element = sync_dirty_requests.regions.first(); element; element = element->next()) { - element->self()->sync(); + RWLockWrite write_lock_regions(sync_dirty_requests.regions.rwlock); + for (SelfList *element = sync_dirty_requests.regions.list.first(); element; element = element->next()) { + bool requires_map_update = element->self()->sync(); + if (requires_map_update) { + iteration_dirty = true; + } } - sync_dirty_requests.regions.clear(); + sync_dirty_requests.regions.list.clear(); // Sync NavLinks. - for (SelfList *element = sync_dirty_requests.links.first(); element; element = element->next()) { - element->self()->sync(); + RWLockWrite write_lock_links(sync_dirty_requests.links.rwlock); + for (SelfList *element = sync_dirty_requests.links.list.first(); element; element = element->next()) { + bool requires_map_update = element->self()->sync(); + if (requires_map_update) { + iteration_dirty = true; + } } - sync_dirty_requests.links.clear(); + sync_dirty_requests.links.list.clear(); } void NavMap2D::_sync_dirty_avoidance_update_requests() { // Sync NavAgents. if (!agents_dirty) { - agents_dirty = sync_dirty_requests.agents.first(); + agents_dirty = sync_dirty_requests.agents.list.first(); } - for (SelfList *element = sync_dirty_requests.agents.first(); element; element = element->next()) { + for (SelfList *element = sync_dirty_requests.agents.list.first(); element; element = element->next()) { element->self()->sync(); } - sync_dirty_requests.agents.clear(); + sync_dirty_requests.agents.list.clear(); // Sync NavObstacles. if (!obstacles_dirty) { - obstacles_dirty = sync_dirty_requests.obstacles.first(); + obstacles_dirty = sync_dirty_requests.obstacles.list.first(); } - for (SelfList *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) { + for (SelfList *element = sync_dirty_requests.obstacles.list.first(); element; element = element->next()) { element->self()->sync(); } - sync_dirty_requests.obstacles.clear(); + sync_dirty_requests.obstacles.list.clear(); +} + +void NavMap2D::add_region_async_thread_join_request(SelfList *p_async_request) { + if (p_async_request->in_list()) { + return; + } + RWLockWrite write_lock(async_dirty_requests.regions.rwlock); + async_dirty_requests.regions.list.add(p_async_request); +} + +void NavMap2D::remove_region_async_thread_join_request(SelfList *p_async_request) { + if (!p_async_request->in_list()) { + return; + } + RWLockWrite write_lock(async_dirty_requests.regions.rwlock); + async_dirty_requests.regions.list.remove(p_async_request); +} + +void NavMap2D::_sync_async_tasks() { + // Sync NavRegions that run async thread tasks. + RWLockWrite write_lock_regions(async_dirty_requests.regions.rwlock); + for (SelfList *element = async_dirty_requests.regions.list.first(); element; element = element->next()) { + element->self()->sync_async_tasks(); + } } void NavMap2D::set_use_async_iterations(bool p_enabled) { @@ -768,4 +786,9 @@ NavMap2D::~NavMap2D() { WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id); iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; } + + RWLockWrite write_lock(iteration_slot_rwlock); + for (NavMapIteration2D &iteration_slot : iteration_slots) { + iteration_slot.clear(); + } } diff --git a/modules/navigation_2d/nav_map_2d.h b/modules/navigation_2d/nav_map_2d.h index 20d7d007bdf..e39958226f2 100644 --- a/modules/navigation_2d/nav_map_2d.h +++ b/modules/navigation_2d/nav_map_2d.h @@ -102,12 +102,31 @@ class NavMap2D : public NavRid2D { Nav2D::PerformanceData performance_data; struct { - SelfList::List regions; - SelfList::List links; - SelfList::List agents; - SelfList::List obstacles; + struct { + RWLock rwlock; + SelfList::List list; + } regions; + struct { + RWLock rwlock; + SelfList::List list; + } links; + struct { + RWLock rwlock; + SelfList::List list; + } agents; + struct { + RWLock rwlock; + SelfList::List list; + } obstacles; } sync_dirty_requests; + struct { + struct { + RWLock rwlock; + SelfList::List list; + } regions; + } async_dirty_requests; + int path_query_slots_max = 4; bool use_async_iterations = true; @@ -117,7 +136,6 @@ class NavMap2D : public NavRid2D { mutable RWLock iteration_slot_rwlock; NavMapIterationBuild2D iteration_build; - bool iteration_build_use_threads = false; WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; static void _build_iteration_threaded(void *p_arg); @@ -160,7 +178,7 @@ public: } Nav2D::PointKey get_point_key(const Vector2 &p_pos) const; - Vector2 get_merge_rasterizer_cell_size() const; + const Vector2 &get_merge_rasterizer_cell_size() const; void query_path(NavMeshQueries2D::NavMeshPathQueryTask2D &p_query_task); @@ -218,6 +236,9 @@ public: Vector2 get_region_connection_pathway_start(NavRegion2D *p_region, int p_connection_id) const; Vector2 get_region_connection_pathway_end(NavRegion2D *p_region, int p_connection_id) const; + void add_region_async_thread_join_request(SelfList *p_async_request); + void remove_region_async_thread_join_request(SelfList *p_async_request); + void add_region_sync_dirty_request(SelfList *p_sync_request); void add_link_sync_dirty_request(SelfList *p_sync_request); void add_agent_sync_dirty_request(SelfList *p_sync_request); @@ -234,6 +255,7 @@ public: private: void _sync_dirty_map_update_requests(); void _sync_dirty_avoidance_update_requests(); + void _sync_async_tasks(); void compute_single_step(uint32_t p_index, NavAgent2D **p_agent); diff --git a/modules/navigation_2d/nav_region_2d.cpp b/modules/navigation_2d/nav_region_2d.cpp index 02408b9af5f..2be4785cd6c 100644 --- a/modules/navigation_2d/nav_region_2d.cpp +++ b/modules/navigation_2d/nav_region_2d.cpp @@ -31,11 +31,11 @@ #include "nav_region_2d.h" #include "nav_map_2d.h" -#include "triangle2.h" -#include "2d/nav_map_builder_2d.h" #include "2d/nav_mesh_queries_2d.h" +#include "2d/nav_region_builder_2d.h" #include "2d/nav_region_iteration_2d.h" +#include "core/config/project_settings.h" using namespace Nav2D; @@ -44,6 +44,7 @@ void NavRegion2D::set_map(NavMap2D *p_map) { return; } + cancel_async_thread_join(); cancel_sync_request(); if (map) { @@ -51,11 +52,14 @@ void NavRegion2D::set_map(NavMap2D *p_map) { } map = p_map; - polygons_dirty = true; + iteration_dirty = true; if (map) { map->add_region(this); request_sync(); + if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) { + request_async_thread_join(); + } } } @@ -64,9 +68,7 @@ void NavRegion2D::set_enabled(bool p_enabled) { return; } enabled = p_enabled; - - // TODO: This should not require a full rebuild as the region has not really changed. - polygons_dirty = true; + iteration_dirty = true; request_sync(); } @@ -74,39 +76,32 @@ void NavRegion2D::set_enabled(bool p_enabled) { void NavRegion2D::set_use_edge_connections(bool p_enabled) { if (use_edge_connections != p_enabled) { use_edge_connections = p_enabled; - polygons_dirty = true; + iteration_dirty = true; } request_sync(); } -void NavRegion2D::set_transform(const Transform2D &p_transform) { +void NavRegion2D::set_transform(Transform2D p_transform) { if (transform == p_transform) { return; } transform = p_transform; - polygons_dirty = true; + iteration_dirty = true; request_sync(); } -void NavRegion2D::set_navigation_polygon(Ref p_navigation_polygon) { +void NavRegion2D::set_navigation_mesh(Ref p_navigation_mesh) { #ifdef DEBUG_ENABLED - if (map && p_navigation_polygon.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_polygon->get_cell_size()))) { - ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_polygon->get_cell_size()), double(map->get_cell_size()))); + if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) { + ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size()))); } #endif // DEBUG_ENABLED - RWLockWrite write_lock(navmesh_rwlock); + navmesh = p_navigation_mesh; - pending_navmesh_vertices.clear(); - pending_navmesh_polygons.clear(); - - if (p_navigation_polygon.is_valid()) { - p_navigation_polygon->get_data(pending_navmesh_vertices, pending_navmesh_polygons); - } - - polygons_dirty = true; + iteration_dirty = true; request_sync(); } @@ -132,7 +127,7 @@ void NavRegion2D::set_navigation_layers(uint32_t p_navigation_layers) { return; } navigation_layers = p_navigation_layers; - region_dirty = true; + iteration_dirty = true; request_sync(); } @@ -143,7 +138,7 @@ void NavRegion2D::set_enter_cost(real_t p_enter_cost) { return; } enter_cost = new_enter_cost; - region_dirty = true; + iteration_dirty = true; request_sync(); } @@ -154,7 +149,7 @@ void NavRegion2D::set_travel_cost(real_t p_travel_cost) { return; } travel_cost = new_travel_cost; - region_dirty = true; + iteration_dirty = true; request_sync(); } @@ -164,139 +159,150 @@ void NavRegion2D::set_owner_id(ObjectID p_owner_id) { return; } owner_id = p_owner_id; - region_dirty = true; + iteration_dirty = true; request_sync(); } +void NavRegion2D::scratch_polygons() { + iteration_dirty = true; + + request_sync(); +} + +real_t NavRegion2D::get_surface_area() const { + RWLockRead read_lock(iteration_rwlock); + return iteration->get_surface_area(); +} + +Rect2 NavRegion2D::get_bounds() const { + RWLockRead read_lock(iteration_rwlock); + return iteration->get_bounds(); +} + +LocalVector const &NavRegion2D::get_polygons() const { + RWLockRead read_lock(iteration_rwlock); + return iteration->get_navmesh_polygons(); +} + bool NavRegion2D::sync() { - RWLockWrite write_lock(region_rwlock); - - bool something_changed = region_dirty || polygons_dirty; - - region_dirty = false; - - update_polygons(); - - if (something_changed) { - iteration_id = iteration_id % UINT32_MAX + 1; + bool requires_map_update = false; + if (!map) { + return requires_map_update; } - return something_changed; + if (iteration_dirty && !iteration_building && !iteration_ready) { + _build_iteration(); + } + + if (iteration_ready) { + _sync_iteration(); + requires_map_update = true; + } + + return requires_map_update; } -void NavRegion2D::update_polygons() { - if (!polygons_dirty) { - return; - } - navmesh_polygons.clear(); - surface_area = 0.0; - bounds = Rect2(); - polygons_dirty = false; +void NavRegion2D::sync_async_tasks() { + if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) { + if (WorkerThreadPool::get_singleton()->is_task_completed(iteration_build_thread_task_id)) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id); - if (map == nullptr) { - return; - } - - RWLockRead read_lock(navmesh_rwlock); - - if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) { - return; - } - - int len = pending_navmesh_vertices.size(); - if (len == 0) { - return; - } - - const Vector2 *vertices_r = pending_navmesh_vertices.ptr(); - - navmesh_polygons.resize(pending_navmesh_polygons.size()); - - real_t _new_region_surface_area = 0.0; - Rect2 _new_bounds; - - bool first_vertex = true; - int navigation_mesh_polygon_index = 0; - - for (Polygon &polygon : navmesh_polygons) { - polygon.surface_area = 0.0; - - Vector navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index]; - navigation_mesh_polygon_index += 1; - - int navigation_mesh_polygon_size = navigation_mesh_polygon.size(); - if (navigation_mesh_polygon_size < 3) { - continue; - } - - const int *indices = navigation_mesh_polygon.ptr(); - bool valid(true); - - polygon.vertices.resize(navigation_mesh_polygon_size); - polygon.edges.resize(navigation_mesh_polygon_size); - - real_t _new_polygon_surface_area = 0.0; - - for (int j(2); j < navigation_mesh_polygon_size; j++) { - const Triangle2 triangle = Triangle2( - transform.xform(vertices_r[indices[0]]), - transform.xform(vertices_r[indices[j - 1]]), - transform.xform(vertices_r[indices[j]])); - - _new_polygon_surface_area += triangle.get_area(); - } - - polygon.surface_area = _new_polygon_surface_area; - _new_region_surface_area += _new_polygon_surface_area; - - for (int j(0); j < navigation_mesh_polygon_size; j++) { - int idx = indices[j]; - if (idx < 0 || idx >= len) { - valid = false; - break; - } - - Vector2 point_position = transform.xform(vertices_r[idx]); - polygon.vertices[j] = point_position; - - if (first_vertex) { - first_vertex = false; - _new_bounds.position = point_position; - } else { - _new_bounds.expand_to(point_position); - } - } - - if (!valid) { - ERR_BREAK_MSG(!valid, "The navigation polygon set in this region is not valid!"); + iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; + iteration_building = false; + iteration_ready = true; + request_sync(); } } - - surface_area = _new_region_surface_area; - bounds = _new_bounds; } -void NavRegion2D::get_iteration_update(NavRegionIteration2D &r_iteration) { - r_iteration.navigation_layers = get_navigation_layers(); - r_iteration.enter_cost = get_enter_cost(); - r_iteration.travel_cost = get_travel_cost(); - r_iteration.owner_object_id = get_owner_id(); - r_iteration.owner_type = get_type(); - r_iteration.owner_rid = get_self(); +void NavRegion2D::_build_iteration() { + if (!iteration_dirty || iteration_building || iteration_ready) { + return; + } - 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(); + iteration_dirty = false; + iteration_building = true; + iteration_ready = false; - r_iteration.navmesh_polygons.clear(); - r_iteration.navmesh_polygons.resize(navmesh_polygons.size()); - for (uint32_t i = 0; i < navmesh_polygons.size(); i++) { - Polygon &navmesh_polygon = navmesh_polygons[i]; - navmesh_polygon.owner = &r_iteration; - r_iteration.navmesh_polygons[i] = navmesh_polygon; + iteration_build.reset(); + + if (navmesh.is_valid()) { + navmesh->get_data(iteration_build.navmesh_data.vertices, iteration_build.navmesh_data.polygons); + } + + iteration_build.map_cell_size = map->get_merge_rasterizer_cell_size(); + + Ref new_iteration; + new_iteration.instantiate(); + + new_iteration->navigation_layers = get_navigation_layers(); + new_iteration->enter_cost = get_enter_cost(); + new_iteration->travel_cost = get_travel_cost(); + new_iteration->owner_object_id = get_owner_id(); + new_iteration->owner_type = get_type(); + new_iteration->owner_rid = get_self(); + new_iteration->enabled = get_enabled(); + new_iteration->transform = get_transform(); + new_iteration->owner_use_edge_connections = get_use_edge_connections(); + + iteration_build.region_iteration = new_iteration; + + if (use_async_iterations) { + iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavRegion2D::_build_iteration_threaded, &iteration_build, true, SNAME("NavRegionBuilder2D")); + request_async_thread_join(); + } else { + NavRegionBuilder2D::build_iteration(iteration_build); + + iteration_building = false; + iteration_ready = true; + } +} + +void NavRegion2D::_build_iteration_threaded(void *p_arg) { + NavRegionIterationBuild2D *_iteration_build = static_cast(p_arg); + + NavRegionBuilder2D::build_iteration(*_iteration_build); +} + +void NavRegion2D::_sync_iteration() { + if (iteration_building || !iteration_ready) { + return; + } + + performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count; + performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count; + performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count; + + RWLockWrite write_lock(iteration_rwlock); + ERR_FAIL_COND(iteration.is_null()); + iteration = Ref(); + DEV_ASSERT(iteration.is_null()); + iteration = iteration_build.region_iteration; + iteration_build.region_iteration = Ref(); + DEV_ASSERT(iteration_build.region_iteration.is_null()); + iteration_id = iteration_id % UINT32_MAX + 1; + + iteration_ready = false; + + cancel_async_thread_join(); +} + +Ref NavRegion2D::get_iteration() { + RWLockRead read_lock(iteration_rwlock); + return iteration; +} + +void NavRegion2D::request_async_thread_join() { + DEV_ASSERT(map); + if (map && !async_list_element.in_list()) { + map->add_region_async_thread_join_request(&async_list_element); + } +} + +void NavRegion2D::cancel_async_thread_join() { + if (map && async_list_element.in_list()) { + map->remove_region_async_thread_join_request(&async_list_element); } } @@ -312,11 +318,42 @@ void NavRegion2D::cancel_sync_request() { } } +void NavRegion2D::set_use_async_iterations(bool p_enabled) { + if (use_async_iterations == p_enabled) { + return; + } +#ifdef THREADS_ENABLED + use_async_iterations = p_enabled; +#endif +} + +bool NavRegion2D::get_use_async_iterations() const { + return use_async_iterations; +} + NavRegion2D::NavRegion2D() : - sync_dirty_request_list_element(this) { + sync_dirty_request_list_element(this), async_list_element(this) { type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION; + iteration_build.region = this; + iteration.instantiate(); + +#ifdef THREADS_ENABLED + use_async_iterations = GLOBAL_GET("navigation/world/region_use_async_iterations"); +#else + use_async_iterations = false; +#endif } NavRegion2D::~NavRegion2D() { + cancel_async_thread_join(); cancel_sync_request(); + + if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id); + iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; + } + + iteration_build.region = nullptr; + iteration_build.region_iteration = Ref(); + iteration = Ref(); } diff --git a/modules/navigation_2d/nav_region_2d.h b/modules/navigation_2d/nav_region_2d.h index 81277696db8..f30b9756a69 100644 --- a/modules/navigation_2d/nav_region_2d.h +++ b/modules/navigation_2d/nav_region_2d.h @@ -36,7 +36,7 @@ #include "core/os/rw_lock.h" #include "scene/resources/2d/navigation_polygon.h" -struct NavRegionIteration2D; +#include "2d/nav_region_iteration_2d.h" class NavRegion2D : public NavBase2D { RWLock region_rwlock; @@ -47,21 +47,30 @@ class NavRegion2D : public NavBase2D { bool use_edge_connections = true; - bool region_dirty = true; - bool polygons_dirty = true; - - LocalVector navmesh_polygons; - - real_t surface_area = 0.0; Rect2 bounds; - RWLock navmesh_rwlock; - Vector pending_navmesh_vertices; - Vector> pending_navmesh_polygons; + Ref navmesh; + + Nav2D::PerformanceData performance_data; uint32_t iteration_id = 0; SelfList sync_dirty_request_list_element; + mutable RWLock iteration_rwlock; + Ref iteration; + + NavRegionIterationBuild2D iteration_build; + bool use_async_iterations = true; + SelfList async_list_element; + WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; + static void _build_iteration_threaded(void *p_arg); + + bool iteration_dirty = true; + bool iteration_building = false; + bool iteration_ready = false; + + void _build_iteration(); + void _sync_iteration(); public: NavRegion2D(); @@ -69,9 +78,7 @@ public: uint32_t get_iteration_id() const { return iteration_id; } - void scratch_polygons() { - polygons_dirty = true; - } + void scratch_polygons(); void set_enabled(bool p_enabled); bool get_enabled() const { return enabled; } @@ -84,22 +91,21 @@ public: virtual void set_use_edge_connections(bool p_enabled) override; virtual bool get_use_edge_connections() const override { return use_edge_connections; } - void set_transform(const Transform2D &p_transform); + void set_transform(Transform2D transform); const Transform2D &get_transform() const { return transform; } - void set_navigation_polygon(Ref p_navigation_polygon); + void set_navigation_mesh(Ref p_navigation_mesh); + Ref get_navigation_mesh() const { return navmesh; } - LocalVector const &get_polygons() const { - return navmesh_polygons; - } + LocalVector const &get_polygons() const; Nav2D::ClosestPointQueryResult get_closest_point_info(const Vector2 &p_point) const; Vector2 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const; - real_t get_surface_area() const { return surface_area; } - Rect2 get_bounds() const { return bounds; } + real_t get_surface_area() const; + Rect2 get_bounds() const; // NavBase properties. virtual void set_navigation_layers(uint32_t p_navigation_layers) override; @@ -111,8 +117,17 @@ public: void request_sync(); void cancel_sync_request(); - void get_iteration_update(NavRegionIteration2D &r_iteration); + void sync_async_tasks(); + void request_async_thread_join(); + void cancel_async_thread_join(); -private: - void update_polygons(); + Ref get_iteration(); + + // Performance Monitor + int get_pm_polygon_count() const { return performance_data.pm_polygon_count; } + int get_pm_edge_count() const { return performance_data.pm_edge_count; } + int get_pm_edge_merge_count() const { return performance_data.pm_edge_merge_count; } + + void set_use_async_iterations(bool p_enabled); + bool get_use_async_iterations() const; }; diff --git a/modules/navigation_2d/nav_utils_2d.h b/modules/navigation_2d/nav_utils_2d.h index 5a4ab9ca4f8..38135b6f855 100644 --- a/modules/navigation_2d/nav_utils_2d.h +++ b/modules/navigation_2d/nav_utils_2d.h @@ -31,12 +31,13 @@ #pragma once #include "core/math/vector3.h" +#include "core/object/ref_counted.h" #include "core/templates/hash_map.h" #include "core/templates/hashfuncs.h" #include "servers/navigation/nav_heap.h" #include "servers/navigation/navigation_utilities.h" -struct NavBaseIteration2D; +class NavBaseIteration2D; namespace Nav2D { struct Polygon; @@ -71,28 +72,28 @@ struct EdgeKey { } }; -struct Edge { - /// The gateway in the edge, as, in some case, the whole edge might not be navigable. - struct Connection { - /// Polygon that this connection leads to. - Polygon *polygon = nullptr; +struct ConnectableEdge { + EdgeKey ek; + uint32_t polygon_index; + Vector2 pathway_start; + Vector2 pathway_end; +}; - /// Edge of the source polygon where this connection starts from. - int edge = -1; +struct Connection { + /// Polygon that this connection leads to. + Polygon *polygon = nullptr; - /// Point on the edge where the gateway leading to the poly starts. - Vector2 pathway_start; + /// Edge of the source polygon where this connection starts from. + int edge = -1; - /// Point on the edge where the gateway leading to the poly ends. - Vector2 pathway_end; - }; + /// Point on the edge where the gateway leading to the poly starts. + Vector2 pathway_start; - /// Connections from this edge to other polygons. - LocalVector connections; + /// Point on the edge where the gateway leading to the poly ends. + Vector2 pathway_end; }; struct Polygon { - /// Id of the polygon in the map. uint32_t id = UINT32_MAX; /// Navigation region or link that contains this polygon. @@ -100,9 +101,6 @@ struct Polygon { LocalVector vertices; - /// The edges of this `Polygon` - LocalVector edges; - real_t surface_area = 0.0; }; @@ -177,7 +175,7 @@ struct ClosestPointQueryResult { }; struct EdgeConnectionPair { - Edge::Connection connections[2]; + Connection connections[2]; int size = 0; }; diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index a027255acbb..ea6b6047736 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -76,6 +76,8 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer2D::region_create); ClassDB::bind_method(D_METHOD("region_get_iteration_id", "region"), &NavigationServer2D::region_get_iteration_id); + ClassDB::bind_method(D_METHOD("region_set_use_async_iterations", "region", "enabled"), &NavigationServer2D::region_set_use_async_iterations); + ClassDB::bind_method(D_METHOD("region_get_use_async_iterations", "region"), &NavigationServer2D::region_get_use_async_iterations); ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer2D::region_set_enabled); ClassDB::bind_method(D_METHOD("region_get_enabled", "region"), &NavigationServer2D::region_get_enabled); ClassDB::bind_method(D_METHOD("region_set_use_edge_connections", "region", "enabled"), &NavigationServer2D::region_set_use_edge_connections); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index 4a9fea9efc0..c4a2937bff7 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -102,6 +102,9 @@ public: virtual RID region_create() = 0; virtual uint32_t region_get_iteration_id(RID p_region) const = 0; + virtual void region_set_use_async_iterations(RID p_region, bool p_enabled) = 0; + virtual bool region_get_use_async_iterations(RID p_region) const = 0; + virtual void region_set_enabled(RID p_region, bool p_enabled) = 0; virtual bool region_get_enabled(RID p_region) const = 0; diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h index 8e0e0b6bada..d5a77e343e7 100644 --- a/servers/navigation_server_2d_dummy.h +++ b/servers/navigation_server_2d_dummy.h @@ -64,6 +64,8 @@ public: RID region_create() override { return RID(); } uint32_t region_get_iteration_id(RID p_region) const override { return 0; } + void region_set_use_async_iterations(RID p_region, bool p_enabled) override {} + bool region_get_use_async_iterations(RID p_region) const override { return false; } void region_set_enabled(RID p_region, bool p_enabled) override {} bool region_get_enabled(RID p_region) const override { return false; } void region_set_use_edge_connections(RID p_region, bool p_enabled) override {} diff --git a/tests/servers/test_navigation_server_2d.h b/tests/servers/test_navigation_server_2d.h index c7b0f0af479..b398536276d 100644 --- a/tests/servers/test_navigation_server_2d.h +++ b/tests/servers/test_navigation_server_2d.h @@ -602,6 +602,7 @@ TEST_SUITE("[Navigation2D]") { navigation_polygon->add_outline(PackedVector2Array({ Vector2(-1000.0, -1000.0), Vector2(1000.0, -1000.0), Vector2(1000.0, 1000.0), Vector2(-1000.0, 1000.0) })); navigation_server->map_set_active(map, true); navigation_server->map_set_use_async_iterations(map, false); + navigation_server->region_set_use_async_iterations(region, false); navigation_server->region_set_map(region, map); navigation_server->region_set_navigation_polygon(region, navigation_polygon); navigation_server->physics_process(0.0); // Give server some cycles to commit. @@ -659,6 +660,7 @@ TEST_SUITE("[Navigation2D]") { RID region = navigation_server->region_create(); navigation_server->map_set_active(map, true); navigation_server->map_set_use_async_iterations(map, false); + navigation_server->region_set_use_async_iterations(region, false); navigation_server->region_set_map(region, map); navigation_server->region_set_navigation_polygon(region, navigation_polygon); navigation_server->physics_process(0.0); // Give server some cycles to commit.