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.cpp320
1 files changed, 270 insertions, 50 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 49029b5513..2a2f8aa1b7 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -1,42 +1,55 @@
-/*************************************************************************/
-/* nav_map.cpp */
-/*************************************************************************/
-/* This file is part of: */
-/* GODOT ENGINE */
-/* https://godotengine.org */
-/*************************************************************************/
-/* 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 */
-/* "Software"), to deal in the Software without restriction, including */
-/* without limitation the rights to use, copy, modify, merge, publish, */
-/* distribute, sublicense, and/or sell copies of the Software, and to */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
-/*************************************************************************/
+/**************************************************************************/
+/* nav_map.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
#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)))
+// Helper macro
+#define APPEND_METADATA(poly) \
+ if (r_path_types) { \
+ r_path_types->push_back(poly->owner->get_type()); \
+ } \
+ if (r_path_rids) { \
+ r_path_rids->push_back(poly->owner->get_self()); \
+ } \
+ if (r_path_owners) { \
+ r_path_owners->push_back(poly->owner->get_owner_id()); \
+ }
+
void NavMap::set_up(Vector3 p_up) {
up = p_up;
regenerate_polygons = true;
@@ -52,6 +65,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 +83,18 @@ 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_navigation_layers) const {
+Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
+ // Clear metadata outputs.
+ if (r_path_types) {
+ r_path_types->clear();
+ }
+ if (r_path_rids) {
+ r_path_rids->clear();
+ }
+ if (r_path_owners) {
+ r_path_owners->clear();
+ }
+
// Find the start poly and the end poly on this map.
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
@@ -109,6 +138,24 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
return Vector<Vector3>();
}
if (begin_poly == end_poly) {
+ if (r_path_types) {
+ r_path_types->resize(2);
+ r_path_types->write[0] = begin_poly->owner->get_type();
+ r_path_types->write[1] = end_poly->owner->get_type();
+ }
+
+ if (r_path_rids) {
+ r_path_rids->resize(2);
+ (*r_path_rids)[0] = begin_poly->owner->get_self();
+ (*r_path_rids)[1] = end_poly->owner->get_self();
+ }
+
+ if (r_path_owners) {
+ r_path_owners->resize(2);
+ r_path_owners->write[0] = begin_poly->owner->get_owner_id();
+ r_path_owners->write[1] = end_poly->owner->get_owner_id();
+ }
+
Vector<Vector3> path;
path.resize(2);
path.write[0] = begin_point;
@@ -134,20 +181,17 @@ 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++) {
@@ -158,17 +202,18 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
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;
int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));
@@ -234,6 +279,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;
@@ -291,6 +337,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
gd::NavigationPoly *p = apex_poly;
path.push_back(end_point);
+ APPEND_METADATA(end_poly);
while (p) {
// Set left and right points of the pathway between polygons.
@@ -307,7 +354,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
left_poly = p;
left_portal = left;
} else {
- clip_path(navigation_polys, path, apex_poly, right_portal, right_poly);
+ clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners);
apex_point = right_portal;
p = right_poly;
@@ -315,7 +362,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
apex_poly = p;
left_portal = apex_point;
right_portal = apex_point;
+
path.push_back(apex_point);
+ APPEND_METADATA(apex_poly->poly);
skip = true;
}
}
@@ -326,7 +375,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
right_poly = p;
right_portal = right;
} else {
- clip_path(navigation_polys, path, apex_poly, left_portal, left_poly);
+ clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners);
apex_point = left_portal;
p = left_poly;
@@ -334,7 +383,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
apex_poly = p;
right_portal = apex_point;
left_portal = apex_point;
+
path.push_back(apex_point);
+ APPEND_METADATA(apex_poly->poly);
}
}
@@ -350,27 +401,62 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// If the last point is not the begin point, add it to the list.
if (path[path.size() - 1] != begin_point) {
path.push_back(begin_point);
+ APPEND_METADATA(begin_poly);
}
path.reverse();
+ if (r_path_types) {
+ r_path_types->reverse();
+ }
+ if (r_path_rids) {
+ r_path_rids->reverse();
+ }
+ if (r_path_owners) {
+ r_path_owners->reverse();
+ }
} else {
path.push_back(end_point);
+ APPEND_METADATA(end_poly);
// 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);
+ APPEND_METADATA(navigation_polys[np_id].poly);
+ } else {
+ path.push_back(navigation_polys[np_id].entry);
+ APPEND_METADATA(navigation_polys[np_id].poly);
+ }
+
np_id = navigation_polys[np_id].back_navigation_poly_id;
}
path.push_back(begin_point);
+ APPEND_METADATA(begin_poly);
+
path.reverse();
+ if (r_path_types) {
+ r_path_types->reverse();
+ }
+ if (r_path_rids) {
+ r_path_rids->reverse();
+ }
+ if (r_path_owners) {
+ r_path_owners->reverse();
+ }
}
+ // Ensure post conditions (path arrays MUST match in size).
+ CRASH_COND(r_path_types && path.size() != r_path_types->size());
+ CRASH_COND(r_path_rids && path.size() != r_path_rids->size());
+ CRASH_COND(r_path_owners && path.size() != r_path_owners->size());
+
return path;
}
@@ -475,6 +561,19 @@ void NavMap::remove_region(NavRegion *p_region) {
}
}
+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 (agents.find(agent) != -1);
}
@@ -526,6 +625,12 @@ void NavMap::sync() {
}
}
+ for (uint32_t l = 0; l < links.size(); l++) {
+ if (links[l]->check_dirty()) {
+ regenerate_links = true;
+ }
+ }
+
if (regenerate_links) {
// Remove regions connections.
for (uint32_t r = 0; r < regions.size(); r++) {
@@ -651,7 +756,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);
+ }
}
}
@@ -694,7 +913,7 @@ void NavMap::dispatch_callbacks() {
}
}
-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 {
+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, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
Vector3 from = path[path.size() - 1];
if (from.is_equal_approx(p_to_point)) {
@@ -720,6 +939,7 @@ void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys
if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) {
path.push_back(inters);
+ APPEND_METADATA(from_poly->poly);
}
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-27 13:58:42 +0000