diff --git a/doc/classes/NavigationAgent.xml b/doc/classes/NavigationAgent.xml index 6bf9e20e0..9a378a58b 100644 --- a/doc/classes/NavigationAgent.xml +++ b/doc/classes/NavigationAgent.xml @@ -17,6 +17,12 @@ Returns the distance to the target position, using the agent's global position. The user must set [member target_position] in order for this to be accurate. + + + + Returns the path query result for the path the agent is currently following. + + @@ -154,6 +160,9 @@ The maximum distance the agent is allowed away from the ideal path to the final position. This can happen due to trying to avoid collisions. When the maximum distance is exceeded, it recalculates the ideal path. + + Additional information to return with the navigation path. + The radius of the avoidance agent. This is the "body" of the avoidance agent and not the avoidance maneuver starting radius (which is controlled by [member neighbor_dist]). Does not affect normal pathfinding. To change an actor's pathfinding radius bake [NavigationMesh] resources with a different [member NavigationMesh.agent_radius] property and use different navigation maps for each actor size. @@ -169,6 +178,17 @@ + + + + Notifies when a navigation link has been reached. + The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: + - [code]location[/code]: The start location of the link that was reached. + - [code]type[/code]: Always [constant NavigationPathQueryResult3D.PATH_SEGMENT_TYPE_LINK]. + - [code]rid[/code]: The [RID] of the link. + - [code]owner[/code]: The object which manages the link (usually [NavigationLink3D]). + + Notifies when the final position is reached. @@ -190,6 +210,17 @@ Notifies when the collision avoidance velocity is calculated after a call to [method set_velocity]. Only emitted when [member avoidance_enabled] is true. + + + + Notifies when a waypoint along the path has been reached. + The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: + - [code]location[/code]: The location of the waypoint that was reached. + - [code]type[/code]: The type of navigation primitive (region or link) that contains this waypoint. + - [code]rid[/code]: The [RID] of the containing navigation primitive (region or link). + - [code]owner[/code]: The object which manages the containing navigation primitive (region or link). + + diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml index 46ff8407a..10b33de72 100644 --- a/doc/classes/NavigationAgent2D.xml +++ b/doc/classes/NavigationAgent2D.xml @@ -17,6 +17,12 @@ Returns the distance to the target position, using the agent's global position. The user must set [member target_position] in order for this to be accurate. + + + + Returns the path query result for the path the agent is currently following. + + @@ -151,6 +157,9 @@ The maximum distance the agent is allowed away from the ideal path to the final position. This can happen due to trying to avoid collisions. When the maximum distance is exceeded, it recalculates the ideal path. + + Additional information to return with the navigation path. + The radius of the avoidance agent. This is the "body" of the avoidance agent and not the avoidance maneuver starting radius (which is controlled by [member neighbor_dist]). Does not affect normal pathfinding. To change an actor's pathfinding radius bake [NavigationMesh] resources with a different [member NavigationMesh.agent_radius] property and use different navigation maps for each actor size. @@ -166,6 +175,17 @@ + + + + Notifies when a navigation link has been reached. + The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: + - [code]location[/code]: The start location of the link that was reached. + - [code]type[/code]: Always [constant NavigationPathQueryResult2D.PATH_SEGMENT_TYPE_LINK]. + - [code]rid[/code]: The [RID] of the link. + - [code]owner[/code]: The object which manages the link (usually [NavigationLink2D]). + + Notifies when the final position is reached. @@ -187,6 +207,17 @@ Notifies when the collision avoidance velocity is calculated after a call to [method set_velocity]. Only emitted when [member avoidance_enabled] is true. + + + + Notifies when a waypoint along the path has been reached. + The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: + - [code]location[/code]: The location of the waypoint that was reached. + - [code]type[/code]: The type of navigation primitive (region or link) that contains this waypoint. + - [code]rid[/code]: The [RID] of the containing navigation primitive (region or link). + - [code]owner[/code]: The object which manages the containing navigation primitive (region or link). + + diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml index a17d560bd..5c5854efb 100644 --- a/doc/classes/NavigationPathQueryParameters2D.xml +++ b/doc/classes/NavigationPathQueryParameters2D.xml @@ -16,6 +16,9 @@ Additional information to include with the navigation path. + + Additional information to include with the navigation path. + The navigation layers the query will use (as a bitmask). @@ -57,5 +60,20 @@ Include all available metadata about the returned path. + + Don't include any additional metadata about the returned path. + + + Include the type of navigation primitive (region or link) that each point of the path goes through. + + + Include the [RID]s of the regions and links that each point of the path goes through. + + + Include the [code]ObjectID[/code]s of the [Object]s which manage the regions and links each point of the path goes through. + + + Include all available metadata about the returned path. + diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml index 8c5155a6b..29e2b821d 100644 --- a/doc/classes/NavigationPathQueryParameters3D.xml +++ b/doc/classes/NavigationPathQueryParameters3D.xml @@ -57,5 +57,20 @@ Include all available metadata about the returned path. + + Don't include any additional metadata about the returned path. + + + Include the type of navigation primitive (region or link) that each point of the path goes through. + + + Include the [RID]s of the regions and links that each point of the path goes through. + + + Include the [code]ObjectID[/code]s of the [Object]s which manage the regions and links each point of the path goes through. + + + Include all available metadata about the returned path. + diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 6f026454e..755c0465a 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -40,6 +40,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) { if (up == p_up) { return; @@ -98,7 +110,18 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { return p; } -Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const { +Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, Array *r_path_rids, Vector *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; @@ -144,6 +167,24 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } 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 path; path.resize(2); path.write[0] = begin_point; @@ -325,6 +366,7 @@ Vector 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. @@ -341,7 +383,7 @@ Vector 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; @@ -349,7 +391,9 @@ Vector 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; } } @@ -360,7 +404,7 @@ Vector 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; @@ -368,7 +412,9 @@ Vector 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); } } @@ -384,12 +430,24 @@ Vector 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.invert(); + if (r_path_types) { + r_path_types->invert(); + } + if (r_path_rids) { + r_path_rids->invert(); + } + if (r_path_owners) { + r_path_owners->invert(); + } + } else { path.push_back(end_point); + APPEND_METADATA(end_poly); // Add mid points int np_id = least_cost_id; @@ -398,18 +456,37 @@ Vector 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.invert(); + if (r_path_types) { + r_path_types->invert(); + } + if (r_path_rids) { + r_path_rids->invert(); + } + if (r_path_owners) { + r_path_owners->invert(); + } } + // 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; } @@ -922,7 +999,7 @@ void NavMap::dispatch_callbacks() { } } -void NavMap::clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { +void NavMap::clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, Array *r_path_rids, Vector *r_path_owners) const { Vector3 from = path[path.size() - 1]; if (from.is_equal_approx(p_to_point)) { @@ -951,6 +1028,7 @@ void NavMap::clip_path(const LocalVector &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 a05d93751..7caefb24a 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -137,7 +137,7 @@ public: gd::PointKey get_point_key(const Vector3 &p_pos) const; - Vector get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const; + Vector get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, Array *r_path_rids, Vector *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; @@ -186,7 +186,7 @@ public: private: void compute_single_step(uint32_t index, RvoAgent **agent); - void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const; + void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, Array *r_path_rids, Vector *r_path_owners) const; }; #endif // NAV_MAP_H diff --git a/modules/navigation/pandemonium_navigation_server.cpp b/modules/navigation/pandemonium_navigation_server.cpp index e8b871e33..5afcb4a39 100644 --- a/modules/navigation/pandemonium_navigation_server.cpp +++ b/modules/navigation/pandemonium_navigation_server.cpp @@ -38,6 +38,8 @@ #include "scene/resources/navigation_mesh.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. @@ -246,7 +248,7 @@ Vector PandemoniumNavigationServer::map_get_path(RID p_map, Vector3 p_o const NavMap *map = map_owner.getornull(p_map); ERR_FAIL_COND_V(map == nullptr, Vector()); - return map->get_path(p_origin, p_destination, p_optimize, p_layers); + return map->get_path(p_origin, p_destination, p_optimize, p_layers, nullptr, nullptr, nullptr); } Vector3 PandemoniumNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -862,21 +864,35 @@ void PandemoniumNavigationServer::process(real_t p_delta_time) { pm_edge_free_count = _new_pm_edge_free_count; } -NavigationUtilities::PathQueryResult PandemoniumNavigationServer::_query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const { - NavigationUtilities::PathQueryResult r_query_result; +PathQueryResult PandemoniumNavigationServer::_query_path(const PathQueryParameters &p_parameters) const { + PathQueryResult r_query_result; const NavMap *map = map_owner.getornull(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) { Vector path; // 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) { - 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) { - 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 & PathMetadataFlags::PATH_INCLUDE_TYPES) != 0) ? &r_query_result.path_types : nullptr, + ((p_parameters.metadata_flags & PathMetadataFlags::PATH_INCLUDE_RIDS) != 0) ? &r_query_result.path_rids : nullptr, + ((p_parameters.metadata_flags & PathMetadataFlags::PATH_INCLUDE_OWNERS) != 0) ? &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 & PathMetadataFlags::PATH_INCLUDE_TYPES) != 0) ? &r_query_result.path_types : nullptr, + ((p_parameters.metadata_flags & PathMetadataFlags::PATH_INCLUDE_RIDS) != 0) ? &r_query_result.path_rids : nullptr, + ((p_parameters.metadata_flags & PathMetadataFlags::PATH_INCLUDE_OWNERS) != 0) ? &r_query_result.path_owner_ids : nullptr); } r_query_result.path.resize(path.size()); diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp index 2cb12a669..722f1d038 100644 --- a/scene/2d/navigation_agent_2d.cpp +++ b/scene/2d/navigation_agent_2d.cpp @@ -77,6 +77,9 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_navigation_layer_value", "layer_number", "value"), &NavigationAgent2D::set_navigation_layer_value); ClassDB::bind_method(D_METHOD("get_navigation_layer_value", "layer_number"), &NavigationAgent2D::get_navigation_layer_value); + ClassDB::bind_method(D_METHOD("set_path_metadata_flags", "flags"), &NavigationAgent2D::set_path_metadata_flags); + ClassDB::bind_method(D_METHOD("get_path_metadata_flags"), &NavigationAgent2D::get_path_metadata_flags); + ClassDB::bind_method(D_METHOD("set_navigation_map", "navigation_map"), &NavigationAgent2D::set_navigation_map); ClassDB::bind_method(D_METHOD("get_navigation_map"), &NavigationAgent2D::get_navigation_map); @@ -86,6 +89,7 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_next_position"), &NavigationAgent2D::get_next_position); ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target); ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity); + ClassDB::bind_method(D_METHOD("get_current_navigation_result"), &NavigationAgent2D::get_current_navigation_result); ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent2D::get_nav_path); ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent2D::get_nav_path_index); ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached); @@ -101,6 +105,7 @@ void NavigationAgent2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance"); ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "10,100,1"), "set_path_max_distance", "get_path_max_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -134,6 +139,8 @@ void NavigationAgent2D::_bind_methods() { ADD_SIGNAL(MethodInfo("path_changed")); ADD_SIGNAL(MethodInfo("target_reached")); + ADD_SIGNAL(MethodInfo("waypoint_reached", PropertyInfo(Variant::DICTIONARY, "details"))); + ADD_SIGNAL(MethodInfo("link_reached", PropertyInfo(Variant::DICTIONARY, "details"))); ADD_SIGNAL(MethodInfo("navigation_finished")); ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR2, "safe_velocity"))); } @@ -237,6 +244,7 @@ NavigationAgent2D::NavigationAgent2D() { avoidance_enabled = false; navigation_layers = 1; + path_metadata_flags = NavigationPathQueryParameters2D::PATH_METADATA_INCLUDE_ALL; path_desired_distance = 1.0; target_desired_distance = 1.0; @@ -370,6 +378,13 @@ bool NavigationAgent2D::get_navigation_layer_value(int p_layer_number) const { return get_navigation_layers() & (1 << (p_layer_number - 1)); } +void NavigationAgent2D::set_path_metadata_flags(const int p_flags) { + path_metadata_flags = p_flags; +} +int NavigationAgent2D::get_path_metadata_flags() const { + return path_metadata_flags; +} + void NavigationAgent2D::set_navigation_map(RID p_navigation_map) { map_override = p_navigation_map; Navigation2DServer::get_singleton()->agent_set_map(agent, map_override); @@ -449,6 +464,10 @@ Vector2 NavigationAgent2D::get_next_position() { } } +Ref NavigationAgent2D::get_current_navigation_result() const { + return navigation_result; +} + const Vector &NavigationAgent2D::get_nav_path() const { return navigation_result->get_path(); } @@ -550,6 +569,7 @@ void NavigationAgent2D::update_navigation() { navigation_query->set_start_position(origin); navigation_query->set_target_position(target_position); navigation_query->set_navigation_layers(navigation_layers); + navigation_query->set_metadata_flags(path_metadata_flags); if (map_override.is_valid()) { navigation_query->set_map(map_override); @@ -578,8 +598,53 @@ void NavigationAgent2D::update_navigation() { if (navigation_finished == false) { // Advances to the next far away position. const Vector &navigation_path = navigation_result->get_path(); + const Vector &navigation_path_types = navigation_result->get_path_types(); + const Array &navigation_path_rids = navigation_result->get_path_rids(); + const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + while (origin.distance_to(navigation_path[nav_path_index]) < path_desired_distance) { + Dictionary details; + + const Vector2 waypoint = navigation_path[nav_path_index]; + details["location"] = waypoint; + + int waypoint_type = -1; + if ((path_metadata_flags & NavigationPathQueryParameters2D::PATH_METADATA_INCLUDE_TYPES) != 0) { + const NavigationPathQueryResult2D::PathSegmentType type = NavigationPathQueryResult2D::PathSegmentType(navigation_path_types[nav_path_index]); + + details["type"] = type; + waypoint_type = type; + } + + if ((path_metadata_flags & NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS) != 0) { + details["rid"] = navigation_path_rids[nav_path_index]; + } + + if ((path_metadata_flags & NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS) != 0) { + const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[nav_path_index]); + + // Get a reference to the owning object. + Object *owner = nullptr; + if (waypoint_owner_id != 0) { + owner = ObjectDB::get_instance(waypoint_owner_id); + } + + details["owner"] = owner; + } + + // Emit a signal for the waypoint + emit_signal("waypoint_reached", details); + + // Emit a signal if we've reached a navigation link + if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) { + emit_signal("link_reached", details); + } + + // Move to the next waypoint on the list + nav_path_index += 1; + + // Check to see if we've finished our route if (nav_path_index == navigation_path.size()) { _check_distance_to_target(); nav_path_index -= 1; diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h index 9e3844138..c90e48d7a 100644 --- a/scene/2d/navigation_agent_2d.h +++ b/scene/2d/navigation_agent_2d.h @@ -49,6 +49,7 @@ class NavigationAgent2D : public Node { bool avoidance_enabled; uint32_t navigation_layers; + int path_metadata_flags; real_t path_desired_distance; real_t target_desired_distance; @@ -118,6 +119,9 @@ public: void set_navigation_layer_value(int p_layer_number, bool p_value); bool get_navigation_layer_value(int p_layer_number) const; + void set_path_metadata_flags(const int p_flags); + int get_path_metadata_flags() const; + void set_navigation_map(RID p_navigation_map); RID get_navigation_map() const; @@ -164,6 +168,8 @@ public: Vector2 get_next_position(); + Ref get_current_navigation_result() const; + const Vector &get_nav_path() const; int get_nav_path_index() const { diff --git a/scene/3d/navigation_agent.cpp b/scene/3d/navigation_agent.cpp index 4dd91c5ef..0d7dc4968 100644 --- a/scene/3d/navigation_agent.cpp +++ b/scene/3d/navigation_agent.cpp @@ -88,12 +88,16 @@ void NavigationAgent::_bind_methods() { ClassDB::bind_method(D_METHOD("set_navigation_layer_value", "layer_number", "value"), &NavigationAgent::set_navigation_layer_value); ClassDB::bind_method(D_METHOD("get_navigation_layer_value", "layer_number"), &NavigationAgent::get_navigation_layer_value); + ClassDB::bind_method(D_METHOD("set_path_metadata_flags", "flags"), &NavigationAgent::set_path_metadata_flags); + ClassDB::bind_method(D_METHOD("get_path_metadata_flags"), &NavigationAgent::get_path_metadata_flags); + ClassDB::bind_method(D_METHOD("set_target_position", "position"), &NavigationAgent::set_target_position); ClassDB::bind_method(D_METHOD("get_target_position"), &NavigationAgent::get_target_position); ClassDB::bind_method(D_METHOD("get_next_position"), &NavigationAgent::get_next_position); ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent::distance_to_target); ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent::set_velocity); + ClassDB::bind_method(D_METHOD("get_current_navigation_result"), &NavigationAgent::get_current_navigation_result); ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent::get_nav_path); ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent::get_nav_path_index); ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent::is_target_reached); @@ -110,6 +114,7 @@ void NavigationAgent::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent_height_offset", PROPERTY_HINT_RANGE, "-100.0,100,0.01"), "set_agent_height_offset", "get_agent_height_offset"); ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1"), "set_path_max_distance", "get_path_max_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags"); ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); @@ -140,6 +145,8 @@ void NavigationAgent::_bind_methods() { ADD_SIGNAL(MethodInfo("path_changed")); ADD_SIGNAL(MethodInfo("target_reached")); + ADD_SIGNAL(MethodInfo("waypoint_reached", PropertyInfo(Variant::DICTIONARY, "details"))); + ADD_SIGNAL(MethodInfo("link_reached", PropertyInfo(Variant::DICTIONARY, "details"))); ADD_SIGNAL(MethodInfo("navigation_finished")); ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity"))); } @@ -243,6 +250,7 @@ NavigationAgent::NavigationAgent() { avoidance_enabled = false; navigation_layers = 1; + path_metadata_flags = NavigationPathQueryParameters3D::PATH_METADATA_INCLUDE_ALL; path_desired_distance = 1.0; target_desired_distance = 1.0; @@ -380,6 +388,13 @@ bool NavigationAgent::get_navigation_layer_value(int p_layer_number) const { return get_navigation_layers() & (1 << (p_layer_number - 1)); } +void NavigationAgent::set_path_metadata_flags(const int p_flags) { + path_metadata_flags = p_flags; +} +int NavigationAgent::get_path_metadata_flags() const { + return path_metadata_flags; +} + void NavigationAgent::set_navigation_map(RID p_navigation_map) { map_override = p_navigation_map; NavigationServer::get_singleton()->agent_set_map(agent, map_override); @@ -466,6 +481,10 @@ Vector3 NavigationAgent::get_next_position() { } } +Ref NavigationAgent::get_current_navigation_result() const { + return navigation_result; +} + const Vector &NavigationAgent::get_nav_path() const { return navigation_result->get_path(); } @@ -573,7 +592,7 @@ void NavigationAgent::update_navigation() { navigation_query->set_start_position(origin); navigation_query->set_target_position(target_position); navigation_query->set_navigation_layers(navigation_layers); - + navigation_query->set_metadata_flags(path_metadata_flags); if (map_override.is_valid()) { navigation_query->set_map(map_override); @@ -602,8 +621,53 @@ void NavigationAgent::update_navigation() { if (navigation_finished == false) { // Advances to the next far away position. const Vector &navigation_path = navigation_result->get_path(); + const Vector &navigation_path_types = navigation_result->get_path_types(); + const Array &navigation_path_rids = navigation_result->get_path_rids(); + const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + while (origin.distance_to(navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0)) < path_desired_distance) { + Dictionary details; + + const Vector3 waypoint = navigation_path[nav_path_index]; + details["location"] = waypoint; + + int waypoint_type = -1; + if ((path_metadata_flags & NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES) != 0) { + const NavigationPathQueryResult3D::PathSegmentType type = NavigationPathQueryResult3D::PathSegmentType(navigation_path_types[nav_path_index]); + + details["type"] = type; + waypoint_type = type; + } + + if ((path_metadata_flags & NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS) != 0) { + details["rid"] = navigation_path_rids[nav_path_index]; + } + + if ((path_metadata_flags & NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS) != 0) { + const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[nav_path_index]); + + // Get a reference to the owning object. + Object *owner = nullptr; + if (waypoint_owner_id != 0) { + owner = ObjectDB::get_instance(waypoint_owner_id); + } + + details["owner"] = owner; + } + + // Emit a signal for the waypoint + emit_signal("waypoint_reached", details); + + // Emit a signal if we've reached a navigation link + if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) { + emit_signal("link_reached", details); + } + + // Move to the next waypoint on the list + nav_path_index += 1; + + // Check to see if we've finished our route if (nav_path_index == navigation_path.size()) { _check_distance_to_target(); nav_path_index -= 1; diff --git a/scene/3d/navigation_agent.h b/scene/3d/navigation_agent.h index b8372c48c..c9bdc05bd 100644 --- a/scene/3d/navigation_agent.h +++ b/scene/3d/navigation_agent.h @@ -51,6 +51,7 @@ class NavigationAgent : public Node { bool avoidance_enabled; uint32_t navigation_layers; + int path_metadata_flags; real_t path_desired_distance; real_t target_desired_distance; @@ -124,6 +125,9 @@ public: void set_navigation_layer_value(int p_layer_number, bool p_value); bool get_navigation_layer_value(int p_layer_number) const; + void set_path_metadata_flags(const int p_flags); + int get_path_metadata_flags() const; + void set_navigation_map(RID p_navigation_map); RID get_navigation_map() const; @@ -180,6 +184,8 @@ public: Vector3 get_next_position(); + Ref get_current_navigation_result() const; + const Vector &get_nav_path() const; int get_nav_path_index() const { diff --git a/servers/navigation/navigation_path_query_result_2d.cpp b/servers/navigation/navigation_path_query_result_2d.cpp index 2130ac322..180cff866 100644 --- a/servers/navigation/navigation_path_query_result_2d.cpp +++ b/servers/navigation/navigation_path_query_result_2d.cpp @@ -54,14 +54,33 @@ Array NavigationPathQueryResult2D::get_path_rids() const { return path_rids; } -void NavigationPathQueryResult2D::set_path_owner_ids(const Vector &p_path_owner_ids) { +void NavigationPathQueryResult2D::set_path_owner_ids(const Vector &p_path_owner_ids) { path_owner_ids = p_path_owner_ids; } -const Vector &NavigationPathQueryResult2D::get_path_owner_ids() const { +const Vector &NavigationPathQueryResult2D::get_path_owner_ids() const { return path_owner_ids; } +void NavigationPathQueryResult2D::set_path_owner_ids_bind(const Array p_path_owner_ids) { + path_owner_ids.resize(p_path_owner_ids.size()); + + for (int i = 0; i < path_owner_ids.size(); ++i) { + path_owner_ids.write[i] = p_path_owner_ids[i]; + } +} +Array NavigationPathQueryResult2D::get_path_owner_ids_bind() const { + Array ret; + + ret.resize(path_owner_ids.size()); + + for (int i = 0; i < path_owner_ids.size(); ++i) { + ret[i] = path_owner_ids[i]; + } + + return ret; +} + void NavigationPathQueryResult2D::reset() { path.clear(); path_types.clear(); @@ -79,15 +98,15 @@ void NavigationPathQueryResult2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path_rids", "path_rids"), &NavigationPathQueryResult2D::set_path_rids); ClassDB::bind_method(D_METHOD("get_path_rids"), &NavigationPathQueryResult2D::get_path_rids); - ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult2D::set_path_owner_ids); - ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult2D::get_path_owner_ids); + ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult2D::set_path_owner_ids_bind); + ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult2D::get_path_owner_ids_bind); ClassDB::bind_method(D_METHOD("reset"), &NavigationPathQueryResult2D::reset); ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR2_ARRAY, "path"), "set_path", "get_path"); ADD_PROPERTY(PropertyInfo(Variant::POOL_INT_ARRAY, "path_types"), "set_path_types", "get_path_types"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "path_rids"), "set_path_rids", "get_path_rids"); - ADD_PROPERTY(PropertyInfo(Variant::POOL_INT_ARRAY, "path_owner_ids"), "set_path_owner_ids", "get_path_owner_ids"); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "path_owner_ids"), "set_path_owner_ids", "get_path_owner_ids"); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_REGION); BIND_ENUM_CONSTANT(PATH_SEGMENT_TYPE_LINK); diff --git a/servers/navigation/navigation_path_query_result_2d.h b/servers/navigation/navigation_path_query_result_2d.h index cf8d5ce67..5f4f348bb 100644 --- a/servers/navigation/navigation_path_query_result_2d.h +++ b/servers/navigation/navigation_path_query_result_2d.h @@ -40,7 +40,7 @@ class NavigationPathQueryResult2D : public Reference { Vector path; Vector path_types; Array path_rids; - Vector path_owner_ids; + Vector path_owner_ids; protected: static void _bind_methods(); @@ -60,8 +60,11 @@ public: void set_path_rids(const Array &p_path_rids); Array get_path_rids() const; - void set_path_owner_ids(const Vector &p_path_owner_ids); - const Vector &get_path_owner_ids() const; + void set_path_owner_ids(const Vector &p_path_owner_ids); + const Vector &get_path_owner_ids() const; + + void set_path_owner_ids_bind(const Array p_path_owner_ids); + Array get_path_owner_ids_bind() const; void reset(); }; diff --git a/servers/navigation/navigation_path_query_result_3d.cpp b/servers/navigation/navigation_path_query_result_3d.cpp index 69f60cff7..741293bbd 100644 --- a/servers/navigation/navigation_path_query_result_3d.cpp +++ b/servers/navigation/navigation_path_query_result_3d.cpp @@ -54,14 +54,33 @@ Array NavigationPathQueryResult3D::get_path_rids() const { return path_rids; } -void NavigationPathQueryResult3D::set_path_owner_ids(const Vector &p_path_owner_ids) { +void NavigationPathQueryResult3D::set_path_owner_ids(const Vector &p_path_owner_ids) { path_owner_ids = p_path_owner_ids; } -const Vector &NavigationPathQueryResult3D::get_path_owner_ids() const { +const Vector &NavigationPathQueryResult3D::get_path_owner_ids() const { return path_owner_ids; } +void NavigationPathQueryResult3D::set_path_owner_ids_bind(const Array p_path_owner_ids) { + path_owner_ids.resize(p_path_owner_ids.size()); + + for (int i = 0; i < path_owner_ids.size(); ++i) { + path_owner_ids.write[i] = p_path_owner_ids[i]; + } +} +Array NavigationPathQueryResult3D::get_path_owner_ids_bind() const { + Array ret; + + ret.resize(path_owner_ids.size()); + + for (int i = 0; i < path_owner_ids.size(); ++i) { + ret[i] = path_owner_ids[i]; + } + + return ret; +} + void NavigationPathQueryResult3D::reset() { path.clear(); path_types.clear(); @@ -79,8 +98,8 @@ void NavigationPathQueryResult3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_path_rids", "path_rids"), &NavigationPathQueryResult3D::set_path_rids); ClassDB::bind_method(D_METHOD("get_path_rids"), &NavigationPathQueryResult3D::get_path_rids); - ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult3D::set_path_owner_ids); - ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult3D::get_path_owner_ids); + ClassDB::bind_method(D_METHOD("set_path_owner_ids", "path_owner_ids"), &NavigationPathQueryResult3D::set_path_owner_ids_bind); + ClassDB::bind_method(D_METHOD("get_path_owner_ids"), &NavigationPathQueryResult3D::get_path_owner_ids_bind); ClassDB::bind_method(D_METHOD("reset"), &NavigationPathQueryResult3D::reset); diff --git a/servers/navigation/navigation_path_query_result_3d.h b/servers/navigation/navigation_path_query_result_3d.h index d2ba14c30..cc19ece80 100644 --- a/servers/navigation/navigation_path_query_result_3d.h +++ b/servers/navigation/navigation_path_query_result_3d.h @@ -41,7 +41,7 @@ class NavigationPathQueryResult3D : public Reference { Vector path; Vector path_types; Array path_rids; - Vector path_owner_ids; + Vector path_owner_ids; protected: static void _bind_methods(); @@ -61,8 +61,11 @@ public: void set_path_rids(const Array &p_path_rids); Array get_path_rids() const; - void set_path_owner_ids(const Vector &p_path_owner_ids); - const Vector &get_path_owner_ids() const; + void set_path_owner_ids(const Vector &p_path_owner_ids); + const Vector &get_path_owner_ids() const; + + void set_path_owner_ids_bind(const Array p_path_owner_ids); + Array get_path_owner_ids_bind() const; void reset(); }; diff --git a/servers/navigation/navigation_utilities.h b/servers/navigation/navigation_utilities.h index 12b67e501..50d8c9fa7 100644 --- a/servers/navigation/navigation_utilities.h +++ b/servers/navigation/navigation_utilities.h @@ -85,14 +85,14 @@ struct PathQueryResult { Vector path; Vector path_types; Array path_rids; - Vector path_owner_ids; + Vector path_owner_ids; }; struct PathQueryResult2D { Vector path; Vector path_types; Array path_rids; - Vector path_owner_ids; + Vector path_owner_ids; }; } //namespace NavigationUtilities diff --git a/servers/navigation_server.cpp b/servers/navigation_server.cpp index 2421b3df2..04527c0a8 100644 --- a/servers/navigation_server.cpp +++ b/servers/navigation_server.cpp @@ -626,6 +626,9 @@ void NavigationServer::query_path(const Ref &p_ const NavigationUtilities::PathQueryResult _query_result = _query_path(p_query_parameters->get_parameters()); p_query_result->set_path(_query_result.path); + p_query_result->set_path_types(_query_result.path_types); + p_query_result->set_path_rids(_query_result.path_rids); + p_query_result->set_path_owner_ids(_query_result.path_owner_ids); } Vector NavigationServerManager::navigation_servers;