summaryrefslogtreecommitdiffstats
path: root/modules/navigation
diff options
context:
space:
mode:
Diffstat (limited to 'modules/navigation')
-rw-r--r--modules/navigation/2d/nav_mesh_generator_2d.cpp208
-rw-r--r--modules/navigation/3d/nav_mesh_generator_3d.cpp21
-rw-r--r--modules/navigation/3d/nav_mesh_queries_3d.cpp2
-rw-r--r--modules/navigation/editor/navigation_mesh_editor_plugin.cpp4
-rw-r--r--modules/navigation/nav_agent.cpp2
-rw-r--r--modules/navigation/nav_agent.h8
-rw-r--r--modules/navigation/nav_link.cpp2
-rw-r--r--modules/navigation/nav_map.cpp56
-rw-r--r--modules/navigation/nav_map.h8
-rw-r--r--modules/navigation/nav_obstacle.h2
-rw-r--r--modules/navigation/nav_region.cpp2
-rw-r--r--modules/navigation/nav_region.h2
-rw-r--r--modules/navigation/nav_utils.h2
13 files changed, 152 insertions, 167 deletions
diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation/2d/nav_mesh_generator_2d.cpp
index 78983187c7..c996c9e935 100644
--- a/modules/navigation/2d/nav_mesh_generator_2d.cpp
+++ b/modules/navigation/2d/nav_mesh_generator_2d.cpp
@@ -691,11 +691,15 @@ void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref<Navig
return;
}
- const Transform2D node_xform = p_source_geometry_data->root_node_transform * Transform2D(0.0, obstacle->get_global_position());
-
+ const Vector2 safe_scale = obstacle->get_global_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
+ // Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
+ const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+ const Vector2 uniform_max_scale = Vector2(scaling_max_value, scaling_max_value);
+ const Transform2D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform2D(obstacle->get_global_rotation(), uniform_max_scale, 0.0, obstacle->get_global_position());
+
Vector<Vector2> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
@@ -709,12 +713,15 @@ void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref<Navig
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
- circle_vertices_ptrw[i] = node_xform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
+ circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh());
}
+ // Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
+ const Transform2D node_xform = p_source_geometry_data->root_node_transform * obstacle->get_global_transform();
+
const Vector<Vector2> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
@@ -755,21 +762,19 @@ void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPoly
for (Node *E : parse_nodes) {
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, E, recurse_children);
}
-};
+}
static void generator_recursive_process_polytree_items(List<TPPLPoly> &p_tppl_in_polygon, const Clipper2Lib::PolyPathD *p_polypath_item) {
using namespace Clipper2Lib;
- Vector<Vector2> polygon_vertices;
+ TPPLPoly tp;
+ int size = p_polypath_item->Polygon().size();
+ tp.Init(size);
+ int j = 0;
for (const PointD &polypath_point : p_polypath_item->Polygon()) {
- polygon_vertices.push_back(Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y)));
- }
-
- TPPLPoly tp;
- tp.Init(polygon_vertices.size());
- for (int j = 0; j < polygon_vertices.size(); j++) {
- tp[j] = polygon_vertices[j];
+ tp[j] = Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y));
+ ++j;
}
if (p_polypath_item->IsHole()) {
@@ -842,87 +847,79 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
return;
}
- if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
- return;
- }
-
- int outline_count = p_navigation_mesh->get_outline_count();
-
- Vector<Vector<Vector2>> traversable_outlines;
- Vector<Vector<Vector2>> obstruction_outlines;
- Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> projected_obstructions;
-
- p_source_geometry_data->get_data(
- traversable_outlines,
- obstruction_outlines,
- projected_obstructions);
-
- if (outline_count == 0 && traversable_outlines.size() == 0) {
- return;
- }
-
using namespace Clipper2Lib;
-
PathsD traversable_polygon_paths;
PathsD obstruction_polygon_paths;
+ int obstruction_polygon_path_size = 0;
+ {
+ RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
- traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
- obstruction_polygon_paths.reserve(obstruction_outlines.size());
+ const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->traversable_outlines;
+ int outline_count = p_navigation_mesh->get_outline_count();
- for (int i = 0; i < outline_count; i++) {
- const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
- PathD subject_path;
- subject_path.reserve(traversable_outline.size());
- for (const Vector2 &traversable_point : traversable_outline) {
- const PointD &point = PointD(traversable_point.x, traversable_point.y);
- subject_path.push_back(point);
+ if (outline_count == 0 && (!p_source_geometry_data->has_data() || (traversable_outlines.is_empty()))) {
+ return;
}
- traversable_polygon_paths.push_back(subject_path);
- }
- for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
- PathD subject_path;
- subject_path.reserve(traversable_outline.size());
- for (const Vector2 &traversable_point : traversable_outline) {
- const PointD &point = PointD(traversable_point.x, traversable_point.y);
- subject_path.push_back(point);
- }
- traversable_polygon_paths.push_back(subject_path);
- }
+ const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->obstruction_outlines;
+ const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
- for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
- PathD clip_path;
- clip_path.reserve(obstruction_outline.size());
- for (const Vector2 &obstruction_point : obstruction_outline) {
- const PointD &point = PointD(obstruction_point.x, obstruction_point.y);
- clip_path.push_back(point);
+ traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
+ obstruction_polygon_paths.reserve(obstruction_outlines.size());
+
+ for (int i = 0; i < outline_count; i++) {
+ const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
+ PathD subject_path;
+ subject_path.reserve(traversable_outline.size());
+ for (const Vector2 &traversable_point : traversable_outline) {
+ subject_path.emplace_back(traversable_point.x, traversable_point.y);
+ }
+ traversable_polygon_paths.push_back(std::move(subject_path));
}
- obstruction_polygon_paths.push_back(clip_path);
- }
- if (!projected_obstructions.is_empty()) {
- for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
- if (projected_obstruction.carve) {
- continue;
+ for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
+ PathD subject_path;
+ subject_path.reserve(traversable_outline.size());
+ for (const Vector2 &traversable_point : traversable_outline) {
+ subject_path.emplace_back(traversable_point.x, traversable_point.y);
}
- if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
- continue;
+ traversable_polygon_paths.push_back(std::move(subject_path));
+ }
+
+ if (!projected_obstructions.is_empty()) {
+ for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+ if (projected_obstruction.carve) {
+ continue;
+ }
+ if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
+ continue;
+ }
+
+ PathD clip_path;
+ clip_path.reserve(projected_obstruction.vertices.size() / 2);
+ for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
+ clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
+ }
+ if (!IsPositive(clip_path)) {
+ std::reverse(clip_path.begin(), clip_path.end());
+ }
+ obstruction_polygon_paths.push_back(std::move(clip_path));
}
+ }
+ obstruction_polygon_path_size = obstruction_polygon_paths.size();
+ for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
PathD clip_path;
- clip_path.reserve(projected_obstruction.vertices.size() / 2);
- for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
- const PointD &point = PointD(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
- clip_path.push_back(point);
- }
- if (!IsPositive(clip_path)) {
- std::reverse(clip_path.begin(), clip_path.end());
+ clip_path.reserve(obstruction_outline.size());
+ for (const Vector2 &obstruction_point : obstruction_outline) {
+ clip_path.emplace_back(obstruction_point.x, obstruction_point.y);
}
- obstruction_polygon_paths.push_back(clip_path);
+ obstruction_polygon_paths.push_back(std::move(clip_path));
}
}
Rect2 baking_rect = p_navigation_mesh->get_baking_rect();
+ PathsD area_obstruction_polygon_paths;
if (baking_rect.has_area()) {
Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
@@ -934,48 +931,27 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
traversable_polygon_paths = RectClip(clipper_rect, traversable_polygon_paths);
- obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
+ area_obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
+ } else {
+ area_obstruction_polygon_paths = obstruction_polygon_paths;
}
- PathsD path_solution;
-
// first merge all traversable polygons according to user specified fill rule
PathsD dummy_clip_path;
traversable_polygon_paths = Union(traversable_polygon_paths, dummy_clip_path, FillRule::NonZero);
// merge all obstruction polygons, don't allow holes for what is considered "solid" 2D geometry
- obstruction_polygon_paths = Union(obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
+ area_obstruction_polygon_paths = Union(area_obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
- path_solution = Difference(traversable_polygon_paths, obstruction_polygon_paths, FillRule::NonZero);
+ PathsD path_solution = Difference(traversable_polygon_paths, area_obstruction_polygon_paths, FillRule::NonZero);
real_t agent_radius_offset = p_navigation_mesh->get_agent_radius();
if (agent_radius_offset > 0.0) {
path_solution = InflatePaths(path_solution, -agent_radius_offset, JoinType::Miter, EndType::Polygon);
}
- if (!projected_obstructions.is_empty()) {
- obstruction_polygon_paths.resize(0);
- for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
- if (!projected_obstruction.carve) {
- continue;
- }
- if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
- continue;
- }
-
- PathD clip_path;
- clip_path.reserve(projected_obstruction.vertices.size() / 2);
- for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
- const PointD &point = PointD(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
- clip_path.push_back(point);
- }
- if (!IsPositive(clip_path)) {
- std::reverse(clip_path.begin(), clip_path.end());
- }
- obstruction_polygon_paths.push_back(clip_path);
- }
- if (obstruction_polygon_paths.size() > 0) {
- path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
- }
+ if (obstruction_polygon_path_size > 0) {
+ obstruction_polygon_paths.resize(obstruction_polygon_path_size);
+ path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
}
//path_solution = RamerDouglasPeucker(path_solution, 0.025); //
@@ -994,33 +970,11 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
path_solution = RectClip(clipper_rect, path_solution);
}
- Vector<Vector<Vector2>> new_baked_outlines;
-
- for (const PathD &scaled_path : path_solution) {
- Vector<Vector2> polypath;
- for (const PointD &scaled_point : scaled_path) {
- polypath.push_back(Vector2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
- }
- new_baked_outlines.push_back(polypath);
- }
-
- if (new_baked_outlines.size() == 0) {
+ if (path_solution.size() == 0) {
p_navigation_mesh->clear();
return;
}
- PathsD polygon_paths;
- polygon_paths.reserve(new_baked_outlines.size());
-
- for (const Vector<Vector2> &baked_outline : new_baked_outlines) {
- PathD polygon_path;
- for (const Vector2 &baked_outline_point : baked_outline) {
- const PointD &point = PointD(baked_outline_point.x, baked_outline_point.y);
- polygon_path.push_back(point);
- }
- polygon_paths.push_back(polygon_path);
- }
-
ClipType clipper_cliptype = ClipType::Union;
List<TPPLPoly> tppl_in_polygon, tppl_out_polygon;
@@ -1028,7 +982,7 @@ void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<Navigation
PolyTreeD polytree;
ClipperD clipper_D;
- clipper_D.AddSubject(polygon_paths);
+ clipper_D.AddSubject(path_solution);
clipper_D.Execute(clipper_cliptype, FillRule::NonZero, polytree);
for (size_t i = 0; i < polytree.Count(); i++) {
diff --git a/modules/navigation/3d/nav_mesh_generator_3d.cpp b/modules/navigation/3d/nav_mesh_generator_3d.cpp
index e92a9d304b..3d0697a7cf 100644
--- a/modules/navigation/3d/nav_mesh_generator_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_generator_3d.cpp
@@ -595,11 +595,17 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
return;
}
- const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis(), obstacle->get_global_position());
-
+ const float elevation = obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y;
+ // Prevent non-positive scaling.
+ const Vector3 safe_scale = obstacle->get_global_basis().get_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
+ // Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
+ const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+ const Vector3 uniform_max_scale = Vector3(scaling_max_value, scaling_max_value, scaling_max_value);
+ const Transform3D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(uniform_max_scale), obstacle->get_global_position());
+
Vector<Vector3> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
@@ -613,12 +619,15 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
- circle_vertices_ptrw[i] = node_xform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
+ circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
}
- p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y - obstacle_radius, obstacle_radius, obstacle->get_carve_navigation_mesh());
+ p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, elevation - obstacle_radius, scaling_max_value * obstacle_radius, obstacle->get_carve_navigation_mesh());
}
+ // Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
+ const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y), obstacle->get_global_position());
+
const Vector<Vector3> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
@@ -635,7 +644,7 @@ void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<Navig
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
obstruction_shape_vertices_ptrw[i].y = 0.0;
}
- p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y, obstacle->get_height(), obstacle->get_carve_navigation_mesh());
+ p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, elevation, safe_scale.y * obstacle->get_height(), obstacle->get_carve_navigation_mesh());
}
void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {
@@ -660,7 +669,7 @@ void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<Navigati
for (Node *parse_node : parse_nodes) {
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, parse_node, recurse_children);
}
-};
+}
void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data) {
if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp
index 70207f86ce..5acc598d43 100644
--- a/modules/navigation/3d/nav_mesh_queries_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp
@@ -234,7 +234,7 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
- for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+ for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
const gd::Edge::Connection &connection = edge.connections[connection_index];
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
diff --git a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
index 7f0cbc7b5e..ed0cdeb287 100644
--- a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
+++ b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
@@ -54,8 +54,8 @@ void NavigationMeshEditor::_node_removed(Node *p_node) {
void NavigationMeshEditor::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
- button_bake->set_icon(get_theme_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
- button_reset->set_icon(get_theme_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
+ button_bake->set_button_icon(get_theme_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
+ button_reset->set_button_icon(get_theme_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
} break;
}
}
diff --git a/modules/navigation/nav_agent.cpp b/modules/navigation/nav_agent.cpp
index 2dbe57eb4a..037b237328 100644
--- a/modules/navigation/nav_agent.cpp
+++ b/modules/navigation/nav_agent.cpp
@@ -308,7 +308,7 @@ void NavAgent::set_avoidance_priority(real_t p_priority) {
rvo_agent_2d.avoidance_priority_ = avoidance_priority;
}
agent_dirty = true;
-};
+}
bool NavAgent::check_dirty() {
const bool was_dirty = agent_dirty;
diff --git a/modules/navigation/nav_agent.h b/modules/navigation/nav_agent.h
index 18997803f2..d56e053ac4 100644
--- a/modules/navigation/nav_agent.h
+++ b/modules/navigation/nav_agent.h
@@ -67,7 +67,7 @@ class NavAgent : public NavRid {
uint32_t avoidance_mask = 1;
real_t avoidance_priority = 1.0;
- Callable avoidance_callback = Callable();
+ Callable avoidance_callback;
bool agent_dirty = true;
@@ -130,13 +130,13 @@ public:
const Vector3 &get_velocity_forced() const { return velocity_forced; }
void set_avoidance_layers(uint32_t p_layers);
- uint32_t get_avoidance_layers() const { return avoidance_layers; };
+ uint32_t get_avoidance_layers() const { return avoidance_layers; }
void set_avoidance_mask(uint32_t p_mask);
- uint32_t get_avoidance_mask() const { return avoidance_mask; };
+ uint32_t get_avoidance_mask() const { return avoidance_mask; }
void set_avoidance_priority(real_t p_priority);
- real_t get_avoidance_priority() const { return avoidance_priority; };
+ real_t get_avoidance_priority() const { return avoidance_priority; }
void set_paused(bool p_paused);
bool get_paused() const;
diff --git a/modules/navigation/nav_link.cpp b/modules/navigation/nav_link.cpp
index c693cc91c8..61c3b59fab 100644
--- a/modules/navigation/nav_link.cpp
+++ b/modules/navigation/nav_link.cpp
@@ -57,7 +57,7 @@ void NavLink::set_enabled(bool p_enabled) {
// TODO: This should not require a full rebuild as the link has not really changed.
link_dirty = true;
-};
+}
void NavLink::set_bidirectional(bool p_bidirectional) {
if (bidirectional == p_bidirectional) {
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 8df1db533d..8055dd4bc8 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -427,25 +427,36 @@ void NavMap::sync() {
_new_pm_polygon_count = polygon_count;
// Group all edges per key.
- HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections;
+ 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++) {
- int next_point = (p + 1) % poly.points.size();
- gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
+ const int next_point = (p + 1) % poly.points.size();
+ const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
- HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey>::Iterator connection = connections.find(ek);
- if (!connection) {
- connections[ek] = Vector<gd::Edge::Connection>();
+ HashMap<gd::EdgeKey, ConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
+ if (!pair_it) {
+ pair_it = connection_pairs_map.insert(ek, ConnectionPair());
_new_pm_edge_count += 1;
+ ++free_edges_count;
}
- if (connections[ek].size() <= 1) {
+ 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;
- connections[ek].push_back(new_connection);
+
+ 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.");
@@ -453,20 +464,23 @@ void NavMap::sync() {
}
}
- Vector<gd::Edge::Connection> free_edges;
- for (KeyValue<gd::EdgeKey, Vector<gd::Edge::Connection>> &E : connections) {
- if (E.value.size() == 2) {
+ free_edges.clear();
+ free_edges.reserve(free_edges_count);
+
+ for (const KeyValue<gd::EdgeKey, ConnectionPair> &pair_it : connection_pairs_map) {
+ const ConnectionPair &pair = pair_it.value;
+ if (pair.size == 2) {
// Connect edge that are shared in different polygons.
- gd::Edge::Connection &c1 = E.value.write[0];
- gd::Edge::Connection &c2 = E.value.write[1];
+ 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.
_new_pm_edge_merge_count += 1;
} else {
- CRASH_COND_MSG(E.value.size() != 1, vformat("Number of connection != 1. Found: %d", E.value.size()));
- if (use_edge_connections && E.value[0].polygon->owner->get_use_edge_connections()) {
- free_edges.push_back(E.value[0]);
+ 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]);
}
}
}
@@ -480,14 +494,14 @@ void NavMap::sync() {
// connection, integration and path finding.
_new_pm_edge_free_count = free_edges.size();
- real_t sqr_edge_connection_margin = edge_connection_margin * edge_connection_margin;
+ const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
- for (int i = 0; i < free_edges.size(); i++) {
+ for (uint32_t i = 0; i < free_edges.size(); i++) {
const gd::Edge::Connection &free_edge = free_edges[i];
Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
- for (int j = 0; j < free_edges.size(); j++) {
+ for (uint32_t j = 0; j < free_edges.size(); j++) {
const gd::Edge::Connection &other_edge = free_edges[j];
if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
continue;
@@ -512,7 +526,7 @@ void NavMap::sync() {
} 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) > sqr_edge_connection_margin) {
+ if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
continue;
}
@@ -523,7 +537,7 @@ void NavMap::sync() {
} 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) > sqr_edge_connection_margin) {
+ if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
continue;
}
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index b9120c04d9..3442b78497 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -128,6 +128,14 @@ class NavMap : public NavRid {
HashMap<NavRegion *, LocalVector<gd::Edge::Connection>> region_external_connections;
+ struct ConnectionPair {
+ gd::Edge::Connection connections[2];
+ int size = 0;
+ };
+
+ HashMap<gd::EdgeKey, ConnectionPair, gd::EdgeKey> connection_pairs_map;
+ LocalVector<gd::Edge::Connection> free_edges;
+
public:
NavMap();
~NavMap();
diff --git a/modules/navigation/nav_obstacle.h b/modules/navigation/nav_obstacle.h
index e231e83836..a6aca9d637 100644
--- a/modules/navigation/nav_obstacle.h
+++ b/modules/navigation/nav_obstacle.h
@@ -92,7 +92,7 @@ public:
bool is_map_changed();
void set_avoidance_layers(uint32_t p_layers);
- uint32_t get_avoidance_layers() const { return avoidance_layers; };
+ uint32_t get_avoidance_layers() const { return avoidance_layers; }
void set_paused(bool p_paused);
bool get_paused() const;
diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
index 2c91b80af2..679997b8e1 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -59,7 +59,7 @@ void NavRegion::set_enabled(bool p_enabled) {
// TODO: This should not require a full rebuild as the region has not really changed.
polygons_dirty = true;
-};
+}
void NavRegion::set_use_edge_connections(bool p_enabled) {
if (use_edge_connections != p_enabled) {
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
index c015802b92..d13f666e9a 100644
--- a/modules/navigation/nav_region.h
+++ b/modules/navigation/nav_region.h
@@ -94,7 +94,7 @@ public:
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
- real_t get_surface_area() const { return surface_area; };
+ real_t get_surface_area() const { return surface_area; }
bool sync();
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
index ba4c44b748..c466c82fc7 100644
--- a/modules/navigation/nav_utils.h
+++ b/modules/navigation/nav_utils.h
@@ -94,7 +94,7 @@ struct Edge {
};
/// Connections from this edge to other polygons.
- Vector<Connection> connections;
+ LocalVector<Connection> connections;
};
struct Polygon {