Ported: [3.5] Replace Navigation std::vector use with LocalVector

Replace Navigation std::vector use with LocalVector.
- smix8
d0a78d05eb

modules/navigation/nav_map.cpp

@@ -126,7 +126,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.
@@ -179,20 +179,18 @@ Vector<Vector3> 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<gd::NavigationPoly>::iterator it = std::find(
+				int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));
-						navigation_polys.begin(),
-						navigation_polys.end(),
-						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) {
+					gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index];
-						it->back_navigation_poly_id = least_cost_id;
+					if (new_distance < avp.traveled_distance) {
-						it->back_navigation_edge = connection.edge;
+						avp.back_navigation_poly_id = least_cost_id;
-						it->back_navigation_edge_pathway_start = connection.pathway_start;
+						avp.back_navigation_edge = connection.edge;
-						it->back_navigation_edge_pathway_end = connection.pathway_end;
+						avp.back_navigation_edge_pathway_start = connection.pathway_start;
-						it->traveled_distance = new_distance;
+						avp.back_navigation_edge_pathway_end = connection.pathway_end;
-						it->entry = new_entry;
+						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<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
+	int64_t region_index = regions.find(p_region);
-	if (it != regions.end()) {
+	if (region_index != -1) {
-		regions.erase(it);
+		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<RvoAgent *>::iterator it = std::find(agents.begin(), agents.end(), agent);
+
-	if (it != agents.end()) {
+	int64_t agent_index = agents.find(agent);
-		agents.erase(it);
+
+	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<RvoAgent *>::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
+	int64_t active_avoidance_agent_index = controlled_agents.find(agent);
-	if (it != controlled_agents.end()) {
+	if (active_avoidance_agent_index != -1) {
-		controlled_agents.erase(it);
+		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++) {
+		for (uint32_t r = 0; r < regions.size(); r++) {
-			std::copy(
+			const LocalVector<gd::Polygon> &polygons_source = regions[r]->get_polygons();
-					regions[r]->get_polygons().data(),
+			for (uint32_t n = 0; n < polygons_source.size(); n++) {
-					regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
+				polygons[count + n] = polygons_source[n];
-					polygons.begin() + count);
+			}
 
 			count += regions[r]->get_polygons().size();
 		}
 
 		// Group all edges per key.
 		Map<gd::EdgeKey, Vector<gd::Edge::Connection>> 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<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek);
+
 				if (!connection) {
 					connections[ek] = Vector<gd::Edge::Connection>();
 				}
+
 				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<RVO::Agent *> 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<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)) {

modules/navigation/nav_map.h

@@ -59,10 +59,10 @@ class NavMap : public NavRid {
 	bool regenerate_polygons;
 	bool regenerate_links;
 
-	std::vector<NavRegion *> regions;
+	LocalVector<NavRegion *> regions;
 
 	/// Map polygons
-	std::vector<gd::Polygon> polygons;
+	LocalVector<gd::Polygon> 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<RvoAgent *> agents;
+	LocalVector<RvoAgent *> agents;
 
 	/// Controlled agents
-	std::vector<RvoAgent *> controlled_agents;
+	LocalVector<RvoAgent *> 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<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;
 	}
 
@@ -144,7 +144,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

modules/navigation/nav_region.h

@@ -53,7 +53,7 @@ class NavRegion : public NavRid {
 	bool polygons_dirty;
 
 	/// Cache
-	std::vector<gd::Polygon> polygons;
+	LocalVector<gd::Polygon> 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<gd::Polygon> const &get_polygons() const {
+	LocalVector<gd::Polygon> const &get_polygons() const {
 		return polygons;
 	}
 

modules/navigation/nav_utils.h

@@ -100,13 +100,13 @@ struct Polygon {
 	NavRegion *owner;
 
 	/// 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;
@@ -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;

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<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;
 }
@@ -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<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;
 }
 
@@ -561,15 +570,15 @@ COMMAND_1(free, RID, p_object) {
 		NavMap *map = map_owner.getornull(p_object);
 
 		// Removes any assigned region
-		std::vector<NavRegion *> regions = map->get_regions();
+		LocalVector<NavRegion *> regions = map->get_regions();
-		for (size_t i(0); i < regions.size(); i++) {
+		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();
+		LocalVector<RvoAgent *> agents = map->get_agents();
-		for (size_t i(0); i < agents.size(); i++) {
+		for (uint32_t i = 0; i < agents.size(); i++) {
 			map->remove_agent(agents[i]);
 			agents[i]->set_map(nullptr);
 		}

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<SetCommand *> commands;
+	LocalVector<SetCommand *> commands;
 
 	mutable RID_Owner<NavMap> map_owner;
 	mutable RID_Owner<NavRegion> region_owner;