From a92e1d4a20d5c3a971cd70fe8757944ac998eb55 Mon Sep 17 00:00:00 2001 From: Marc Gilleron Date: Wed, 3 Apr 2024 19:44:45 +0100 Subject: [PATCH] Optimize some parts of NavMap::sync --- modules/navigation/nav_map.cpp | 61 ++++++++++++++++++++++------------ 1 file changed, 40 insertions(+), 21 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 8df1db533d9..04c8a5a9437 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -426,26 +426,42 @@ void NavMap::sync() { _new_pm_polygon_count = polygon_count; + struct ConnectionPair { + gd::Edge::Connection connections[2]; + int size = 0; + }; + // Group all edges per key. - HashMap, gd::EdgeKey> connections; + HashMap connection_pairs_map; + 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>::Iterator connection = connections.find(ek); - if (!connection) { - connections[ek] = Vector(); + HashMap::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 +469,23 @@ void NavMap::sync() { } } - Vector free_edges; - for (KeyValue> &E : connections) { - if (E.value.size() == 2) { + LocalVector free_edges; + 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. - 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 +499,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 +531,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 +542,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; }