summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--modules/navigation/godot_navigation_server.cpp20
-rw-r--r--modules/navigation/godot_navigation_server.h2
-rw-r--r--modules/navigation/nav_map.cpp74
-rw-r--r--modules/navigation/nav_map.h14
-rw-r--r--modules/navigation/nav_region.h4
-rw-r--r--modules/navigation/nav_utils.h8
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) {}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-21 22:36:02 +0000