From 8d4922cfb15b211b48bf26e8335365c13ecfb8b5 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Thu, 28 Jul 2022 19:24:14 +0200 Subject: Replace Navigation std::vector use with LocalVector Replace Navigation std::vector use with LocalVector. --- modules/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 | 8 +-- 6 files changed, 64 insertions(+), 58 deletions(-) (limited to 'modules') diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp index bcbc721dbb..2cdb5b7cb4 100644 --- a/modules/navigation/godot_navigation_server.cpp +++ b/modules/navigation/godot_navigation_server.cpp @@ -249,8 +249,10 @@ Array GodotNavigationServer::map_get_regions(RID p_map) const { Array regions_rids; const NavMap *map = map_owner.get_or_null(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; } @@ -259,8 +261,10 @@ Array GodotNavigationServer::map_get_agents(RID p_map) const { Array agents_rids; const NavMap *map = map_owner.get_or_null(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; } @@ -539,15 +543,15 @@ COMMAND_1(free, RID, p_object) { NavMap *map = map_owner.get_or_null(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 8e7e99888c..da1f8cba0b 100644 --- a/modules/navigation/godot_navigation_server.h +++ b/modules/navigation/godot_navigation_server.h @@ -69,7 +69,7 @@ class GodotNavigationServer : public NavigationServer3D { /// 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 46daa54239..49029b5513 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -117,7 +117,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. @@ -170,20 +170,18 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p const Vector3 new_entry = Geometry3D::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. @@ -470,15 +468,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_at_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) { @@ -490,15 +488,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_at_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); @@ -506,22 +504,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_at_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; } @@ -529,33 +528,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. HashMap, gd::EdgeKey> 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); @@ -662,6 +661,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++) { @@ -683,7 +683,7 @@ void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) { void NavMap::step(real_t p_deltatime) { deltatime = p_deltatime; if (controlled_agents.size() > 0) { - WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_step, controlled_agents.data(), controlled_agents.size(), -1, true, SNAME("NavigationMapAgents")); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_step, controlled_agents.ptr(), controlled_agents.size(), -1, true, SNAME("NavigationMapAgents")); WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); } } @@ -694,7 +694,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 98a5c24b3e..e50a1afbe9 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -58,10 +58,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; @@ -70,10 +70,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; @@ -111,14 +111,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; } @@ -135,7 +135,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 484856ae36..c9d2d80f6c 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: NavRegion() {} @@ -93,7 +93,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 a9f4e0e2fc..47f04b6a75 100644 --- a/modules/navigation/nav_utils.h +++ b/modules/navigation/nav_utils.h @@ -34,7 +34,7 @@ #include "core/math/vector3.h" #include "core/templates/hash_map.h" #include "core/templates/hashfuncs.h" -#include "core/templates/vector.h" +#include "core/templates/local_vector.h" #include class NavRegion; @@ -96,13 +96,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; @@ -124,6 +124,8 @@ struct NavigationPoly { /// The distance to the destination. float traveled_distance = 0.0; + NavigationPoly() { poly = nullptr; } + NavigationPoly(const Polygon *p_poly) : poly(p_poly) {} -- cgit v1.2.3