diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index d1fac97b93e7..41ef298bc3bb 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -562,6 +562,13 @@ Returns all navigation regions [RID]s that are currently assigned to the requested navigation [param map]. + + + + + Returns [code]true[/code] if the [param map] synchronization uses an async process that runs on a background thread. + + @@ -608,6 +615,14 @@ Set the map's link connection radius used to connect links to navigation polygons. + + + + + + If [param enabled] is [code]true[/code] the [param map] synchronization uses an async process that runs on a background thread. + + diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index ab65f3f74376..6f0c68740a5b 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -641,6 +641,13 @@ Returns the map's up direction. + + + + + Returns [code]true[/code] if the [param map] synchronization uses an async process that runs on a background thread. + + @@ -711,6 +718,14 @@ Sets the map up direction. + + + + + + If [param enabled] is [code]true[/code] the [param map] synchronization uses an async process that runs on a background thread. + + diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index 9f3571dc5d30..45ff4ff74f2a 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -2165,6 +2165,9 @@ Maximum number of threads that can run pathfinding queries simultaneously on the same pathfinding graph, for example the same navigation map. Additional threads increase memory consumption and synchronization time due to the need for extra data copies prepared for each thread. A value of [code]-1[/code] means unlimited and the maximum available OS processor count is used. Defaults to [code]1[/code] when the OS does not support threads. + + If enabled, navigation map synchronization uses an async process that runs on a background thread. This avoids stalling the main thread but adds an additional delay to any navigation map change. + Maximum number of characters allowed to send as output from the debugger. Over this value, content is dropped. This helps not to stall the debugger connection. diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp index de78ea5826a8..6522770e0f72 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.cpp +++ b/modules/navigation/2d/godot_navigation_server_2d.cpp @@ -259,6 +259,14 @@ uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const { return NavigationServer3D::get_singleton()->map_get_iteration_id(p_map); } +void GodotNavigationServer2D::map_set_use_async_iterations(RID p_map, bool p_enabled) { + return NavigationServer3D::get_singleton()->map_set_use_async_iterations(p_map, p_enabled); +} + +bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const { + return NavigationServer3D::get_singleton()->map_get_use_async_iterations(p_map); +} + void FORWARD_2(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real); real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid); diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h index 80fdea5ba9db..f4a5023a0df5 100644 --- a/modules/navigation/2d/godot_navigation_server_2d.h +++ b/modules/navigation/2d/godot_navigation_server_2d.h @@ -78,6 +78,8 @@ class GodotNavigationServer2D : public NavigationServer2D { virtual void map_force_update(RID p_map) override; virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override; virtual uint32_t map_get_iteration_id(RID p_map) const override; + virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) override; + virtual bool map_get_use_async_iterations(RID p_map) const override; virtual RID region_create() override; virtual void region_set_enabled(RID p_region, bool p_enabled) override; diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 80e87a4b2e8d..1783e6bbdc95 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -364,6 +364,19 @@ RID GodotNavigationServer3D::agent_get_map(RID p_agent) const { return RID(); } +COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled) { + NavMap *map = map_owner.get_or_null(p_map); + ERR_FAIL_NULL(map); + map->set_use_async_iterations(p_enabled); +} + +bool GodotNavigationServer3D::map_get_use_async_iterations(RID p_map) const { + const NavMap *map = map_owner.get_or_null(p_map); + ERR_FAIL_NULL_V(map, false); + + return map->get_use_async_iterations(); +} + Vector3 GodotNavigationServer3D::map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const { const NavMap *map = map_owner.get_or_null(p_map); ERR_FAIL_NULL_V(map, Vector3()); diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h index ce8d333535f2..268dbee8e737 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.h +++ b/modules/navigation/3d/godot_navigation_server_3d.h @@ -147,6 +147,9 @@ class GodotNavigationServer3D : public NavigationServer3D { virtual void map_force_update(RID p_map) override; virtual uint32_t map_get_iteration_id(RID p_map) const override; + COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled); + virtual bool map_get_use_async_iterations(RID p_map) const override; + virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override; virtual RID region_create() override; diff --git a/modules/navigation/3d/nav_base_iteration_3d.h b/modules/navigation/3d/nav_base_iteration_3d.h new file mode 100644 index 000000000000..65e79af1a2ac --- /dev/null +++ b/modules/navigation/3d/nav_base_iteration_3d.h @@ -0,0 +1,48 @@ +/**************************************************************************/ +/* nav_base_iteration_3d.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. */ +/**************************************************************************/ + +#ifndef NAV_BASE_ITERATION_3D_H +#define NAV_BASE_ITERATION_3D_H + +#include "servers/navigation/navigation_utilities.h" + +struct NavBaseIteration { + uint32_t id = UINT32_MAX; + bool enabled = true; + uint32_t navigation_layers = 1; + real_t enter_cost = 0.0; + real_t travel_cost = 1.0; + NavigationUtilities::PathSegmentType owner_type; + ObjectID owner_object_id; + RID owner_rid; + bool owner_use_edge_connections = false; +}; + +#endif // NAV_BASE_ITERATION_3D_H diff --git a/modules/navigation/3d/nav_map_builder_3d.cpp b/modules/navigation/3d/nav_map_builder_3d.cpp new file mode 100644 index 000000000000..bdac1b5c45a5 --- /dev/null +++ b/modules/navigation/3d/nav_map_builder_3d.cpp @@ -0,0 +1,399 @@ +/**************************************************************************/ +/* nav_map_builder_3d.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. */ +/**************************************************************************/ + +#ifndef _3D_DISABLED + +#include "nav_map_builder_3d.h" + +#include "../nav_link.h" +#include "../nav_map.h" +#include "../nav_region.h" +#include "nav_map_iteration_3d.h" +#include "nav_region_iteration_3d.h" + +gd::PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &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)); + const int z = static_cast(Math::floor(p_pos.z / p_cell_size.z)); + + gd::PointKey p; + p.key = 0; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) { + _build_step_gather_region_polygons(r_build); + + _build_step_find_edge_connection_pairs(r_build); + + _build_step_merge_edge_connection_pairs(r_build); + + _build_step_edge_connection_margin_connections(r_build); + + _build_step_navlink_connections(r_build); + + _build_update_map_iteration(r_build); +} + +void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r_build) { + NavMapIteration *map_iteration = r_build.map_iteration; + gd::PerformanceData &performance_data = r_build.performance_data; + int polygon_count = r_build.polygon_count; + int navmesh_polygon_count = r_build.navmesh_polygon_count; + + // Remove regions connections. + map_iteration->external_region_connections.clear(); + for (const NavRegionIteration ®ion : map_iteration->region_iterations) { + map_iteration->external_region_connections[region.id] = LocalVector(); + } + + polygon_count = 0; + navmesh_polygon_count = 0; + for (NavRegionIteration ®ion : map_iteration->region_iterations) { + for (gd::Polygon ®ion_polygon : region.navmesh_polygons) { + region_polygon.id = polygon_count; + region_polygon.owner = ®ion; + + polygon_count++; + navmesh_polygon_count++; + } + } + + performance_data.pm_polygon_count = polygon_count; + r_build.polygon_count = polygon_count; + r_build.navmesh_polygon_count = navmesh_polygon_count; +} + +void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build) { + NavMapIteration *map_iteration = r_build.map_iteration; + HashMap &iter_connection_pairs_map = r_build.iter_connection_pairs_map; + gd::PerformanceData &performance_data = r_build.performance_data; + int free_edge_count = r_build.free_edge_count; + + iter_connection_pairs_map.clear(); + iter_connection_pairs_map.reserve(map_iteration->region_iterations.size()); + + for (NavRegionIteration ®ion : map_iteration->region_iterations) { + for (gd::Polygon ®ion_polygon : region.navmesh_polygons) { + for (uint32_t p = 0; p < region_polygon.points.size(); p++) { + const int next_point = (p + 1) % region_polygon.points.size(); + const gd::EdgeKey ek(region_polygon.points[p].key, region_polygon.points[next_point].key); + + HashMap::Iterator pair_it = iter_connection_pairs_map.find(ek); + if (!pair_it) { + pair_it = iter_connection_pairs_map.insert(ek, gd::EdgeConnectionPair()); + performance_data.pm_edge_count += 1; + ++free_edge_count; + } + gd::EdgeConnectionPair &pair = pair_it->value; + if (pair.size < 2) { + pair.connections[pair.size].polygon = ®ion_polygon; + pair.connections[pair.size].edge = p; + pair.connections[pair.size].pathway_start = region_polygon.points[p].pos; + pair.connections[pair.size].pathway_end = region_polygon.points[next_point].pos; + ++pair.size; + if (pair.size == 2) { + --free_edge_count; + } + + } else { + // The edge is already connected with another edge, skip. + ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001."); + } + } + } + } + r_build.free_edge_count = free_edge_count; +} + +void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build) { + HashMap &iter_connection_pairs_map = r_build.iter_connection_pairs_map; + LocalVector &iter_free_edges = r_build.iter_free_edges; + bool use_edge_connections = r_build.use_edge_connections; + gd::PerformanceData &performance_data = r_build.performance_data; + + iter_free_edges.clear(); + iter_free_edges.resize(r_build.free_edge_count); + uint32_t iter_free_edge_index = 0; + + for (const KeyValue &pair_it : iter_connection_pairs_map) { + const gd::EdgeConnectionPair &pair = pair_it.value; + + if (pair.size == 2) { + // Connect edge that are shared in different polygons. + const gd::Edge::Connection &c1 = pair.connections[0]; + const gd::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; + } else { + CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size)); + if (use_edge_connections && pair.connections[0].polygon->owner->owner_use_edge_connections) { + iter_free_edges[iter_free_edge_index++] = pair.connections[0]; + } + } + } + + iter_free_edges.resize(iter_free_edge_index); +} + +void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build) { + NavMapIteration *map_iteration = r_build.map_iteration; + const LocalVector &iter_free_edges = r_build.iter_free_edges; + bool use_edge_connections = r_build.use_edge_connections; + gd::PerformanceData &performance_data = r_build.performance_data; + const real_t edge_connection_margin = r_build.edge_connection_margin; + // Find the compatible near edges. + // + // Note: + // Considering that the edges must be compatible (for obvious reasons) + // to be connected, create new polygons to remove that small gap is + // not really useful and would result in wasteful computation during + // connection, integration and path finding. + + performance_data.pm_edge_free_count = iter_free_edges.size(); + + if (!use_edge_connections) { + return; + } + + const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin; + + for (uint32_t i = 0; i < iter_free_edges.size(); i++) { + const gd::Edge::Connection &free_edge = iter_free_edges[i]; + + Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos; + Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos; + + Vector3 edge_vector = edge_p2 - edge_p1; + real_t edge_vector_length_squared = edge_vector.length_squared(); + + for (uint32_t j = 0; j < iter_free_edges.size(); j++) { + const gd::Edge::Connection &other_edge = iter_free_edges[j]; + if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) { + continue; + } + + Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos; + Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos; + + // Compute the projection of the opposite edge on the current one + real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector_length_squared); + real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector_length_squared); + if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) { + continue; + } + + // Check if the two edges are close to each other enough and compute a pathway between the two regions. + Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1; + Vector3 other1; + if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) { + other1 = other_edge_p1; + } else { + other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); + } + if (other1.distance_squared_to(self1) > edge_connection_margin_squared) { + continue; + } + + Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1; + Vector3 other2; + if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) { + other2 = other_edge_p2; + } else { + other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); + } + if (other2.distance_squared_to(self2) > edge_connection_margin_squared) { + continue; + } + + // The edges can now be connected. + gd::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); + + // Add the connection to the region_connection map. + map_iteration->external_region_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection); + performance_data.pm_edge_connection_count += 1; + } + } +} + +void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_build) { + NavMapIteration *map_iteration = r_build.map_iteration; + const Vector3 &merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size; + real_t link_connection_radius = r_build.link_connection_radius; + real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius; + int polygon_count = r_build.polygon_count; + int link_polygon_count = r_build.link_polygon_count; + + // Search for polygons within range of a nav link. + for (NavLinkIteration &link : map_iteration->link_iterations) { + if (!link.enabled) { + continue; + } + const Vector3 link_start_pos = link.start_position; + const Vector3 link_end_pos = link.end_position; + + gd::Polygon *closest_start_polygon = nullptr; + real_t closest_start_sqr_dist = link_connection_radius_sqr; + Vector3 closest_start_point; + + gd::Polygon *closest_end_polygon = nullptr; + real_t closest_end_sqr_dist = link_connection_radius_sqr; + Vector3 closest_end_point; + + for (NavRegionIteration ®ion : map_iteration->region_iterations) { + AABB region_bounds = region.bounds.grow(link_connection_radius); + if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) { + continue; + } + for (gd::Polygon &polyon : region.navmesh_polygons) { + for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) { + const Face3 face(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos); + + { + const Vector3 start_point = face.get_closest_point_to(link_start_pos); + const real_t sqr_dist = start_point.distance_squared_to(link_start_pos); + + // Pick the polygon that is within our radius and is closer than anything we've seen yet. + if (sqr_dist < closest_start_sqr_dist) { + closest_start_sqr_dist = sqr_dist; + closest_start_point = start_point; + closest_start_polygon = &polyon; + } + } + + { + const Vector3 end_point = face.get_closest_point_to(link_end_pos); + const real_t sqr_dist = end_point.distance_squared_to(link_end_pos); + + // Pick the polygon that is within our radius and is closer than anything we've seen yet. + if (sqr_dist < closest_end_sqr_dist) { + closest_end_sqr_dist = sqr_dist; + closest_end_point = end_point; + closest_end_polygon = &polyon; + } + } + } + } + } + + // If we have both a start and end point, then create a synthetic polygon to route through. + if (closest_start_polygon && closest_end_polygon) { + link.navmesh_polygons.resize(1); + gd::Polygon &new_polygon = link.navmesh_polygons[0]; + new_polygon.id = polygon_count++; + new_polygon.owner = &link; + + link_polygon_count++; + + new_polygon.edges.clear(); + new_polygon.edges.resize(4); + new_polygon.points.resize(4); + + // Build a set of vertices that create a thin polygon going from the start to the end point. + new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) }; + new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) }; + new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) }; + new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) }; + + // Setup connections to go forward in the link. + { + gd::Edge::Connection entry_connection; + entry_connection.polygon = &new_polygon; + entry_connection.edge = -1; + entry_connection.pathway_start = new_polygon.points[0].pos; + entry_connection.pathway_end = new_polygon.points[1].pos; + closest_start_polygon->edges[0].connections.push_back(entry_connection); + + gd::Edge::Connection exit_connection; + exit_connection.polygon = closest_end_polygon; + exit_connection.edge = -1; + exit_connection.pathway_start = new_polygon.points[2].pos; + exit_connection.pathway_end = new_polygon.points[3].pos; + new_polygon.edges[2].connections.push_back(exit_connection); + } + + // If the link is bi-directional, create connections from the end to the start. + if (link.bidirectional) { + gd::Edge::Connection entry_connection; + entry_connection.polygon = &new_polygon; + entry_connection.edge = -1; + entry_connection.pathway_start = new_polygon.points[2].pos; + entry_connection.pathway_end = new_polygon.points[3].pos; + closest_end_polygon->edges[0].connections.push_back(entry_connection); + + gd::Edge::Connection exit_connection; + exit_connection.polygon = closest_start_polygon; + exit_connection.edge = -1; + exit_connection.pathway_start = new_polygon.points[0].pos; + exit_connection.pathway_end = new_polygon.points[1].pos; + new_polygon.edges[0].connections.push_back(exit_connection); + } + } + } + + r_build.polygon_count = polygon_count; + r_build.link_polygon_count = link_polygon_count; +} + +void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) { + NavMapIteration *map_iteration = r_build.map_iteration; + + map_iteration->navmesh_polygon_count = r_build.navmesh_polygon_count; + map_iteration->link_polygon_count = r_build.link_polygon_count; + + // TODO: This copying is for compatibility with legacy functions that expect a big polygon soup array. + // Those functions should be changed to work hierarchical with the region iteration polygons directly. + map_iteration->navmesh_polygons.resize(map_iteration->navmesh_polygon_count); + uint32_t polygon_index = 0; + for (NavRegionIteration ®ion : map_iteration->region_iterations) { + for (gd::Polygon ®ion_polygon : region.navmesh_polygons) { + map_iteration->navmesh_polygons[polygon_index++] = region_polygon; + } + } + + map_iteration->path_query_slots_mutex.lock(); + for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) { + p_path_query_slot.path_corridor.clear(); + p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count); + p_path_query_slot.traversable_polys.clear(); + p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25); + } + map_iteration->path_query_slots_mutex.unlock(); +} + +#endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_map_builder_3d.h b/modules/navigation/3d/nav_map_builder_3d.h new file mode 100644 index 000000000000..819097d1320b --- /dev/null +++ b/modules/navigation/3d/nav_map_builder_3d.h @@ -0,0 +1,52 @@ +/**************************************************************************/ +/* nav_map_builder_3d.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. */ +/**************************************************************************/ + +#ifndef NAV_MAP_BUILDER_3D_H +#define NAV_MAP_BUILDER_3D_H + +#include "../nav_utils.h" + +struct NavMapIterationBuild; + +class NavMapBuilder3D { + static void _build_step_gather_region_polygons(NavMapIterationBuild &r_build); + static void _build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build); + static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build); + static void _build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build); + static void _build_step_navlink_connections(NavMapIterationBuild &r_build); + static void _build_update_map_iteration(NavMapIterationBuild &r_build); + +public: + static gd::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size); + + static void build_navmap_iteration(NavMapIterationBuild &r_build); +}; + +#endif // NAV_MAP_BUILDER_3D_H diff --git a/modules/navigation/3d/nav_map_iteration_3d.h b/modules/navigation/3d/nav_map_iteration_3d.h new file mode 100644 index 000000000000..74a94e2c2515 --- /dev/null +++ b/modules/navigation/3d/nav_map_iteration_3d.h @@ -0,0 +1,114 @@ +/**************************************************************************/ +/* nav_map_iteration_3d.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. */ +/**************************************************************************/ + +#ifndef NAV_MAP_ITERATION_3D_H +#define NAV_MAP_ITERATION_3D_H + +#include "../nav_rid.h" +#include "../nav_utils.h" +#include "nav_mesh_queries_3d.h" + +#include "core/math/math_defs.h" +#include "core/os/semaphore.h" + +struct NavLinkIteration; +class NavRegion; +struct NavRegionIteration; +struct NavMapIteration; + +struct NavMapIterationBuild { + Vector3 merge_rasterizer_cell_size; + bool use_edge_connections = true; + real_t edge_connection_margin; + real_t link_connection_radius; + gd::PerformanceData performance_data; + int polygon_count = 0; + int free_edge_count = 0; + + HashMap iter_connection_pairs_map; + LocalVector iter_free_edges; + + NavMapIteration *map_iteration = nullptr; + + int navmesh_polygon_count = 0; + int link_polygon_count = 0; + + void reset() { + performance_data.reset(); + + iter_connection_pairs_map.clear(); + iter_free_edges.clear(); + polygon_count = 0; + free_edge_count = 0; + + navmesh_polygon_count = 0; + link_polygon_count = 0; + } +}; + +struct NavMapIteration { + mutable SafeNumeric users; + RWLock rwlock; + + Vector3 map_up; + LocalVector navmesh_polygons; + + LocalVector region_iterations; + LocalVector link_iterations; + + int navmesh_polygon_count = 0; + int link_polygon_count = 0; + + // The edge connections that the map builds on top with the edge connection margin. + HashMap> external_region_connections; + + HashMap region_ptr_to_region_id; + + LocalVector path_query_slots; + Mutex path_query_slots_mutex; + Semaphore path_query_slots_semaphore; +}; + +class NavMapIterationRead { + const NavMapIteration &map_iteration; + +public: + _ALWAYS_INLINE_ NavMapIterationRead(const NavMapIteration &p_iteration) : + map_iteration(p_iteration) { + map_iteration.rwlock.read_lock(); + map_iteration.users.increment(); + } + _ALWAYS_INLINE_ ~NavMapIterationRead() { + map_iteration.users.decrement(); + map_iteration.rwlock.read_unlock(); + } +}; + +#endif // NAV_MAP_ITERATION_3D_H diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp index 85424e100aef..57c559d2e13b 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.cpp +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -34,6 +34,7 @@ #include "../nav_base.h" #include "../nav_map.h" +#include "nav_region_iteration_3d.h" #include "core/math/geometry_3d.h" #include "servers/navigation/navigation_utilities.h" @@ -128,41 +129,41 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVectorowner->get_type(); - p_query_task.path_meta_point_types[1] = end_poly->owner->get_type(); + p_query_task.path_meta_point_types[0] = p_begin_polygon->owner->owner_type; + p_query_task.path_meta_point_types[1] = p_end_polygon->owner->owner_type; } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { p_query_task.path_meta_point_rids.resize(2); - p_query_task.path_meta_point_rids[0] = begin_poly->owner->get_self(); - p_query_task.path_meta_point_rids[1] = end_poly->owner->get_self(); + p_query_task.path_meta_point_rids[0] = p_begin_polygon->owner->owner_rid; + p_query_task.path_meta_point_rids[1] = p_end_polygon->owner->owner_rid; } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { p_query_task.path_meta_point_owners.resize(2); - p_query_task.path_meta_point_owners[0] = begin_poly->owner->get_owner_id(); - p_query_task.path_meta_point_owners[1] = end_poly->owner->get_owner_id(); + p_query_task.path_meta_point_owners[0] = p_begin_polygon->owner->owner_object_id; + p_query_task.path_meta_point_owners[1] = p_end_polygon->owner->owner_object_id; } p_query_task.path_points.resize(2); - p_query_task.path_points[0] = begin_point; - p_query_task.path_points[1] = end_point; + p_query_task.path_points[0] = p_query_task.begin_position; + p_query_task.path_points[1] = p_query_task.end_position; } -void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) { +void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon) { if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { - p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type()); + p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->owner_type); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { - p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self()); + p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->owner_rid); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { - p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id()); + p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->owner_object_id); } p_query_task.path_points.push_back(p_point); @@ -267,33 +268,31 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) { +void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons) { p_query_task.path_points.clear(); p_query_task.path_meta_point_types.clear(); p_query_task.path_meta_point_rids.clear(); p_query_task.path_meta_point_owners.clear(); - // Find begin polyon and begin position closest to start position and - // end polyon and end position closest to target position on the map. - const gd::Polygon *begin_poly = nullptr; - const gd::Polygon *end_poly = nullptr; - Vector3 begin_point; - Vector3 end_point; - - _query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point); + _query_task_find_start_end_positions(p_query_task, p_polygons); // Check for trivial cases - if (!begin_poly || !end_poly) { + if (!p_query_task.begin_polygon || !p_query_task.end_polygon) { p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED; return; } - if (begin_poly == end_poly) { - _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point); + if (p_query_task.begin_polygon == p_query_task.end_polygon) { + _query_task_create_same_polygon_two_point_path(p_query_task, p_query_task.begin_polygon, p_query_task.end_polygon); return; } - _query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point); + DEV_ASSERT(p_query_task.path_query_slot->path_corridor.size() == p_polygons.size() + p_query_task.link_polygons_size); + _query_task_build_path_corridor(p_query_task, p_polygons); + + if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) { + return; + } if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) { return; @@ -302,17 +301,17 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu // Post-Process path. switch (p_query_task.path_postprocessing) { case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { - _path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up); + _query_task_post_process_corridorfunnel(p_query_task); } break; case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { - _path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point); + _query_task_post_process_edgecentered(p_query_task); } break; case PathPostProcessing::PATH_POSTPROCESSING_NONE: { - _path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point); + _query_task_post_process_nopostprocessing(p_query_task); } break; default: { WARN_PRINT("No match for used PathPostProcessing - fallback to default"); - _path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up); + _query_task_post_process_corridorfunnel(p_query_task); } break; } @@ -343,21 +342,25 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; } -void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) { +void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons) { + const gd::Polygon *begin_polygon = p_query_task.begin_polygon; + const gd::Polygon *end_polygon = p_query_task.end_polygon; + const Vector3 &begin_position = p_query_task.begin_position; + Vector3 &end_position = p_query_task.end_position; // List of all reachable navigation polys. LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; for (gd::NavigationPoly &polygon : navigation_polys) { polygon.reset(); } - DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_link_polygons_size); + DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_query_task.link_polygons_size); // Initialize the matching navigation polygon. - gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; - begin_navigation_poly.poly = begin_poly; - begin_navigation_poly.entry = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; + gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_polygon->id]; + begin_navigation_poly.poly = begin_polygon; + begin_navigation_poly.entry = begin_position; + begin_navigation_poly.back_navigation_edge_pathway_start = begin_position; + begin_navigation_poly.back_navigation_edge_pathway_end = begin_position; // Heap of polygons to travel next. gd::Heap @@ -366,7 +369,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p traversable_polys.reserve(p_polygons.size() * 0.25); // This is an implementation of the A* algorithm. - p_query_task.least_cost_id = begin_poly->id; + p_query_task.least_cost_id = begin_polygon->id; int prev_least_cost_id = -1; bool found_route = false; @@ -382,16 +385,16 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p const gd::Edge::Connection &connection = edge.connections[connection_index]; // Only consider the connection to another polygon if this polygon is in a region with compatible layers. - if ((p_query_task.navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { + if ((p_query_task.navigation_layers & connection.polygon->owner->navigation_layers) == 0) { continue; } const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id]; real_t poly_enter_cost = 0.0; - real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); + real_t poly_travel_cost = least_cost_poly.poly->owner->travel_cost; - if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { - poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); + if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->owner_rid != least_cost_poly.poly->owner->owner_rid) { + poly_enter_cost = least_cost_poly.poly->owner->enter_cost; } prev_least_cost_id = p_query_task.least_cost_id; @@ -412,8 +415,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p 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) * - neighbor_poly.poly->owner->get_travel_cost(); + new_entry.distance_to(end_position) * + neighbor_poly.poly->owner->travel_cost; neighbor_poly.entry = new_entry; // Update the priority of the polygon in the heap. @@ -428,8 +431,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p 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) * - neighbor_poly.poly->owner->get_travel_cost(); + new_entry.distance_to(end_position) * + neighbor_poly.poly->owner->travel_cost; neighbor_poly.entry = new_entry; // Add the polygon to the heap of polygons to traverse next. @@ -450,33 +453,33 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p } // Set as end point the furthest reachable point. - end_poly = reachable_end; + end_polygon = reachable_end; real_t end_d = FLT_MAX; - for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { - Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); + for (size_t point_id = 2; point_id < end_polygon->points.size(); point_id++) { + Face3 f(end_polygon->points[0].pos, end_polygon->points[point_id - 1].pos, end_polygon->points[point_id].pos); Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { - end_point = spoint; + end_position = spoint; end_d = dpoint; } } // Search all faces of start polygon as well. bool closest_point_on_start_poly = false; - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + for (size_t point_id = 2; point_id < begin_polygon->points.size(); point_id++) { + Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos); Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { - end_point = spoint; + end_position = spoint; end_d = dpoint; closest_point_on_start_poly = true; } } if (closest_point_on_start_poly) { - _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point); + _query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, end_polygon); p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; return; } @@ -484,9 +487,9 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p for (gd::NavigationPoly &nav_poly : navigation_polys) { nav_poly.poly = nullptr; } - navigation_polys[begin_poly->id].poly = begin_poly; + navigation_polys[begin_polygon->id].poly = begin_polygon; - p_query_task.least_cost_id = begin_poly->id; + p_query_task.least_cost_id = begin_polygon->id; prev_least_cost_id = -1; reachable_end = nullptr; @@ -507,7 +510,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p } // Check if we reached the end - if (navigation_polys[p_query_task.least_cost_id].poly == end_poly) { + if (navigation_polys[p_query_task.least_cost_id].poly == end_polygon) { found_route = true; break; } @@ -518,16 +521,16 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p if (!found_route) { real_t end_d = FLT_MAX; // Search all faces of the start polygon for the closest point to our target position. - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + for (size_t point_id = 2; point_id < begin_polygon->points.size(); point_id++) { + Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos); Vector3 spoint = f.get_closest_point_to(p_query_task.target_position); real_t dpoint = spoint.distance_to(p_query_task.target_position); if (dpoint < end_d) { - end_point = spoint; + end_position = spoint; end_d = dpoint; } } - _query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point); + _query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, begin_polygon); p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED; return; } @@ -575,15 +578,18 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D } } -void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) { +void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task) { + const Vector3 &begin_position = p_query_task.begin_position; + const Vector3 &end_position = p_query_task.end_position; + const Vector3 &map_up = p_query_task.map_up; LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; // Set the apex poly/point to the end point - gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id]; + gd::NavigationPoly *apex_poly = &p_path_corridor[p_query_task.least_cost_id]; Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; - const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway); - if (p_end_point.is_equal_approx(back_edge_closest_point)) { + const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_position, back_pathway); + if (end_position.is_equal_approx(back_edge_closest_point)) { // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. if (apex_poly->back_navigation_poly_id != -1) { @@ -591,7 +597,7 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue } } - Vector3 apex_point = p_end_point; + Vector3 apex_point = end_position; gd::NavigationPoly *left_poly = apex_poly; Vector3 left_portal = apex_point; @@ -600,24 +606,24 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue gd::NavigationPoly *p = apex_poly; - _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + _query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon); while (p) { // Set left and right points of the pathway between polygons. Vector3 left = p->back_navigation_edge_pathway_start; Vector3 right = p->back_navigation_edge_pathway_end; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) { + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(map_up) < 0) { SWAP(left, right); } bool skip = false; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) { + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(map_up) >= 0) { //process - if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) { + if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(map_up) > 0) { left_poly = p; left_portal = left; } else { - clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up); + _query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly); apex_point = right_portal; p = right_poly; @@ -632,13 +638,13 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue } } - if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) { + if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(map_up) <= 0) { //process - if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) { + if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(map_up) < 0) { right_poly = p; right_portal = right; } else { - clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up); + _query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly); apex_point = left_portal; p = left_poly; @@ -661,18 +667,20 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue } // If the last point is not the begin point, add it to the list. - if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) { - _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); + if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_position) { + _query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon); } } -void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) { +void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task) { + const Vector3 &begin_position = p_query_task.begin_position; + const Vector3 &end_position = p_query_task.end_position; LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; - _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + _query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon); // Add mid points. - int np_id = p_least_cost_id; + int np_id = p_query_task.least_cost_id; while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) { if (p_path_corridor[np_id].back_navigation_edge != -1) { int prev = p_path_corridor[np_id].back_navigation_edge; @@ -687,57 +695,67 @@ void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQuery np_id = p_path_corridor[np_id].back_navigation_poly_id; } - _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); + _query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon); } -void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) { +void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task) { + const Vector3 &begin_position = p_query_task.begin_position; + const Vector3 &end_position = p_query_task.end_position; LocalVector &p_path_corridor = p_query_task.path_query_slot->path_corridor; - _query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon); + _query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon); // Add mid points. - int np_id = p_least_cost_id; + int np_id = p_query_task.least_cost_id; while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) { _query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly); np_id = p_path_corridor[np_id].back_navigation_poly_id; } - _query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly); + _query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon); } -void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point) { +void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons) { + // Find begin polyon and begin position closest to start position and + // end polyon and end position closest to target position on the map. real_t begin_d = FLT_MAX; real_t end_d = FLT_MAX; + Vector3 begin_position; + Vector3 end_position; + // Find the initial poly and the end poly on this map. - for (const gd::Polygon &p : p_polygons) { + for (const gd::Polygon &polygon : p_polygons) { // Only consider the polygon if it in a region with compatible layers. - if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) { + if ((p_query_task.navigation_layers & polygon.owner->navigation_layers) == 0) { continue; } // For each face check the distance between the origin/destination. - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + for (size_t point_id = 2; point_id < polygon.points.size(); point_id++) { + const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos); Vector3 point = face.get_closest_point_to(p_query_task.start_position); real_t distance_to_point = point.distance_to(p_query_task.start_position); if (distance_to_point < begin_d) { begin_d = distance_to_point; - *r_begin_poly = &p; - r_begin_point = point; + p_query_task.begin_polygon = &polygon; + begin_position = point; } point = face.get_closest_point_to(p_query_task.target_position); distance_to_point = point.distance_to(p_query_task.target_position); if (distance_to_point < end_d) { end_d = distance_to_point; - *r_end_poly = &p; - r_end_point = point; + p_query_task.end_polygon = &polygon; + end_position = point; } } } + + p_query_task.begin_position = begin_position; + p_query_task.end_position = end_position; } Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) { @@ -864,7 +882,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co closest_point_distance_squared = distance_squared; result.point = p_point - plane_normalized * distance; result.normal = plane_normal; - result.owner = polygon.owner->get_self(); + result.owner = polygon.owner->owner_rid; if (Math::is_zero_approx(distance)) { break; @@ -876,7 +894,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co closest_point_distance_squared = distance; result.point = closest_on_polygon; result.normal = plane_normal; - result.owner = polygon.owner->get_self(); + result.owner = polygon.owner->owner_rid; } } } @@ -889,7 +907,9 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up) { +void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) { + const Vector3 &map_up = p_query_task.map_up; + LocalVector &path_corridor = p_query_task.path_query_slot->path_corridor; Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1]; if (from.is_equal_approx(p_to_point)) { @@ -897,7 +917,7 @@ void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const Loc } Plane cut_plane; - cut_plane.normal = (from - p_to_point).cross(p_map_up); + cut_plane.normal = (from - p_to_point).cross(map_up); if (cut_plane.normal == Vector3()) { return; } @@ -909,7 +929,7 @@ void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const Loc Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end; ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1); - from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id]; + from_poly = &path_corridor[from_poly->back_navigation_poly_id]; if (!pathway_start.is_equal_approx(pathway_end)) { Vector3 inters; diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h index 2fd433e07d46..7d7202f2c4ec 100644 --- a/modules/navigation/3d/nav_mesh_queries_3d.h +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -74,10 +74,15 @@ class NavMeshQueries3D { // Path building. Vector3 begin_position; Vector3 end_position; + const gd::Polygon *begin_polygon = nullptr; + const gd::Polygon *end_polygon = nullptr; uint32_t least_cost_id = 0; + + // Map. Vector3 map_up; NavMap *map = nullptr; PathQuerySlot *path_query_slot = nullptr; + uint32_t link_polygons_size = 0; // Path points. LocalVector path_points; @@ -103,17 +108,15 @@ class NavMeshQueries3D { static void map_query_path(NavMap *map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback); - static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size); - - static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point); - static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon); - static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point); - static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_polygon, Vector3 end_point); - static void _path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up); - static void _path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point); - static void _path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point); - - static void clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up); + static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons); + static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *p_begin_polygon, const gd::Polygon *p_end_polygon); + static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon); + static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons); + static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector &p_polygons); + static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task); + static void _query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task); + static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task); + static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly); static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task); 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); diff --git a/modules/navigation/3d/nav_region_iteration_3d.h b/modules/navigation/3d/nav_region_iteration_3d.h new file mode 100644 index 000000000000..0535e64854ef --- /dev/null +++ b/modules/navigation/3d/nav_region_iteration_3d.h @@ -0,0 +1,46 @@ +/**************************************************************************/ +/* nav_region_iteration_3d.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. */ +/**************************************************************************/ + +#ifndef NAV_REGION_ITERATION_3D_H +#define NAV_REGION_ITERATION_3D_H + +#include "../nav_utils.h" +#include "nav_base_iteration_3d.h" + +#include "core/math/aabb.h" + +struct NavRegionIteration : NavBaseIteration { + Transform3D transform; + LocalVector navmesh_polygons; + real_t surface_area = 0.0; + AABB bounds; +}; + +#endif // NAV_REGION_ITERATION_3D_H diff --git a/modules/navigation/nav_link.cpp b/modules/navigation/nav_link.cpp index 51c59f45a039..bb32551a4b79 100644 --- a/modules/navigation/nav_link.cpp +++ b/modules/navigation/nav_link.cpp @@ -122,3 +122,15 @@ NavLink::NavLink() : NavLink::~NavLink() { cancel_sync_request(); } + +void NavLink::get_iteration_update(NavLinkIteration &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.enabled = enabled; + r_iteration.start_position = start_position; + r_iteration.end_position = end_position; +} diff --git a/modules/navigation/nav_link.h b/modules/navigation/nav_link.h index eb2cbed35e30..623ff00bf661 100644 --- a/modules/navigation/nav_link.h +++ b/modules/navigation/nav_link.h @@ -31,9 +31,17 @@ #ifndef NAV_LINK_H #define NAV_LINK_H +#include "3d/nav_base_iteration_3d.h" #include "nav_base.h" #include "nav_utils.h" +struct NavLinkIteration : NavBaseIteration { + bool bidirectional = true; + Vector3 start_position; + Vector3 end_position; + LocalVector navmesh_polygons; +}; + #include "core/templates/self_list.h" class NavLink : public NavBase { @@ -78,6 +86,8 @@ class NavLink : public NavBase { void sync(); void request_sync(); void cancel_sync_request(); + + void get_iteration_update(NavLinkIteration &r_iteration); }; #endif // NAV_LINK_H diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 07ee96dc321f..b7dbd62d5ff6 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -30,6 +30,9 @@ #include "nav_map.h" +#include "3d/nav_map_builder_3d.h" +#include "3d/nav_mesh_queries_3d.h" +#include "3d/nav_region_iteration_3d.h" #include "nav_agent.h" #include "nav_link.h" #include "nav_obstacle.h" @@ -49,6 +52,18 @@ #define NAVMAP_ITERATION_ZERO_ERROR_MSG() #endif // DEBUG_ENABLED +#define GET_MAP_ITERATION() \ + iteration_slot_rwlock.read_lock(); \ + NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \ + NavMapIterationRead iteration_read_lock(map_iteration); \ + iteration_slot_rwlock.read_unlock(); + +#define GET_MAP_ITERATION_CONST() \ + iteration_slot_rwlock.read_lock(); \ + const NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \ + NavMapIterationRead iteration_read_lock(map_iteration); \ + iteration_slot_rwlock.read_unlock(); + void NavMap::set_up(Vector3 p_up) { if (up == p_up) { return; @@ -61,7 +76,7 @@ void NavMap::set_cell_size(real_t p_cell_size) { if (cell_size == p_cell_size) { return; } - cell_size = p_cell_size; + cell_size = MAX(p_cell_size, NavigationDefaults3D::navmesh_cell_size_min); _update_merge_rasterizer_cell_dimensions(); map_settings_dirty = true; } @@ -70,7 +85,7 @@ void NavMap::set_cell_height(real_t p_cell_height) { if (cell_height == p_cell_height) { return; } - cell_height = p_cell_height; + cell_height = MAX(p_cell_height, NavigationDefaults3D::navmesh_cell_size_min); _update_merge_rasterizer_cell_dimensions(); map_settings_dirty = true; } @@ -79,7 +94,7 @@ void NavMap::set_merge_rasterizer_cell_scale(float p_value) { if (merge_rasterizer_cell_scale == p_value) { return; } - merge_rasterizer_cell_scale = p_value; + merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults3D::navmesh_cell_size_min); _update_merge_rasterizer_cell_dimensions(); map_settings_dirty = true; } @@ -108,10 +123,14 @@ void NavMap::set_link_connection_radius(real_t p_link_connection_radius) { iteration_dirty = true; } +const Vector3 &NavMap::get_merge_rasterizer_cell_size() const { + return merge_rasterizer_cell_size; +} + gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { - const int x = static_cast(Math::floor(p_pos.x / merge_rasterizer_cell_size)); - const int y = static_cast(Math::floor(p_pos.y / merge_rasterizer_cell_height)); - const int z = static_cast(Math::floor(p_pos.z / merge_rasterizer_cell_size)); + const int x = static_cast(Math::floor(p_pos.x / merge_rasterizer_cell_size.x)); + const int y = static_cast(Math::floor(p_pos.y / merge_rasterizer_cell_size.y)); + const int z = static_cast(Math::floor(p_pos.z / merge_rasterizer_cell_size.z)); gd::PointKey p; p.key = 0; @@ -122,85 +141,91 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { } void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) { - RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { return; } - path_query_slots_semaphore.wait(); + GET_MAP_ITERATION(); - path_query_slots_mutex.lock(); - for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) { + map_iteration.path_query_slots_semaphore.wait(); + + map_iteration.path_query_slots_mutex.lock(); + for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) { if (!p_path_query_slot.in_use) { p_path_query_slot.in_use = true; p_query_task.path_query_slot = &p_path_query_slot; break; } } - path_query_slots_mutex.unlock(); + map_iteration.path_query_slots_mutex.unlock(); if (p_query_task.path_query_slot == nullptr) { - path_query_slots_semaphore.post(); + map_iteration.path_query_slots_semaphore.post(); ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(."); } - p_query_task.map_up = get_up(); + p_query_task.map_up = map_iteration.map_up; + p_query_task.link_polygons_size = map_iteration.link_polygon_count; - NavMeshQueries3D::query_task_polygons_get_path(p_query_task, polygons, up, link_polygons.size()); + NavMeshQueries3D::query_task_polygons_get_path(p_query_task, map_iteration.navmesh_polygons); - path_query_slots_mutex.lock(); + map_iteration.path_query_slots_mutex.lock(); uint32_t used_slot_index = p_query_task.path_query_slot->slot_index; - path_query_slots[used_slot_index].in_use = false; + map_iteration.path_query_slots[used_slot_index].in_use = false; p_query_task.path_query_slot = nullptr; - path_query_slots_mutex.unlock(); + map_iteration.path_query_slots_mutex.unlock(); - path_query_slots_semaphore.post(); + map_iteration.path_query_slots_semaphore.post(); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { - RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision); + GET_MAP_ITERATION_CONST(); + + return NavMeshQueries3D::polygons_get_closest_point_to_segment(map_iteration.navmesh_polygons, p_from, p_to, p_use_collision); } Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { - RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point); + GET_MAP_ITERATION_CONST(); + + return NavMeshQueries3D::polygons_get_closest_point(map_iteration.navmesh_polygons, p_point); } Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { - RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point); + GET_MAP_ITERATION_CONST(); + + return NavMeshQueries3D::polygons_get_closest_point_normal(map_iteration.navmesh_polygons, p_point); } RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { - RWLockRead read_lock(map_rwlock); if (iteration_id == 0) { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return RID(); } - return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point); + GET_MAP_ITERATION_CONST(); + + return NavMeshQueries3D::polygons_get_closest_point_owner(map_iteration.navmesh_polygons, p_point); } gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const { - RWLockRead read_lock(map_rwlock); + GET_MAP_ITERATION_CONST(); - return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point); + return NavMeshQueries3D::polygons_get_closest_point_info(map_iteration.navmesh_polygons, p_point); } void NavMap::add_region(NavRegion *p_region) { @@ -310,21 +335,21 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) { } Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const { - RWLockRead read_lock(map_rwlock); - - const LocalVector map_regions = get_regions(); + GET_MAP_ITERATION_CONST(); - if (map_regions.is_empty()) { + if (map_iteration.region_iterations.is_empty()) { return Vector3(); } - LocalVector accessible_regions; + LocalVector accessible_regions; + accessible_regions.reserve(map_iteration.region_iterations.size()); - for (const NavRegion *region : map_regions) { - if (!region->get_enabled() || (p_navigation_layers & region->get_navigation_layers()) == 0) { + for (uint32_t i = 0; i < map_iteration.region_iterations.size(); i++) { + const NavRegionIteration ®ion = map_iteration.region_iterations[i]; + if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) { continue; } - accessible_regions.push_back(region); + accessible_regions.push_back(i); } if (accessible_regions.is_empty()) { @@ -337,9 +362,9 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) RBMap accessible_regions_area_map; for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) { - const NavRegion *region = accessible_regions[accessible_region_index]; + const NavRegionIteration ®ion = map_iteration.region_iterations[accessible_regions[accessible_region_index]]; - real_t region_surface_area = region->get_surface_area(); + real_t region_surface_area = region.surface_area; if (region_surface_area == 0.0f) { continue; @@ -360,320 +385,167 @@ Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) uint32_t random_region_index = E->value; ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3()); - const NavRegion *random_region = accessible_regions[random_region_index]; - ERR_FAIL_NULL_V(random_region, Vector3()); + const NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]]; - return random_region->get_random_point(p_navigation_layers, p_uniformly); + return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); } else { uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1); - const NavRegion *random_region = accessible_regions[random_region_index]; - ERR_FAIL_NULL_V(random_region, Vector3()); + const NavRegionIteration &random_region = map_iteration.region_iterations[accessible_regions[random_region_index]]; - return random_region->get_random_point(p_navigation_layers, p_uniformly); + return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); } } -void NavMap::sync() { - RWLockWrite write_lock(map_rwlock); +void NavMap::_build_iteration() { + if (!iteration_dirty || iteration_building || iteration_ready) { + return; + } - performance_data.pm_region_count = regions.size(); - performance_data.pm_agent_count = agents.size(); - performance_data.pm_link_count = links.size(); - performance_data.pm_obstacle_count = obstacles.size(); + // Get the next free iteration slot that should be potentially unused. + iteration_slot_rwlock.read_lock(); + NavMapIteration &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2]; + // Check if the iteration slot is truly free or still used by an external thread. + bool iteration_is_free = next_map_iteration.users.get() == 0; + iteration_slot_rwlock.read_unlock(); - _sync_dirty_map_update_requests(); + if (!iteration_is_free) { + // A long running pathfinding thread or something is still reading + // from this older iteration and needs to finish first. + // Return and wait for the next sync cycle to check again. + return; + } - if (iteration_dirty) { - 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; + // Iteration slot is free and no longer used by anything, let's build. - // Remove regions connections. - region_external_connections.clear(); - for (NavRegion *region : regions) { - region_external_connections[region] = LocalVector(); - } + iteration_dirty = false; + iteration_building = true; + iteration_ready = false; - // Resize the polygon count. - int polygon_count = 0; - for (const NavRegion *region : regions) { - if (!region->get_enabled()) { - continue; - } - polygon_count += region->get_polygons().size(); - } - polygons.resize(polygon_count); + // We don't need to hold any lock because at this point nothing else can touch it. + // All new queries are already forwarded to the other iteration slot. - // Copy all region polygons in the map. - polygon_count = 0; - for (const NavRegion *region : regions) { - if (!region->get_enabled()) { - continue; - } - const LocalVector &polygons_source = region->get_polygons(); - for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons[polygon_count] = polygons_source[n]; - polygons[polygon_count].id = polygon_count; - polygon_count++; - } - } + iteration_build.reset(); - performance_data.pm_polygon_count = polygon_count; - - // Group all edges per key. - connection_pairs_map.clear(); - connection_pairs_map.reserve(polygons.size()); - int free_edges_count = 0; // How many ConnectionPairs have only one Connection. - - for (gd::Polygon &poly : polygons) { - for (uint32_t p = 0; p < poly.points.size(); p++) { - const int next_point = (p + 1) % poly.points.size(); - const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key); - - HashMap::Iterator pair_it = connection_pairs_map.find(ek); - if (!pair_it) { - pair_it = connection_pairs_map.insert(ek, ConnectionPair()); - performance_data.pm_edge_count += 1; - ++free_edges_count; - } - ConnectionPair &pair = pair_it->value; - if (pair.size < 2) { - // Add the polygon/edge tuple to this key. - gd::Edge::Connection new_connection; - new_connection.polygon = &poly; - new_connection.edge = p; - new_connection.pathway_start = poly.points[p].pos; - new_connection.pathway_end = poly.points[next_point].pos; - - pair.connections[pair.size] = new_connection; - ++pair.size; - if (pair.size == 2) { - --free_edges_count; - } - - } else { - // The edge is already connected with another edge, skip. - ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001."); - } - } - } + iteration_build.merge_rasterizer_cell_size = get_merge_rasterizer_cell_size(); + iteration_build.use_edge_connections = get_use_edge_connections(); + iteration_build.edge_connection_margin = get_edge_connection_margin(); + iteration_build.link_connection_radius = get_link_connection_radius(); - free_edges.clear(); - free_edges.reserve(free_edges_count); - - for (const KeyValue &pair_it : connection_pairs_map) { - const ConnectionPair &pair = pair_it.value; - if (pair.size == 2) { - // Connect edge that are shared in different polygons. - const gd::Edge::Connection &c1 = pair.connections[0]; - const gd::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; - } 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()) { - free_edges.push_back(pair.connections[0]); - } - } - } + uint32_t enabled_region_count = 0; + uint32_t enabled_link_count = 0; - // Find the compatible near edges. - // - // Note: - // Considering that the edges must be compatible (for obvious reasons) - // to be connected, create new polygons to remove that small gap is - // not really useful and would result in wasteful computation during - // connection, integration and path finding. - performance_data.pm_edge_free_count = free_edges.size(); - - const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin; - - for (uint32_t i = 0; i < free_edges.size(); i++) { - const gd::Edge::Connection &free_edge = free_edges[i]; - Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos; - Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos; - - for (uint32_t j = 0; j < free_edges.size(); j++) { - const gd::Edge::Connection &other_edge = free_edges[j]; - if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) { - continue; - } - - Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos; - Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos; - - // Compute the projection of the opposite edge on the current one - Vector3 edge_vector = edge_p2 - edge_p1; - real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared()); - real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared()); - if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) { - continue; - } - - // Check if the two edges are close to each other enough and compute a pathway between the two regions. - Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1; - Vector3 other1; - if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) { - other1 = other_edge_p1; - } else { - other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); - } - if (other1.distance_squared_to(self1) > edge_connection_margin_squared) { - continue; - } - - Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1; - Vector3 other2; - if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) { - other2 = other_edge_p2; - } else { - other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); - } - if (other2.distance_squared_to(self2) > edge_connection_margin_squared) { - continue; - } - - // The edges can now be connected. - gd::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); - - // Add the connection to the region_connection map. - region_external_connections[(NavRegion *)free_edge.polygon->owner].push_back(new_connection); - performance_data.pm_edge_connection_count += 1; - } + for (NavRegion *region : regions) { + if (!region->get_enabled()) { + continue; } + enabled_region_count++; + } + for (NavLink *link : links) { + if (!link->get_enabled()) { + continue; + } + enabled_link_count++; + } - uint32_t link_poly_idx = 0; - link_polygons.resize(links.size()); + next_map_iteration.region_ptr_to_region_id.clear(); - // Search for polygons within range of a nav link. - for (const NavLink *link : links) { - if (!link->get_enabled()) { - continue; - } - const Vector3 start = link->get_start_position(); - const Vector3 end = link->get_end_position(); - - gd::Polygon *closest_start_polygon = nullptr; - real_t closest_start_sqr_dist = link_connection_radius * link_connection_radius; - Vector3 closest_start_point; - - gd::Polygon *closest_end_polygon = nullptr; - real_t closest_end_sqr_dist = link_connection_radius * link_connection_radius; - Vector3 closest_end_point; - - // Create link to any polygons within the search radius of the start point. - for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) { - gd::Polygon &start_poly = polygons[start_index]; - - // For each face check the distance to the start - for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) { - const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos); - const Vector3 start_point = start_face.get_closest_point_to(start); - const real_t sqr_dist = start_point.distance_squared_to(start); - - // Pick the polygon that is within our radius and is closer than anything we've seen yet. - if (sqr_dist < closest_start_sqr_dist) { - closest_start_sqr_dist = sqr_dist; - closest_start_point = start_point; - closest_start_polygon = &start_poly; - } - } - } + next_map_iteration.region_iterations.clear(); + next_map_iteration.link_iterations.clear(); - // Find any polygons within the search radius of the end point. - for (gd::Polygon &end_poly : polygons) { - // For each face check the distance to the end - for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) { - const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos); - const Vector3 end_point = end_face.get_closest_point_to(end); - const real_t sqr_dist = end_point.distance_squared_to(end); - - // Pick the polygon that is within our radius and is closer than anything we've seen yet. - if (sqr_dist < closest_end_sqr_dist) { - closest_end_sqr_dist = sqr_dist; - closest_end_point = end_point; - closest_end_polygon = &end_poly; - } - } - } + next_map_iteration.region_iterations.resize(enabled_region_count); + next_map_iteration.link_iterations.resize(enabled_link_count); - // If we have both a start and end point, then create a synthetic polygon to route through. - if (closest_start_polygon && closest_end_polygon) { - gd::Polygon &new_polygon = link_polygons[link_poly_idx++]; - new_polygon.id = polygon_count++; - new_polygon.owner = link; - - new_polygon.edges.clear(); - new_polygon.edges.resize(4); - new_polygon.points.clear(); - new_polygon.points.reserve(4); - - // Build a set of vertices that create a thin polygon going from the start to the end point. - new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) }); - new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) }); - new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) }); - new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) }); - - // Setup connections to go forward in the link. - { - gd::Edge::Connection entry_connection; - entry_connection.polygon = &new_polygon; - entry_connection.edge = -1; - entry_connection.pathway_start = new_polygon.points[0].pos; - entry_connection.pathway_end = new_polygon.points[1].pos; - closest_start_polygon->edges[0].connections.push_back(entry_connection); - - gd::Edge::Connection exit_connection; - exit_connection.polygon = closest_end_polygon; - exit_connection.edge = -1; - exit_connection.pathway_start = new_polygon.points[2].pos; - exit_connection.pathway_end = new_polygon.points[3].pos; - new_polygon.edges[2].connections.push_back(exit_connection); - } - - // If the link is bi-directional, create connections from the end to the start. - if (link->is_bidirectional()) { - gd::Edge::Connection entry_connection; - entry_connection.polygon = &new_polygon; - entry_connection.edge = -1; - entry_connection.pathway_start = new_polygon.points[2].pos; - entry_connection.pathway_end = new_polygon.points[3].pos; - closest_end_polygon->edges[0].connections.push_back(entry_connection); - - gd::Edge::Connection exit_connection; - exit_connection.polygon = closest_start_polygon; - exit_connection.edge = -1; - exit_connection.pathway_start = new_polygon.points[0].pos; - exit_connection.pathway_end = new_polygon.points[1].pos; - new_polygon.edges[0].connections.push_back(exit_connection); - } - } + uint32_t region_id_count = 0; + uint32_t link_id_count = 0; + + for (NavRegion *region : regions) { + if (!region->get_enabled()) { + continue; + } + NavRegionIteration ®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; + } + for (NavLink *link : links) { + if (!link->get_enabled()) { + continue; } + NavLinkIteration &link_iteration = next_map_iteration.link_iterations[link_id_count]; + link_iteration.id = link_id_count++; + link->get_iteration_update(link_iteration); + } + + next_map_iteration.map_up = get_up(); + + iteration_build.map_iteration = &next_map_iteration; + + if (use_async_iterations) { + iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D")); + } else { + NavMapBuilder3D::build_navmap_iteration(iteration_build); + + iteration_building = false; + iteration_ready = true; + } +} + +void NavMap::_build_iteration_threaded(void *p_arg) { + NavMapIterationBuild *_iteration_build = static_cast(p_arg); + + NavMapBuilder3D::build_navmap_iteration(*_iteration_build); +} + +void NavMap::_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; + 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; + + iteration_id = iteration_id % UINT32_MAX + 1; + + // Finally ping-pong switch the iteration slot. + iteration_slot_rwlock.write_lock(); + uint32_t next_iteration_slot_index = (iteration_slot_index + 1) % 2; + iteration_slot_index = next_iteration_slot_index; + iteration_slot_rwlock.write_unlock(); + + iteration_ready = false; +} + +void NavMap::sync() { + // Performance Monitor. + performance_data.pm_region_count = regions.size(); + performance_data.pm_agent_count = agents.size(); + performance_data.pm_link_count = links.size(); + performance_data.pm_obstacle_count = obstacles.size(); + + _sync_dirty_map_update_requests(); - // Some code treats 0 as a failure case, so we avoid returning 0 and modulo wrap UINT32_MAX manually. - iteration_id = iteration_id % UINT32_MAX + 1; + if (iteration_dirty && !iteration_building && !iteration_ready) { + _build_iteration(); + } + if (use_async_iterations && 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); - path_query_slots_mutex.lock(); - for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) { - p_path_query_slot.path_corridor.clear(); - p_path_query_slot.path_corridor.resize(polygons.size() + link_polygons.size()); - p_path_query_slot.traversable_polys.clear(); - p_path_query_slot.traversable_polys.reserve(polygons.size() * 0.25); + iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID; + iteration_building = false; + iteration_ready = true; } - path_query_slots_mutex.unlock(); + } + if (iteration_ready) { + _sync_iteration(); } map_settings_dirty = false; - iteration_dirty = false; _sync_avoidance(); } @@ -857,27 +729,39 @@ void NavMap::dispatch_callbacks() { } void NavMap::_update_merge_rasterizer_cell_dimensions() { - merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale; - merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale; + merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale; + merge_rasterizer_cell_size.y = cell_height * merge_rasterizer_cell_scale; + merge_rasterizer_cell_size.z = cell_size * merge_rasterizer_cell_scale; } int NavMap::get_region_connections_count(NavRegion *p_region) const { ERR_FAIL_NULL_V(p_region, 0); - HashMap>::ConstIterator found_connections = region_external_connections.find(p_region); - if (found_connections) { - return found_connections->value.size(); + GET_MAP_ITERATION_CONST(); + + HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + if (found_id) { + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + if (found_connections) { + return found_connections->value.size(); + } } + return 0; } Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const { ERR_FAIL_NULL_V(p_region, Vector3()); - HashMap>::ConstIterator found_connections = region_external_connections.find(p_region); - if (found_connections) { - ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); - return found_connections->value[p_connection_id].pathway_start; + GET_MAP_ITERATION_CONST(); + + HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + if (found_id) { + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + if (found_connections) { + ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); + return found_connections->value[p_connection_id].pathway_start; + } } return Vector3(); @@ -886,10 +770,15 @@ Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_c Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const { ERR_FAIL_NULL_V(p_region, Vector3()); - HashMap>::ConstIterator found_connections = region_external_connections.find(p_region); - if (found_connections) { - ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); - return found_connections->value[p_connection_id].pathway_end; + GET_MAP_ITERATION_CONST(); + + HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region); + if (found_id) { + HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value); + if (found_connections) { + ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3()); + return found_connections->value[p_connection_id].pathway_end; + } } return Vector3(); @@ -997,6 +886,19 @@ void NavMap::_sync_dirty_avoidance_update_requests() { sync_dirty_requests.obstacles.clear(); } +void NavMap::set_use_async_iterations(bool p_enabled) { + if (use_async_iterations == p_enabled) { + return; + } +#ifdef THREADS_ENABLED + use_async_iterations = p_enabled; +#endif +} + +bool NavMap::get_use_async_iterations() const { + return use_async_iterations; +} + NavMap::NavMap() { avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads"); avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads"); @@ -1014,13 +916,26 @@ NavMap::NavMap() { path_query_slots_max = 1; } - path_query_slots.resize(path_query_slots_max); - for (uint32_t i = 0; i < path_query_slots.size(); i++) { - path_query_slots[i].slot_index = i; + iteration_slots.resize(2); + + for (NavMapIteration &iteration_slot : iteration_slots) { + iteration_slot.path_query_slots.resize(path_query_slots_max); + for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) { + iteration_slot.path_query_slots[i].slot_index = i; + } + iteration_slot.path_query_slots_semaphore.post(path_query_slots_max); } - path_query_slots_semaphore.post(path_query_slots_max); +#ifdef THREADS_ENABLED + use_async_iterations = GLOBAL_GET("navigation/world/map_use_async_iterations"); +#else + use_async_iterations = false; +#endif } NavMap::~NavMap() { + 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; + } } diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index ce247b83c164..97fc9c8e1fac 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -31,6 +31,7 @@ #ifndef NAV_MAP_H #define NAV_MAP_H +#include "3d/nav_map_iteration_3d.h" #include "3d/nav_mesh_queries_3d.h" #include "nav_rid.h" #include "nav_utils.h" @@ -50,8 +51,6 @@ class NavAgent; class NavObstacle; class NavMap : public NavRid { - RWLock map_rwlock; - /// Map Up Vector3 up = Vector3(0, 1, 0); @@ -61,8 +60,8 @@ class NavMap : public NavRid { real_t cell_height = NavigationDefaults3D::navmesh_cell_height; // For the inter-region merging to work, internal rasterization is performed. - float merge_rasterizer_cell_size = NavigationDefaults3D::navmesh_cell_size; - float merge_rasterizer_cell_height = NavigationDefaults3D::navmesh_cell_height; + Vector3 merge_rasterizer_cell_size = Vector3(cell_size, cell_height, cell_size); + // This value is used to control sensitivity of internal rasterizer. float merge_rasterizer_cell_scale = 1.0; @@ -74,17 +73,12 @@ class NavMap : public NavRid { real_t link_connection_radius = NavigationDefaults3D::link_connection_radius; bool map_settings_dirty = true; - bool iteration_dirty = true; /// Map regions LocalVector regions; /// Map links LocalVector links; - LocalVector link_polygons; - - /// Map polygons - LocalVector polygons; /// RVO avoidance worlds RVO2D::RVOSimulator2D rvo_simulation_2d; @@ -119,16 +113,6 @@ class NavMap : public NavRid { // Performance Monitor gd::PerformanceData performance_data; - HashMap> region_external_connections; - - struct ConnectionPair { - gd::Edge::Connection connections[2]; - int size = 0; - }; - - HashMap connection_pairs_map; - LocalVector free_edges; - struct { SelfList::List regions; SelfList::List links; @@ -136,10 +120,25 @@ class NavMap : public NavRid { SelfList::List obstacles; } sync_dirty_requests; - LocalVector path_query_slots; int path_query_slots_max = 4; - Mutex path_query_slots_mutex; - Semaphore path_query_slots_semaphore; + + bool use_async_iterations = true; + + uint32_t iteration_slot_index = 0; + LocalVector iteration_slots; + mutable RWLock iteration_slot_rwlock; + + NavMapIterationBuild 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); + + bool iteration_dirty = true; + bool iteration_building = false; + bool iteration_ready = false; + + void _build_iteration(); + void _sync_iteration(); public: NavMap(); @@ -181,6 +180,7 @@ class NavMap : public NavRid { } gd::PointKey get_point_key(const Vector3 &p_pos) const; + const Vector3 &get_merge_rasterizer_cell_size() const; void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task); @@ -250,6 +250,9 @@ class NavMap : public NavRid { void remove_agent_sync_dirty_request(SelfList *p_sync_request); void remove_obstacle_sync_dirty_request(SelfList *p_sync_request); + void set_use_async_iterations(bool p_enabled); + bool get_use_async_iterations() const; + private: void _sync_dirty_map_update_requests(); void _sync_dirty_avoidance_update_requests(); diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp index 118970bc46c3..04cfcbc19524 100644 --- a/modules/navigation/nav_region.cpp +++ b/modules/navigation/nav_region.cpp @@ -32,7 +32,9 @@ #include "nav_map.h" +#include "3d/nav_map_builder_3d.h" #include "3d/nav_mesh_queries_3d.h" +#include "3d/nav_region_iteration_3d.h" void NavRegion::set_map(NavMap *p_map) { if (map == p_map) { @@ -153,8 +155,9 @@ void NavRegion::update_polygons() { if (!polygons_dirty) { return; } - polygons.clear(); + navmesh_polygons.clear(); surface_area = 0.0; + bounds = AABB(); polygons_dirty = false; if (map == nullptr) { @@ -174,14 +177,15 @@ void NavRegion::update_polygons() { const Vector3 *vertices_r = pending_navmesh_vertices.ptr(); - polygons.resize(pending_navmesh_polygons.size()); + navmesh_polygons.resize(pending_navmesh_polygons.size()); real_t _new_region_surface_area = 0.0; + AABB _new_bounds; - // Build + bool first_vertex = true; int navigation_mesh_polygon_index = 0; - for (gd::Polygon &polygon : polygons) { - polygon.owner = this; + + for (gd::Polygon &polygon : navmesh_polygons) { polygon.surface_area = 0.0; Vector navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index]; @@ -221,7 +225,14 @@ void NavRegion::update_polygons() { Vector3 point_position = transform.xform(vertices_r[idx]); polygon.points[j].pos = point_position; - polygon.points[j].key = map->get_point_key(point_position); + polygon.points[j].key = NavMapBuilder3D::get_point_key(point_position, map->get_merge_rasterizer_cell_size()); + + if (first_vertex) { + first_vertex = false; + _new_bounds.position = point_position; + } else { + _new_bounds.expand_to(point_position); + } } if (!valid) { @@ -230,6 +241,60 @@ void NavRegion::update_polygons() { } surface_area = _new_region_surface_area; + bounds = _new_bounds; +} + +void NavRegion::get_iteration_update(NavRegionIteration &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.enabled = enabled; + r_iteration.transform = transform; + r_iteration.owner_use_edge_connections = use_edge_connections; + r_iteration.bounds = get_bounds(); + + r_iteration.navmesh_polygons.resize(navmesh_polygons.size()); + + for (uint32_t i = 0; i < navmesh_polygons.size(); i++) { + const gd::Polygon &from_polygon = navmesh_polygons[i]; + gd::Polygon &to_polygon = r_iteration.navmesh_polygons[i]; + + to_polygon.surface_area = from_polygon.surface_area; + to_polygon.owner = &r_iteration; + to_polygon.points.resize(from_polygon.points.size()); + + const LocalVector &from_points = from_polygon.points; + LocalVector &to_points = to_polygon.points; + + to_points.resize(from_points.size()); + + for (uint32_t j = 0; j < from_points.size(); j++) { + to_points[j].pos = from_points[j].pos; + to_points[j].key = from_points[j].key; + } + + const LocalVector &from_edges = from_polygon.edges; + LocalVector &to_edges = to_polygon.edges; + + to_edges.resize(from_edges.size()); + + for (uint32_t j = 0; j < from_edges.size(); j++) { + const LocalVector &from_connections = from_edges[j].connections; + LocalVector &to_connections = to_edges[j].connections; + + to_connections.resize(from_connections.size()); + + for (uint32_t k = 0; k < from_connections.size(); k++) { + to_connections[k] = from_connections[k]; + } + } + } + + r_iteration.surface_area = surface_area; + r_iteration.owner_rid = get_self(); } void NavRegion::request_sync() { diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h index 16e8bb1d8464..09e92159f004 100644 --- a/modules/navigation/nav_region.h +++ b/modules/navigation/nav_region.h @@ -37,6 +37,8 @@ #include "core/os/rw_lock.h" #include "scene/resources/navigation_mesh.h" +struct NavRegionIteration; + class NavRegion : public NavBase { RWLock region_rwlock; @@ -48,10 +50,10 @@ class NavRegion : public NavBase { bool polygons_dirty = true; - /// Cache - LocalVector polygons; + LocalVector navmesh_polygons; real_t surface_area = 0.0; + AABB bounds; RWLock navmesh_rwlock; Vector pending_navmesh_vertices; @@ -88,7 +90,7 @@ class NavRegion : public NavBase { void set_navigation_mesh(Ref p_navigation_mesh); LocalVector const &get_polygons() const { - return polygons; + return navmesh_polygons; } Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const; @@ -96,11 +98,14 @@ class NavRegion : public NavBase { Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const; real_t get_surface_area() const { return surface_area; } + AABB get_bounds() const { return bounds; } bool sync(); void request_sync(); void cancel_sync_request(); + void get_iteration_update(NavRegionIteration &r_iteration); + private: void update_polygons(); }; diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h index ea22fb0bab3d..31bd5b571d7c 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -35,8 +35,9 @@ #include "core/templates/hash_map.h" #include "core/templates/hashfuncs.h" #include "core/templates/local_vector.h" +#include "servers/navigation/navigation_utilities.h" -class NavBase; +struct NavBaseIteration; namespace gd { struct Polygon; @@ -102,7 +103,7 @@ struct Polygon { uint32_t id = UINT32_MAX; /// Navigation region or link that contains this polygon. - const NavBase *owner = nullptr; + const NavBaseIteration *owner = nullptr; /// The points of this `Polygon` LocalVector points; @@ -308,6 +309,11 @@ class Heap { } }; +struct EdgeConnectionPair { + gd::Edge::Connection connections[2]; + int size = 0; +}; + struct PerformanceData { int pm_region_count = 0; int pm_agent_count = 0; @@ -318,6 +324,18 @@ struct PerformanceData { int pm_edge_connection_count = 0; int pm_edge_free_count = 0; int pm_obstacle_count = 0; + + void reset() { + pm_region_count = 0; + pm_agent_count = 0; + pm_link_count = 0; + pm_polygon_count = 0; + pm_edge_count = 0; + pm_edge_merge_count = 0; + pm_edge_connection_count = 0; + pm_edge_free_count = 0; + pm_obstacle_count = 0; + } }; } // namespace gd diff --git a/servers/navigation/navigation_globals.h b/servers/navigation/navigation_globals.h index aa54f955192e..44fb149abc9b 100644 --- a/servers/navigation/navigation_globals.h +++ b/servers/navigation/navigation_globals.h @@ -39,6 +39,7 @@ namespace NavigationDefaults3D { // each cell has the following cell_size and cell_height. constexpr float navmesh_cell_size{ 0.25f }; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size. constexpr float navmesh_cell_height{ 0.25f }; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height. +constexpr float navmesh_cell_size_min{ 0.01f }; constexpr auto navmesh_cell_size_hint{ "0.001,100,0.001,or_greater" }; // Map. diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index cd0dd7e3a042..a24a3312684c 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -60,6 +60,8 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer2D::map_force_update); ClassDB::bind_method(D_METHOD("map_get_iteration_id", "map"), &NavigationServer2D::map_get_iteration_id); + ClassDB::bind_method(D_METHOD("map_set_use_async_iterations", "map", "enabled"), &NavigationServer2D::map_set_use_async_iterations); + ClassDB::bind_method(D_METHOD("map_get_use_async_iterations", "map"), &NavigationServer2D::map_get_use_async_iterations); ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer2D::map_get_random_point); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index e7c7cf065331..510cf3965255 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -104,6 +104,9 @@ class NavigationServer2D : public Object { virtual void map_force_update(RID p_map) = 0; virtual uint32_t map_get_iteration_id(RID p_map) const = 0; + virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) = 0; + virtual bool map_get_use_async_iterations(RID p_map) const = 0; + virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const = 0; /// Creates a new region. diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h index c54e62f80e6d..dd6e618e0b4b 100644 --- a/servers/navigation_server_2d_dummy.h +++ b/servers/navigation_server_2d_dummy.h @@ -60,6 +60,8 @@ class NavigationServer2DDummy : public NavigationServer2D { void map_force_update(RID p_map) override {} Vector2 map_get_random_point(RID p_map, uint32_t p_naviation_layers, bool p_uniformly) const override { return Vector2(); } uint32_t map_get_iteration_id(RID p_map) const override { return 0; } + void map_set_use_async_iterations(RID p_map, bool p_enabled) override {} + bool map_get_use_async_iterations(RID p_map) const override { return false; } RID region_create() override { return RID(); } void region_set_enabled(RID p_region, bool p_enabled) override {} diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index 4c36df83f334..05ce3d8d3564 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -70,6 +70,8 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer3D::map_force_update); ClassDB::bind_method(D_METHOD("map_get_iteration_id", "map"), &NavigationServer3D::map_get_iteration_id); + ClassDB::bind_method(D_METHOD("map_set_use_async_iterations", "map", "enabled"), &NavigationServer3D::map_set_use_async_iterations); + ClassDB::bind_method(D_METHOD("map_get_use_async_iterations", "map"), &NavigationServer3D::map_get_use_async_iterations); ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer3D::map_get_random_point); @@ -245,6 +247,8 @@ NavigationServer3D::NavigationServer3D() { GLOBAL_DEF_BASIC("navigation/3d/default_edge_connection_margin", NavigationDefaults3D::edge_connection_margin); GLOBAL_DEF_BASIC("navigation/3d/default_link_connection_radius", NavigationDefaults3D::link_connection_radius); + GLOBAL_DEF("navigation/world/map_use_async_iterations", true); + GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true); GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true); diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index df5f7658bb2a..5a9896a86a3b 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -118,6 +118,9 @@ class NavigationServer3D : public Object { virtual void map_force_update(RID p_map) = 0; virtual uint32_t map_get_iteration_id(RID p_map) const = 0; + virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) = 0; + virtual bool map_get_use_async_iterations(RID p_map) const = 0; + virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const = 0; /// Creates a new region. diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h index 418f7b373d25..2ab4c6e6e2c9 100644 --- a/servers/navigation_server_3d_dummy.h +++ b/servers/navigation_server_3d_dummy.h @@ -67,6 +67,8 @@ class NavigationServer3DDummy : public NavigationServer3D { TypedArray map_get_obstacles(RID p_map) const override { return TypedArray(); } void map_force_update(RID p_map) override {} uint32_t map_get_iteration_id(RID p_map) const override { return 0; } + void map_set_use_async_iterations(RID p_map, bool p_enabled) override {} + bool map_get_use_async_iterations(RID p_map) const override { return false; } RID region_create() override { return RID(); } void region_set_enabled(RID p_region, bool p_enabled) override {} diff --git a/tests/servers/test_navigation_server_3d.h b/tests/servers/test_navigation_server_3d.h index 46a571682075..5600da948c83 100644 --- a/tests/servers/test_navigation_server_3d.h +++ b/tests/servers/test_navigation_server_3d.h @@ -572,6 +572,7 @@ TEST_SUITE("[Navigation]") { RID map = navigation_server->map_create(); RID region = navigation_server->region_create(); Ref navigation_mesh = memnew(NavigationMesh); + navigation_server->map_set_use_async_iterations(map, false); navigation_server->map_set_active(map, true); navigation_server->region_set_map(region, map); navigation_server->region_set_navigation_mesh(region, navigation_mesh); @@ -667,6 +668,7 @@ TEST_SUITE("[Navigation]") { RID map = navigation_server->map_create(); RID region = navigation_server->region_create(); Ref navigation_mesh = memnew(NavigationMesh); + navigation_server->map_set_use_async_iterations(map, false); navigation_server->map_set_active(map, true); navigation_server->region_set_map(region, map); navigation_server->region_set_navigation_mesh(region, navigation_mesh); @@ -716,6 +718,7 @@ TEST_SUITE("[Navigation]") { RID map = navigation_server->map_create(); 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_map(region, map); navigation_server->region_set_navigation_mesh(region, navigation_mesh); navigation_server->process(0.0); // Give server some cycles to commit.