summaryrefslogtreecommitdiff
path: root/modules/navigation
diff options
context:
space:
mode:
Diffstat (limited to 'modules/navigation')
-rw-r--r--modules/navigation/godot_navigation_server.cpp32
-rw-r--r--modules/navigation/nav_base.h4
-rw-r--r--modules/navigation/nav_link.h4
-rw-r--r--modules/navigation/nav_map.cpp85
-rw-r--r--modules/navigation/nav_map.h4
-rw-r--r--modules/navigation/nav_region.h4
6 files changed, 118 insertions, 15 deletions
diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index c5c1912621..58a0982425 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -36,6 +36,8 @@
#include "navigation_mesh_generator.h"
#endif
+using namespace NavigationUtilities;
+
/// Creates a struct for each function and a function that once called creates
/// an instance of that struct with the submitted parameters.
/// Then, that struct is stored in an array; the `sync` function consume that array.
@@ -228,7 +230,7 @@ Vector<Vector3> GodotNavigationServer::map_get_path(RID p_map, Vector3 p_origin,
const NavMap *map = map_owner.get_or_null(p_map);
ERR_FAIL_COND_V(map == nullptr, Vector<Vector3>());
- return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers);
+ return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers, nullptr, nullptr, nullptr);
}
Vector3 GodotNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -838,20 +840,34 @@ void GodotNavigationServer::process(real_t p_delta_time) {
}
}
-NavigationUtilities::PathQueryResult GodotNavigationServer::_query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const {
- NavigationUtilities::PathQueryResult r_query_result;
+PathQueryResult GodotNavigationServer::_query_path(const PathQueryParameters &p_parameters) const {
+ PathQueryResult r_query_result;
const NavMap *map = map_owner.get_or_null(p_parameters.map);
ERR_FAIL_COND_V(map == nullptr, r_query_result);
// run the pathfinding
- if (p_parameters.pathfinding_algorithm == NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
+ if (p_parameters.pathfinding_algorithm == PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
// while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing
- if (p_parameters.path_postprocessing == NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
- r_query_result.path = map->get_path(p_parameters.start_position, p_parameters.target_position, true, p_parameters.navigation_layers);
- } else if (p_parameters.path_postprocessing == NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
- r_query_result.path = map->get_path(p_parameters.start_position, p_parameters.target_position, false, p_parameters.navigation_layers);
+ if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
+ r_query_result.path = map->get_path(
+ p_parameters.start_position,
+ p_parameters.target_position,
+ true,
+ p_parameters.navigation_layers,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
+ } else if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
+ r_query_result.path = map->get_path(
+ p_parameters.start_position,
+ p_parameters.target_position,
+ false,
+ p_parameters.navigation_layers,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
+ p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
}
} else {
return r_query_result;
diff --git a/modules/navigation/nav_base.h b/modules/navigation/nav_base.h
index f5d2880d36..a3ab77301b 100644
--- a/modules/navigation/nav_base.h
+++ b/modules/navigation/nav_base.h
@@ -33,6 +33,7 @@
#include "nav_rid.h"
#include "nav_utils.h"
+#include "servers/navigation/navigation_utilities.h"
class NavMap;
@@ -42,8 +43,11 @@ protected:
float enter_cost = 0.0;
float travel_cost = 1.0;
ObjectID owner_id;
+ NavigationUtilities::PathSegmentType type;
public:
+ NavigationUtilities::PathSegmentType get_type() const { return type; }
+
void set_navigation_layers(uint32_t p_navigation_layers) { navigation_layers = p_navigation_layers; }
uint32_t get_navigation_layers() const { return navigation_layers; }
diff --git a/modules/navigation/nav_link.h b/modules/navigation/nav_link.h
index 8f51a63951..e9c69286f9 100644
--- a/modules/navigation/nav_link.h
+++ b/modules/navigation/nav_link.h
@@ -43,6 +43,10 @@ class NavLink : public NavBase {
bool link_dirty = true;
public:
+ NavLink() {
+ type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+ }
+
void set_map(NavMap *p_map);
NavMap *get_map() const {
return map;
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 83862e1e34..8d58c9335d 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -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;
@@ -115,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;
@@ -296,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.
@@ -312,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;
@@ -320,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;
}
}
@@ -331,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;
@@ -339,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);
}
}
@@ -355,12 +401,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 +426,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;
}
@@ -837,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)) {
@@ -863,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);
}
}
}
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index a3da9fa727..3be13133d2 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -115,7 +115,7 @@ public:
gd::PointKey get_point_key(const Vector3 &p_pos) const;
- Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const;
+ Vector<Vector3> 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;
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
Vector3 get_closest_point(const Vector3 &p_point) const;
Vector3 get_closest_point_normal(const Vector3 &p_point) const;
@@ -154,7 +154,7 @@ public:
private:
void compute_single_step(uint32_t index, RvoAgent **agent);
- void 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 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;
};
#endif // NAV_MAP_H
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
index 8d2b5aa9eb..3ef3ef5748 100644
--- a/modules/navigation/nav_region.h
+++ b/modules/navigation/nav_region.h
@@ -48,7 +48,9 @@ class NavRegion : public NavBase {
LocalVector<gd::Polygon> polygons;
public:
- NavRegion() {}
+ NavRegion() {
+ type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
+ }
void scratch_polygons() {
polygons_dirty = true;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 01:35:25 +0000