summaryrefslogtreecommitdiff
path: root/modules/navigation/nav_map.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'modules/navigation/nav_map.cpp')
-rw-r--r--modules/navigation/nav_map.cpp113
1 files changed, 58 insertions, 55 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index cbc0adc574..49029b5513 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -30,9 +30,9 @@
#include "nav_map.h"
+#include "core/object/worker_thread_pool.h"
#include "nav_region.h"
#include "rvo_agent.h"
-
#include <algorithm>
#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
@@ -65,7 +65,7 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
return p;
}
-Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
+Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
// Find the start poly and the end poly on this map.
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
@@ -78,7 +78,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
const gd::Polygon &p = polygons[i];
// Only consider the polygon if it in a region with compatible layers.
- if ((p_layers & p.owner->get_layers()) == 0) {
+ if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) {
continue;
}
@@ -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.
@@ -140,6 +140,8 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
float reachable_d = 1e30;
bool is_reachable = true;
+ gd::NavigationPoly *prev_least_cost_poly = nullptr;
+
while (true) {
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
@@ -152,28 +154,34 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
const gd::Edge::Connection &connection = edge.connections[connection_index];
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
- if ((p_layers & connection.polygon->owner->get_layers()) == 0) {
+ if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
continue;
}
+ float region_enter_cost = 0.0;
+ float region_travel_cost = least_cost_poly->poly->owner->get_travel_cost();
+
+ if (prev_least_cost_poly != nullptr && !(prev_least_cost_poly->poly->owner->get_self() == least_cost_poly->poly->owner->get_self())) {
+ region_enter_cost = least_cost_poly->poly->owner->get_enter_cost();
+ }
+ prev_least_cost_poly = least_cost_poly;
+
Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
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) + least_cost_poly->traveled_distance;
+ 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.
@@ -238,7 +246,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
gd::NavigationPoly *np = &navigation_polys[element->get()];
float cost = np->traveled_distance;
- cost += np->entry.distance_to(end_point);
+ cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost());
if (cost < least_cost) {
least_cost_id = np->self_id;
least_cost = cost;
@@ -249,7 +257,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Stores the further reachable end polygon, in case our goal is not reachable.
if (is_reachable) {
- float d = navigation_polys[least_cost_id].entry.distance_to(p_destination);
+ float d = navigation_polys[least_cost_id].entry.distance_to(p_destination) * navigation_polys[least_cost_id].poly->owner->get_travel_cost();
if (reachable_d > d) {
reachable_d = d;
reachable_end = navigation_polys[least_cost_id].poly;
@@ -460,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) {
@@ -480,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);
@@ -496,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;
}
@@ -519,37 +528,37 @@ 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.
- Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
- for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
+ HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections;
+ 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);
+ HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey>::Iterator connection = connections.find(ek);
if (!connection) {
connections[ek] = Vector<gd::Edge::Connection>();
}
@@ -563,7 +572,7 @@ void NavMap::sync() {
connections[ek].push_back(new_connection);
} else {
// The edge is already connected with another edge, skip.
- ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
+ ERR_PRINT_ONCE("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problems.");
}
}
}
@@ -652,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++) {
@@ -673,14 +683,8 @@ 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) {
- if (step_work_pool.get_thread_count() == 0) {
- step_work_pool.init();
- }
- step_work_pool.do_work(
- controlled_agents.size(),
- this,
- &NavMap::compute_single_step,
- controlled_agents.data());
+ 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);
}
}
@@ -690,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)) {
@@ -726,5 +730,4 @@ NavMap::NavMap() {
}
NavMap::~NavMap() {
- step_work_pool.finish();
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-04-18 14:10:32 +0000