Replace Navigation std::vector use with LocalVector

Replace Navigation std::vector use with LocalVector.

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) {}