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.cpp241
1 files changed, 170 insertions, 71 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 83862e1e34..d763b1d3bc 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -1,32 +1,32 @@
-/*************************************************************************/
-/* 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"
@@ -38,6 +38,18 @@
#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;
@@ -71,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;
@@ -80,9 +103,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
float begin_d = 1e20;
float end_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 (const gd::Polygon &p : polygons) {
// Only consider the polygon if it in a region with compatible layers.
if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) {
continue;
@@ -115,6 +136,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;
@@ -149,9 +188,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
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++) {
- const gd::Edge &edge = navigation_polys[least_cost_id].poly->edges[i];
-
+ for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
// 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];
@@ -188,7 +225,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
avp.entry = new_entry;
}
} else {
- // Add the neighbour polygon to the reachable ones.
+ // Add the neighbor polygon to the reachable ones.
gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon);
new_navigation_poly.self_id = navigation_polys.size();
new_navigation_poly.back_navigation_poly_id = least_cost_id;
@@ -199,7 +236,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
new_navigation_poly.entry = new_entry;
navigation_polys.push_back(new_navigation_poly);
- // Add the neighbour polygon to the polygons to visit.
+ // Add the neighbor polygon to the polygons to visit.
to_visit.push_back(navigation_polys.size() - 1);
}
}
@@ -296,6 +333,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.
@@ -312,7 +350,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;
@@ -320,7 +358,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;
}
}
@@ -331,7 +371,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;
@@ -339,7 +379,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);
}
}
@@ -355,12 +397,23 @@ 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;
@@ -369,18 +422,37 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
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;
}
@@ -389,9 +461,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
Vector3 closest_point;
real_t closest_point_d = 1e20;
- for (size_t i(0); i < polygons.size(); i++) {
- const gd::Polygon &p = polygons[i];
-
+ for (const gd::Polygon &p : polygons) {
// 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[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
@@ -535,54 +605,70 @@ void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
}
void NavMap::sync() {
+ // Performance Monitor
+ int _new_pm_region_count = regions.size();
+ int _new_pm_agent_count = agents.size();
+ int _new_pm_link_count = links.size();
+ int _new_pm_polygon_count = pm_polygon_count;
+ int _new_pm_edge_count = pm_edge_count;
+ int _new_pm_edge_merge_count = pm_edge_merge_count;
+ int _new_pm_edge_connection_count = pm_edge_connection_count;
+ int _new_pm_edge_free_count = pm_edge_free_count;
+
// Check if we need to update the links.
if (regenerate_polygons) {
- for (uint32_t r = 0; r < regions.size(); r++) {
- regions[r]->scratch_polygons();
+ for (NavRegion *region : regions) {
+ region->scratch_polygons();
}
regenerate_links = true;
}
- for (uint32_t r = 0; r < regions.size(); r++) {
- if (regions[r]->sync()) {
+ for (NavRegion *region : regions) {
+ if (region->sync()) {
regenerate_links = true;
}
}
- for (uint32_t l = 0; l < links.size(); l++) {
- if (links[l]->check_dirty()) {
+ for (NavLink *link : links) {
+ if (link->check_dirty()) {
regenerate_links = true;
}
}
if (regenerate_links) {
+ _new_pm_polygon_count = 0;
+ _new_pm_edge_count = 0;
+ _new_pm_edge_merge_count = 0;
+ _new_pm_edge_connection_count = 0;
+ _new_pm_edge_free_count = 0;
+
// Remove regions connections.
- for (uint32_t r = 0; r < regions.size(); r++) {
- regions[r]->get_connections().clear();
+ for (NavRegion *region : regions) {
+ region->get_connections().clear();
}
// Resize the polygon count.
int count = 0;
- for (uint32_t r = 0; r < regions.size(); r++) {
- count += regions[r]->get_polygons().size();
+ for (const NavRegion *region : regions) {
+ count += region->get_polygons().size();
}
polygons.resize(count);
// Copy all region polygons in the map.
count = 0;
- for (uint32_t r = 0; r < regions.size(); r++) {
- const LocalVector<gd::Polygon> &polygons_source = regions[r]->get_polygons();
+ for (const NavRegion *region : regions) {
+ const LocalVector<gd::Polygon> &polygons_source = region->get_polygons();
for (uint32_t n = 0; n < polygons_source.size(); n++) {
polygons[count + n] = polygons_source[n];
}
- count += regions[r]->get_polygons().size();
+ count += region->get_polygons().size();
}
+ _new_pm_polygon_count = polygons.size();
+
// Group all edges per key.
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 (gd::Polygon &poly : polygons) {
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);
@@ -590,6 +676,7 @@ void NavMap::sync() {
HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey>::Iterator connection = connections.find(ek);
if (!connection) {
connections[ek] = Vector<gd::Edge::Connection>();
+ _new_pm_edge_count += 1;
}
if (connections[ek].size() <= 1) {
// Add the polygon/edge tuple to this key.
@@ -615,6 +702,7 @@ void NavMap::sync() {
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.
+ _new_pm_edge_merge_count += 1;
} else {
CRASH_COND_MSG(E.value.size() != 1, vformat("Number of connection != 1. Found: %d", E.value.size()));
free_edges.push_back(E.value[0]);
@@ -628,6 +716,8 @@ void NavMap::sync() {
// to be connected, create new polygons to remove that small gap is
// not really useful and would result in wasteful computation during
// connection, integration and path finding.
+ _new_pm_edge_free_count = free_edges.size();
+
for (int i = 0; i < free_edges.size(); i++) {
const gd::Edge::Connection &free_edge = free_edges[i];
Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
@@ -681,6 +771,7 @@ void NavMap::sync() {
// Add the connection to the region_connection map.
((NavRegion *)free_edge.polygon->owner)->get_connections().push_back(new_connection);
+ _new_pm_edge_connection_count += 1;
}
}
@@ -688,10 +779,9 @@ void NavMap::sync() {
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();
+ for (const NavLink *link : links) {
+ const Vector3 start = link->get_start_position();
+ const Vector3 end = link->get_end_position();
gd::Polygon *closest_start_polygon = nullptr;
real_t closest_start_distance = link_connection_radius;
@@ -721,9 +811,7 @@ void NavMap::sync() {
}
// 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 (gd::Polygon &end_poly : polygons) {
// 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);
@@ -807,8 +895,8 @@ void NavMap::sync() {
// 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++) {
- raw_agents.push_back(agents[i]->get_agent());
+ for (RvoAgent *agent : agents) {
+ raw_agents.push_back(agent->get_agent());
}
rvo.buildAgentTree(raw_agents);
}
@@ -816,6 +904,16 @@ void NavMap::sync() {
regenerate_polygons = false;
regenerate_links = false;
agents_dirty = false;
+
+ // Performance Monitor
+ pm_region_count = _new_pm_region_count;
+ pm_agent_count = _new_pm_agent_count;
+ pm_link_count = _new_pm_link_count;
+ pm_polygon_count = _new_pm_polygon_count;
+ pm_edge_count = _new_pm_edge_count;
+ pm_edge_merge_count = _new_pm_edge_merge_count;
+ pm_edge_connection_count = _new_pm_edge_connection_count;
+ pm_edge_free_count = _new_pm_edge_free_count;
}
void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) {
@@ -832,12 +930,12 @@ void NavMap::step(real_t p_deltatime) {
}
void NavMap::dispatch_callbacks() {
- for (int i(0); i < static_cast<int>(controlled_agents.size()); i++) {
- controlled_agents[i]->dispatch_callback();
+ for (RvoAgent *agent : controlled_agents) {
+ agent->dispatch_callback();
}
}
-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)) {
@@ -863,6 +961,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-04-06 00:40:55 +0000