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.cpp270
1 files changed, 203 insertions, 67 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 059dc989a8..83862e1e34 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -30,9 +30,10 @@
#include "nav_map.h"
+#include "core/object/worker_thread_pool.h"
+#include "nav_link.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)))
@@ -52,6 +53,11 @@ void NavMap::set_edge_connection_margin(float p_edge_connection_margin) {
regenerate_links = true;
}
+void NavMap::set_link_connection_radius(float p_link_connection_radius) {
+ link_connection_radius = p_link_connection_radius;
+ regenerate_links = true;
+}
+
gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
const int x = int(Math::floor(p_pos.x / cell_size));
const int y = int(Math::floor(p_pos.y / cell_size));
@@ -65,7 +71,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 +84,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 +123,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.
@@ -134,56 +140,52 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// This is an implementation of the A* algorithm.
int least_cost_id = 0;
+ int prev_least_cost_id = -1;
bool found_route = false;
const gd::Polygon *reachable_end = nullptr;
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++) {
- gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
-
- const gd::Edge &edge = least_cost_poly->poly->edges[i];
+ const gd::Edge &edge = navigation_polys[least_cost_id].poly->edges[i];
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
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();
+ const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
+ float poly_enter_cost = 0.0;
+ float poly_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();
+ if (prev_least_cost_id != -1 && (navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self())) {
+ poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
}
- prev_least_cost_poly = least_cost_poly;
+ prev_least_cost_id = least_cost_id;
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) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance;
+ 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) * poly_travel_cost) + poly_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.
@@ -236,6 +238,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
to_visit.clear();
to_visit.push_back(0);
least_cost_id = 0;
+ prev_least_cost_id = -1;
reachable_end = nullptr;
@@ -362,10 +365,15 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Add mid points
int np_id = least_cost_id;
while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
- int prev = navigation_polys[np_id].back_navigation_edge;
- int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
- Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
- path.push_back(point);
+ if (navigation_polys[np_id].back_navigation_edge != -1) {
+ int prev = navigation_polys[np_id].back_navigation_edge;
+ int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
+ Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+ path.push_back(point);
+ } else {
+ path.push_back(navigation_polys[np_id].entry);
+ }
+
np_id = navigation_polys[np_id].back_navigation_poly_id;
}
@@ -470,15 +478,28 @@ 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;
+ }
+}
+
+void NavMap::add_link(NavLink *p_link) {
+ links.push_back(p_link);
+ regenerate_links = true;
+}
+
+void NavMap::remove_link(NavLink *p_link) {
+ int64_t link_index = links.find(p_link);
+ if (link_index != -1) {
+ links.remove_at_unordered(link_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 +511,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,56 +527,63 @@ 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;
}
}
+ for (uint32_t l = 0; l < links.size(); l++) {
+ if (links[l]->check_dirty()) {
+ regenerate_links = true;
+ }
+ }
+
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);
@@ -573,7 +601,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,7 +680,121 @@ void NavMap::sync() {
free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
// Add the connection to the region_connection map.
- free_edge.polygon->owner->get_connections().push_back(new_connection);
+ ((NavRegion *)free_edge.polygon->owner)->get_connections().push_back(new_connection);
+ }
+ }
+
+ uint32_t link_poly_idx = 0;
+ link_polygons.resize(links.size());
+
+ // Search for polygons within range of a nav link.
+ for (uint32_t l = 0; l < links.size(); l++) {
+ const NavLink *link = links[l];
+ const Vector3 start = link->get_start_location();
+ const Vector3 end = link->get_end_location();
+
+ gd::Polygon *closest_start_polygon = nullptr;
+ real_t closest_start_distance = link_connection_radius;
+ Vector3 closest_start_point;
+
+ gd::Polygon *closest_end_polygon = nullptr;
+ real_t closest_end_distance = link_connection_radius;
+ Vector3 closest_end_point;
+
+ // Create link to any polygons within the search radius of the start point.
+ for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) {
+ gd::Polygon &start_poly = polygons[start_index];
+
+ // For each face check the distance to the start
+ for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) {
+ const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos);
+ const Vector3 start_point = start_face.get_closest_point_to(start);
+ const real_t start_distance = start_point.distance_to(start);
+
+ // Pick the polygon that is within our radius and is closer than anything we've seen yet.
+ if (start_distance <= link_connection_radius && start_distance < closest_start_distance) {
+ closest_start_distance = start_distance;
+ closest_start_point = start_point;
+ closest_start_polygon = &start_poly;
+ }
+ }
+ }
+
+ // Find any polygons within the search radius of the end point.
+ for (uint32_t end_index = 0; end_index < polygons.size(); end_index++) {
+ gd::Polygon &end_poly = polygons[end_index];
+
+ // For each face check the distance to the end
+ for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) {
+ const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos);
+ const Vector3 end_point = end_face.get_closest_point_to(end);
+ const real_t end_distance = end_point.distance_to(end);
+
+ // Pick the polygon that is within our radius and is closer than anything we've seen yet.
+ if (end_distance <= link_connection_radius && end_distance < closest_end_distance) {
+ closest_end_distance = end_distance;
+ closest_end_point = end_point;
+ closest_end_polygon = &end_poly;
+ }
+ }
+ }
+
+ // If we have both a start and end point, then create a synthetic polygon to route through.
+ if (closest_start_polygon && closest_end_polygon) {
+ gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
+ new_polygon.owner = link;
+
+ new_polygon.edges.clear();
+ new_polygon.edges.resize(4);
+ new_polygon.points.clear();
+ new_polygon.points.reserve(4);
+
+ // Build a set of vertices that create a thin polygon going from the start to the end point.
+ new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
+ new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
+ new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
+ new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
+
+ Vector3 center;
+ for (int p = 0; p < 4; ++p) {
+ center += new_polygon.points[p].pos;
+ }
+ new_polygon.center = center / real_t(new_polygon.points.size());
+ new_polygon.clockwise = true;
+
+ // Setup connections to go forward in the link.
+ {
+ gd::Edge::Connection entry_connection;
+ entry_connection.polygon = &new_polygon;
+ entry_connection.edge = -1;
+ entry_connection.pathway_start = new_polygon.points[0].pos;
+ entry_connection.pathway_end = new_polygon.points[1].pos;
+ closest_start_polygon->edges[0].connections.push_back(entry_connection);
+
+ gd::Edge::Connection exit_connection;
+ exit_connection.polygon = closest_end_polygon;
+ exit_connection.edge = -1;
+ exit_connection.pathway_start = new_polygon.points[2].pos;
+ exit_connection.pathway_end = new_polygon.points[3].pos;
+ new_polygon.edges[2].connections.push_back(exit_connection);
+ }
+
+ // If the link is bi-directional, create connections from the end to the start.
+ if (link->is_bidirectional()) {
+ gd::Edge::Connection entry_connection;
+ entry_connection.polygon = &new_polygon;
+ entry_connection.edge = -1;
+ entry_connection.pathway_start = new_polygon.points[2].pos;
+ entry_connection.pathway_end = new_polygon.points[3].pos;
+ closest_end_polygon->edges[0].connections.push_back(entry_connection);
+
+ gd::Edge::Connection exit_connection;
+ exit_connection.polygon = closest_start_polygon;
+ exit_connection.edge = -1;
+ exit_connection.pathway_start = new_polygon.points[0].pos;
+ exit_connection.pathway_end = new_polygon.points[1].pos;
+ new_polygon.edges[0].connections.push_back(exit_connection);
+ }
}
}
@@ -662,6 +804,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,14 +826,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);
}
}
@@ -700,7 +837,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)) {
@@ -736,5 +873,4 @@ NavMap::NavMap() {
}
NavMap::~NavMap() {
- step_work_pool.finish();
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-04-15 21:03:09 +0000