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.cpp253
1 files changed, 111 insertions, 142 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 3150ca0bc8..49029b5513 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -5,8 +5,8 @@
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
-/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -30,16 +30,11 @@
#include "nav_map.h"
-#include "core/os/threaded_array_processor.h"
+#include "core/object/worker_thread_pool.h"
#include "nav_region.h"
#include "rvo_agent.h"
-
#include <algorithm>
-/**
- @author AndreaCatania
-*/
-
#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
void NavMap::set_up(Vector3 p_up) {
@@ -70,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;
@@ -83,16 +78,13 @@ 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;
}
- // For each point cast a face and check the distance between the origin/destination
- for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
- const Vector3 p1 = p.points[point_id].pos;
- const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
- const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
- const Face3 face(p1, p2, p3);
+ // For each face check the distance between the origin/destination
+ for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+ const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 point = face.get_closest_point_to(p_origin);
float distance_to_point = point.distance_to(p_origin);
@@ -125,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.
@@ -148,11 +140,13 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
float reachable_d = 1e30;
bool is_reachable = true;
- while (true) {
- gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+ 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 < least_cost_poly->poly->edges.size(); i++) {
+ 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];
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
@@ -160,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.
@@ -218,7 +218,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
end_poly = reachable_end;
end_d = 1e20;
for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
- Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+ Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
Vector3 spoint = f.get_closest_point_to(p_destination);
float dpoint = spoint.distance_to(p_destination);
if (dpoint < end_d) {
@@ -233,6 +233,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
navigation_polys.push_back(np);
to_visit.clear();
to_visit.push_back(0);
+ least_cost_id = 0;
reachable_end = nullptr;
@@ -245,24 +246,24 @@ 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;
}
}
+ ERR_BREAK(least_cost_id == -1);
+
// 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;
}
}
- ERR_BREAK(least_cost_id == -1);
-
// Check if we reached the end
if (navigation_polys[least_cost_id].poly == end_poly) {
found_route = true;
@@ -358,11 +359,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) {
- path.push_back(navigation_polys[np_id].entry);
+ 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);
np_id = navigation_polys[np_id].back_navigation_poly_id;
}
+ path.push_back(begin_point);
path.reverse();
}
@@ -374,13 +379,12 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
Vector3 closest_point;
real_t closest_point_d = 1e20;
- // Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
- // For each point cast a face and check the distance to the segment
+ // For each face check the distance to the segment
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 inters;
if (f.intersects_segment(p_from, p_to, &inters)) {
const real_t d = closest_point_d = p_from.distance_to(inters);
@@ -420,82 +424,42 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
}
Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
-
- Vector3 closest_point;
- real_t closest_point_d = 1e20;
-
- // Find the initial poly and the end poly on this map.
- for (size_t i(0); i < polygons.size(); i++) {
- const gd::Polygon &p = polygons[i];
-
- // For each point cast a face and check the distance to the point
- for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
- const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_d = d;
- }
- }
- }
-
- return closest_point;
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.point;
}
Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
-
- Vector3 closest_point;
- Vector3 closest_point_normal;
- real_t closest_point_d = 1e20;
-
- // Find the initial poly and the end poly on this map.
- for (size_t i(0); i < polygons.size(); i++) {
- const gd::Polygon &p = polygons[i];
-
- // For each point cast a face and check the distance to the point
- for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
- const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_normal = f.get_plane().normal;
- closest_point_d = d;
- }
- }
- }
-
- return closest_point_normal;
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.normal;
}
RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.owner;
+}
- Vector3 closest_point;
- RID closest_point_owner;
- real_t closest_point_d = 1e20;
+gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
+ gd::ClosestPointQueryResult result;
+ real_t closest_point_ds = 1e20;
- // Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
- // For each point cast a face and check the distance to the point
+ // For each face check the distance to the point
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_owner = p.owner->get_self();
- closest_point_d = d;
+ const real_t ds = inters.distance_squared_to(p_point);
+ if (ds < closest_point_ds) {
+ result.point = inters;
+ result.normal = f.get_plane().normal;
+ result.owner = p.owner->get_self();
+ closest_point_ds = ds;
}
}
}
- return closest_point_owner;
+ return result;
}
void NavMap::add_region(NavRegion *p_region) {
@@ -504,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) {
@@ -524,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);
@@ -540,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;
}
@@ -563,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>();
}
@@ -607,23 +572,23 @@ 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.");
}
}
}
Vector<gd::Edge::Connection> free_edges;
- for (Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *E = connections.front(); E; E = E->next()) {
- if (E->get().size() == 2) {
+ for (KeyValue<gd::EdgeKey, Vector<gd::Edge::Connection>> &E : connections) {
+ if (E.value.size() == 2) {
// Connect edge that are shared in different polygons.
- gd::Edge::Connection &c1 = E->get().write[0];
- gd::Edge::Connection &c2 = E->get().write[1];
+ gd::Edge::Connection &c1 = E.value.write[0];
+ gd::Edge::Connection &c2 = E.value.write[1];
c1.polygon->edges[c1.edge].connections.push_back(c2);
c2.polygon->edges[c2.edge].connections.push_back(c1);
// Note: The pathway_start/end are full for those connection and do not need to be modified.
} else {
- CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size()));
- free_edges.push_back(E->get()[0]);
+ CRASH_COND_MSG(E.value.size() != 1, vformat("Number of connection != 1. Found: %d", E.value.size()));
+ free_edges.push_back(E.value[0]);
}
}
@@ -664,7 +629,7 @@ void NavMap::sync() {
} else {
other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
}
- if ((self1 - other1).length() > edge_connection_margin) {
+ if (other1.distance_to(self1) > edge_connection_margin) {
continue;
}
@@ -675,7 +640,7 @@ void NavMap::sync() {
} else {
other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
}
- if ((self2 - other2).length() > edge_connection_margin) {
+ if (other2.distance_to(self2) > edge_connection_margin) {
continue;
}
@@ -696,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++) {
@@ -717,11 +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) {
- thread_process_array(
- 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);
}
}
@@ -731,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)) {
@@ -762,3 +725,9 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
}
}
}
+
+NavMap::NavMap() {
+}
+
+NavMap::~NavMap() {
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-04-15 11:12:00 +0000