From 65afe14ca4cb1cae13dbf12f276d536deb8d4a9a Mon Sep 17 00:00:00 2001 From: Relintai Date: Sat, 30 Jul 2022 00:44:53 +0200 Subject: [PATCH] Ported: [3.5] Replace Navigation std::vector use with LocalVector Replace Navigation std::vector use with LocalVector. - smix8 https://github.com/godotengine/godot/commit/d0a78d05eb2c5c845adbc7e7b3de3d4f52af8774 --- modules/navigation/nav_map.cpp | 80 ++++++++++--------- modules/navigation/nav_map.h | 14 ++-- modules/navigation/nav_region.h | 4 +- modules/navigation/nav_utils.h | 6 +- .../pandemonium_navigation_server.cpp | 25 ++++-- .../pandemonium_navigation_server.h | 2 +- 6 files changed, 74 insertions(+), 57 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 6b48c1000..f01746132 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -126,7 +126,7 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } // List of all reachable navigation polys. - std::vector navigation_polys; + LocalVector navigation_polys; navigation_polys.reserve(polygons.size() * 0.75); // Add the start polygon to the reachable navigation polygons. @@ -179,20 +179,18 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, pathway); const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance; - const std::vector::iterator it = std::find( - navigation_polys.begin(), - navigation_polys.end(), - gd::NavigationPoly(connection.polygon)); + int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon)); - if (it != navigation_polys.end()) { + if (already_visited_polygon_index != -1) { // Polygon already visited, check if we can reduce the travel cost. - if (new_distance < it->traveled_distance) { - it->back_navigation_poly_id = least_cost_id; - it->back_navigation_edge = connection.edge; - it->back_navigation_edge_pathway_start = connection.pathway_start; - it->back_navigation_edge_pathway_end = connection.pathway_end; - it->traveled_distance = new_distance; - it->entry = new_entry; + gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index]; + if (new_distance < avp.traveled_distance) { + avp.back_navigation_poly_id = least_cost_id; + avp.back_navigation_edge = connection.edge; + avp.back_navigation_edge_pathway_start = connection.pathway_start; + avp.back_navigation_edge_pathway_end = connection.pathway_end; + avp.traveled_distance = new_distance; + avp.entry = new_entry; } } else { // Add the neighbour polygon to the reachable ones. @@ -479,15 +477,15 @@ void NavMap::add_region(NavRegion *p_region) { } void NavMap::remove_region(NavRegion *p_region) { - const std::vector::iterator it = std::find(regions.begin(), regions.end(), p_region); - if (it != regions.end()) { - regions.erase(it); + int64_t region_index = regions.find(p_region); + if (region_index != -1) { + regions.remove_unordered(region_index); regenerate_links = true; } } bool NavMap::has_agent(RvoAgent *agent) const { - return std::find(agents.begin(), agents.end(), agent) != agents.end(); + return (agents.find(agent) != -1); } void NavMap::add_agent(RvoAgent *agent) { @@ -499,15 +497,18 @@ void NavMap::add_agent(RvoAgent *agent) { void NavMap::remove_agent(RvoAgent *agent) { remove_agent_as_controlled(agent); - const std::vector::iterator it = std::find(agents.begin(), agents.end(), agent); - if (it != agents.end()) { - agents.erase(it); + + int64_t agent_index = agents.find(agent); + + if (agent_index != -1) { + agents.remove_unordered(agent_index); agents_dirty = true; } } void NavMap::set_agent_as_controlled(RvoAgent *agent) { - const bool exist = std::find(controlled_agents.begin(), controlled_agents.end(), agent) != controlled_agents.end(); + const bool exist = (controlled_agents.find(agent) != -1); + if (!exist) { ERR_FAIL_COND(!has_agent(agent)); controlled_agents.push_back(agent); @@ -515,22 +516,23 @@ void NavMap::set_agent_as_controlled(RvoAgent *agent) { } void NavMap::remove_agent_as_controlled(RvoAgent *agent) { - const std::vector::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent); - if (it != controlled_agents.end()) { - controlled_agents.erase(it); + int64_t active_avoidance_agent_index = controlled_agents.find(agent); + if (active_avoidance_agent_index != -1) { + controlled_agents.remove_unordered(active_avoidance_agent_index); + agents_dirty = true; } } void NavMap::sync() { // Check if we need to update the links. if (regenerate_polygons) { - for (size_t r(0); r < regions.size(); r++) { + for (uint32_t r = 0; r < regions.size(); r++) { regions[r]->scratch_polygons(); } regenerate_links = true; } - for (size_t r(0); r < regions.size(); r++) { + for (uint32_t r = 0; r < regions.size(); r++) { if (regions[r]->sync()) { regenerate_links = true; } @@ -538,13 +540,13 @@ void NavMap::sync() { if (regenerate_links) { // Remove regions connections. - for (size_t r(0); r < regions.size(); r++) { + for (uint32_t r = 0; r < regions.size(); r++) { regions[r]->get_connections().clear(); } // Resize the polygon count. int count = 0; - for (size_t r(0); r < regions.size(); r++) { + for (uint32_t r = 0; r < regions.size(); r++) { count += regions[r]->get_polygons().size(); } @@ -552,28 +554,31 @@ void NavMap::sync() { // Copy all region polygons in the map. count = 0; - for (size_t r(0); r < regions.size(); r++) { - std::copy( - regions[r]->get_polygons().data(), - regions[r]->get_polygons().data() + regions[r]->get_polygons().size(), - polygons.begin() + count); + for (uint32_t r = 0; r < regions.size(); r++) { + const LocalVector &polygons_source = regions[r]->get_polygons(); + for (uint32_t n = 0; n < polygons_source.size(); n++) { + polygons[count + n] = polygons_source[n]; + } count += regions[r]->get_polygons().size(); } // Group all edges per key. Map> connections; - for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { + + for (uint32_t poly_id = 0; poly_id < polygons.size(); poly_id++) { gd::Polygon &poly(polygons[poly_id]); - for (size_t p(0); p < poly.points.size(); p++) { + 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); Map>::Element *connection = connections.find(ek); + if (!connection) { connections[ek] = Vector(); } + if (connections[ek].size() <= 1) { // Add the polygon/edge tuple to this key. gd::Edge::Connection new_connection; @@ -674,6 +679,7 @@ void NavMap::sync() { // Update agents tree. if (agents_dirty) { + // cannot use LocalVector here as RVO library expects std::vector to build KdTree std::vector raw_agents; raw_agents.reserve(agents.size()); for (size_t i(0); i < agents.size(); i++) { @@ -703,7 +709,7 @@ void NavMap::step(real_t p_deltatime) { controlled_agents.size(), this, &NavMap::compute_single_step, - controlled_agents.data()); + controlled_agents.ptr()); } } @@ -713,7 +719,7 @@ void NavMap::dispatch_callbacks() { } } -void NavMap::clip_path(const std::vector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { +void NavMap::clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { Vector3 from = path[path.size() - 1]; if (from.is_equal_approx(p_to_point)) { diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index 81a77f10b..ca4620bfc 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -59,10 +59,10 @@ class NavMap : public NavRid { bool regenerate_polygons; bool regenerate_links; - std::vector regions; + LocalVector regions; /// Map polygons - std::vector polygons; + LocalVector polygons; /// Rvo world RVO::KdTree rvo; @@ -71,10 +71,10 @@ class NavMap : public NavRid { bool agents_dirty; /// All the Agents (even the controlled one) - std::vector agents; + LocalVector agents; /// Controlled agents - std::vector controlled_agents; + LocalVector controlled_agents; /// Physics delta time real_t deltatime; @@ -120,14 +120,14 @@ public: void add_region(NavRegion *p_region); void remove_region(NavRegion *p_region); - const std::vector &get_regions() const { + const LocalVector &get_regions() const { return regions; } bool has_agent(RvoAgent *agent) const; void add_agent(RvoAgent *agent); void remove_agent(RvoAgent *agent); - const std::vector &get_agents() const { + const LocalVector &get_agents() const { return agents; } @@ -144,7 +144,7 @@ public: private: void compute_single_step(uint32_t index, RvoAgent **agent); - void clip_path(const std::vector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const; + void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const; }; #endif // NAV_MAP_H diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h index 437f2f7ba..9508e2d18 100644 --- a/modules/navigation/nav_region.h +++ b/modules/navigation/nav_region.h @@ -53,7 +53,7 @@ class NavRegion : public NavRid { bool polygons_dirty; /// Cache - std::vector polygons; + LocalVector polygons; public: void scratch_polygons() { @@ -91,7 +91,7 @@ public: Vector3 get_connection_pathway_start(int p_connection_id) const; Vector3 get_connection_pathway_end(int p_connection_id) const; - std::vector const &get_polygons() const { + LocalVector const &get_polygons() const { return polygons; } diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h index ffd2535c9..38f9f4361 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -100,13 +100,13 @@ struct Polygon { NavRegion *owner; /// The points of this `Polygon` - std::vector points; + LocalVector points; /// Are the points clockwise ? bool clockwise; /// The edges of this `Polygon` - std::vector edges; + LocalVector edges; /// The center of this `Polygon` Vector3 center; @@ -132,6 +132,8 @@ struct NavigationPoly { /// The distance to the destination. float traveled_distance; + NavigationPoly() { poly = nullptr; } + NavigationPoly(const Polygon *p_poly) { self_id = 0; poly = p_poly; diff --git a/modules/navigation/pandemonium_navigation_server.cpp b/modules/navigation/pandemonium_navigation_server.cpp index 4b3012565..7a73f798d 100644 --- a/modules/navigation/pandemonium_navigation_server.cpp +++ b/modules/navigation/pandemonium_navigation_server.cpp @@ -267,8 +267,12 @@ Array PandemoniumNavigationServer::map_get_regions(RID p_map) const { Array regions_rids; const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == nullptr, regions_rids); - for (NavRegion *region : map->get_regions()) { - regions_rids.push_back(region->get_self()); + + const LocalVector regions = map->get_regions(); + regions_rids.resize(regions.size()); + + for (uint32_t i = 0; i < regions.size(); i++) { + regions_rids[i] = regions[i]->get_self(); } return regions_rids; } @@ -277,9 +281,14 @@ Array PandemoniumNavigationServer::map_get_agents(RID p_map) const { Array agents_rids; const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == nullptr, agents_rids); - for (RvoAgent *agent : map->get_agents()) { - agents_rids.push_back(agent->get_self()); + + const LocalVector agents = map->get_agents(); + agents_rids.resize(agents.size()); + + for (uint32_t i = 0; i < agents.size(); i++) { + agents_rids[i] = agents[i]->get_self(); } + return agents_rids; } @@ -561,15 +570,15 @@ COMMAND_1(free, RID, p_object) { NavMap *map = map_owner.getornull(p_object); // Removes any assigned region - std::vector regions = map->get_regions(); - for (size_t i(0); i < regions.size(); i++) { + LocalVector regions = map->get_regions(); + for (uint32_t i = 0; i < regions.size(); i++) { map->remove_region(regions[i]); regions[i]->set_map(nullptr); } // Remove any assigned agent - std::vector agents = map->get_agents(); - for (size_t i(0); i < agents.size(); i++) { + LocalVector agents = map->get_agents(); + for (uint32_t i = 0; i < agents.size(); i++) { map->remove_agent(agents[i]); agents[i]->set_map(nullptr); } diff --git a/modules/navigation/pandemonium_navigation_server.h b/modules/navigation/pandemonium_navigation_server.h index 7053dede7..b7718b489 100644 --- a/modules/navigation/pandemonium_navigation_server.h +++ b/modules/navigation/pandemonium_navigation_server.h @@ -66,7 +66,7 @@ class PandemoniumNavigationServer : public NavigationServer { /// Mutex used to make any operation threadsafe. Mutex operations_mutex; - std::vector commands; + LocalVector commands; mutable RID_Owner map_owner; mutable RID_Owner region_owner;