diff options
author | RĂ©mi Verschelde <remi@verschelde.fr> | 2022-07-29 09:42:19 +0200 |
---|---|---|
committer | GitHub <noreply@github.com> | 2022-07-29 09:42:19 +0200 |
commit | 0cf26fb8c8b3be9777815c134bf18c2b9d7e5700 (patch) | |
tree | 1b05e8eb5d87739e78c182db52838c761e28c48f /modules | |
parent | f821190747d486e8aeaa5438bcc40cf01e3931f9 (diff) | |
parent | 8d4922cfb15b211b48bf26e8335365c13ecfb8b5 (diff) |
Merge pull request #63584 from smix8/navigation_std_to_localvector_4.x
Diffstat (limited to 'modules')
-rw-r--r-- | modules/navigation/godot_navigation_server.cpp | 20 | ||||
-rw-r--r-- | modules/navigation/godot_navigation_server.h | 2 | ||||
-rw-r--r-- | modules/navigation/nav_map.cpp | 74 | ||||
-rw-r--r-- | modules/navigation/nav_map.h | 14 | ||||
-rw-r--r-- | modules/navigation/nav_region.h | 4 | ||||
-rw-r--r-- | modules/navigation/nav_utils.h | 8 |
6 files changed, 64 insertions, 58 deletions
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<NavRegion *> 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<RvoAgent *> 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<NavRegion *> regions = map->get_regions(); - for (size_t i(0); i < regions.size(); i++) { + LocalVector<NavRegion *> 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<RvoAgent *> agents = map->get_agents(); - for (size_t i(0); i < agents.size(); i++) { + LocalVector<RvoAgent *> 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<SetCommand *> commands; + LocalVector<SetCommand *> commands; mutable RID_Owner<NavMap> map_owner; mutable RID_Owner<NavRegion> 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<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } // List of all reachable navigation polys. - std::vector<gd::NavigationPoly> navigation_polys; + LocalVector<gd::NavigationPoly> navigation_polys; navigation_polys.reserve(polygons.size() * 0.75); // Add the start polygon to the reachable navigation polygons. @@ -170,20 +170,18 @@ Vector<Vector3> 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<gd::NavigationPoly>::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<NavRegion *>::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<RvoAgent *>::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<RvoAgent *>::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<gd::Polygon> &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, Vector<gd::Edge::Connection>, 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<RVO::Agent *> 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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { +void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &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<NavRegion *> regions; + LocalVector<NavRegion *> regions; /// Map polygons - std::vector<gd::Polygon> polygons; + LocalVector<gd::Polygon> 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<RvoAgent *> agents; + LocalVector<RvoAgent *> agents; /// Controlled agents - std::vector<RvoAgent *> controlled_agents; + LocalVector<RvoAgent *> 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<NavRegion *> &get_regions() const { + const LocalVector<NavRegion *> &get_regions() const { return regions; } bool has_agent(RvoAgent *agent) const; void add_agent(RvoAgent *agent); void remove_agent(RvoAgent *agent); - const std::vector<RvoAgent *> &get_agents() const { + const LocalVector<RvoAgent *> &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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const; + void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &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<gd::Polygon> polygons; + LocalVector<gd::Polygon> 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<gd::Polygon> const &get_polygons() const { + LocalVector<gd::Polygon> 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 <vector> class NavRegion; @@ -96,13 +96,13 @@ struct Polygon { NavRegion *owner = nullptr; /// The points of this `Polygon` - std::vector<Point> points; + LocalVector<Point> points; /// Are the points clockwise ? bool clockwise; /// The edges of this `Polygon` - std::vector<Edge> edges; + LocalVector<Edge> 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) {} |