From d0a78d05eb2c5c845adbc7e7b3de3d4f52af8774 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Fri, 29 Jul 2022 11:13:20 +0200 Subject: [PATCH] [3.5] Replace Navigation std::vector use with LocalVector Replace Navigation std::vector use with LocalVector. --- .../navigation/godot_navigation_server.cpp | 20 +++-- modules/navigation/godot_navigation_server.h | 2 +- modules/navigation/nav_map.cpp | 74 +++++++++---------- modules/navigation/nav_map.h | 14 ++-- modules/navigation/nav_region.h | 4 +- modules/navigation/nav_utils.h | 6 +- 6 files changed, 63 insertions(+), 57 deletions(-) diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp index 8b90ccfeb44..ffbe6d36501 100644 --- a/modules/navigation/godot_navigation_server.cpp +++ b/modules/navigation/godot_navigation_server.cpp @@ -263,8 +263,10 @@ Array GodotNavigationServer::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; } @@ -273,8 +275,10 @@ Array GodotNavigationServer::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; } @@ -553,15 +557,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/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h index faf76817a90..bd20ae863e8 100644 --- a/modules/navigation/godot_navigation_server.h +++ b/modules/navigation/godot_navigation_server.h @@ -67,7 +67,7 @@ class GodotNavigationServer : 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; diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index d961795b1af..4fae3e16e84 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -124,7 +124,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. @@ -177,20 +177,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. @@ -477,15 +475,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) { @@ -497,15 +495,15 @@ 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); @@ -513,22 +511,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; } @@ -536,33 +535,33 @@ 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(); } polygons.resize(count); // 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); @@ -669,6 +668,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++) { @@ -697,7 +697,7 @@ void NavMap::step(real_t p_deltatime) { controlled_agents.size(), this, &NavMap::compute_single_step, - controlled_agents.data()); + controlled_agents.ptr()); } } @@ -707,7 +707,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 4377a4c4882..d315aaeeda2 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -59,10 +59,10 @@ class NavMap : public NavRid { bool regenerate_polygons = true; bool regenerate_links = true; - 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 = false; /// 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 = 0.0; @@ -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 14318f07acc..021d8fdba11 100644 --- a/modules/navigation/nav_region.h +++ b/modules/navigation/nav_region.h @@ -53,7 +53,7 @@ class NavRegion : public NavRid { bool polygons_dirty = true; /// 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 d348730593c..fc1ac949710 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -90,13 +90,13 @@ struct Polygon { NavRegion *owner = nullptr; /// 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; @@ -118,6 +118,8 @@ struct NavigationPoly { /// The distance to the destination. float traveled_distance = 0.0; + NavigationPoly() { poly = nullptr; } + NavigationPoly(const Polygon *p_poly) : poly(p_poly) {}