From fce16b6662b49ba19599ce00b1b204f1dd92645c Mon Sep 17 00:00:00 2001 From: Ershn Date: Tue, 3 Oct 2023 12:20:38 +0900 Subject: [PATCH] Make target_desired_distance affect the navigation of NavigationAgent2D/3D When the target is reachable, stop the navigation only when the target is reached. --- doc/classes/NavigationAgent2D.xml | 23 ++-- doc/classes/NavigationAgent3D.xml | 23 ++-- scene/2d/navigation_agent_2d.cpp | 217 ++++++++++++++++++----------- scene/2d/navigation_agent_2d.h | 17 ++- scene/3d/navigation_agent_3d.cpp | 222 ++++++++++++++++++------------ scene/3d/navigation_agent_3d.h | 17 ++- 6 files changed, 329 insertions(+), 190 deletions(-) diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml index 4df8d4143b31..fc7faf9260d0 100644 --- a/doc/classes/NavigationAgent2D.xml +++ b/doc/classes/NavigationAgent2D.xml @@ -84,8 +84,8 @@ - Returns [code]true[/code] if the end of the currently loaded navigation path has been reached. - [b]Note:[/b] While true prefer to stop calling update functions like [method get_next_path_position]. This avoids jittering the standing agent due to calling repeated path updates. + Returns [code]true[/code] if the agent's navigation has finished. If the target is reachable, navigation ends when the target is reached. If the target is unreachable, navigation ends when the last waypoint of the path is reached. + [b]Note:[/b] While [code]true[/code] prefer to stop calling update functions like [method get_next_path_position]. This avoids jittering the standing agent due to calling repeated path updates. @@ -97,7 +97,7 @@ - Returns true if [member target_position] is reached. It may not always be possible to reach the target position. It should always be possible to reach the final position though. See [method get_final_position]. + Returns [code]true[/code] if the agent reached the target, i.e. the agent moved within [member target_desired_distance] of the [member target_position]. It may not always be possible to reach the target but it should always be possible to reach the final position. See [method get_final_position]. @@ -180,7 +180,7 @@ The distance to search for other agents. - The distance threshold before a path point is considered to be reached. This allows agents to not have to hit a path point on the path exactly, but only to reach its general area. If this value is set too high, the NavigationAgent will skip points on the path, which can lead to leaving the navigation mesh. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot or undershoot the distance to the next point on each physics frame update. + The distance threshold before a path point is considered to be reached. This allows agents to not have to hit a path point on the path exactly, but only to reach its general area. If this value is set too high, the NavigationAgent will skip points on the path, which can lead to it leaving the navigation mesh. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot the distance to the next point on each physics frame update. 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. @@ -199,7 +199,9 @@ 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. - The distance threshold before the final target point is considered to be reached. This allows agents to not have to hit the point of the final target exactly, but only to reach its general area. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot or undershoot the distance to the final target point on each physics frame update. + The distance threshold before the target is considered to be reached. On reaching the target, [signal target_reached] is emitted and navigation ends (see [method is_navigation_finished] and [signal navigation_finished]). + You can make navigation end early by setting this property to a value greater than [member path_desired_distance] (navigation will end before reaching the last waypoint). + You can also make navigation end closer to the target than each individual path position by setting this property to a value lower than [member path_desired_distance] (navigation won't immediately end when reaching the last waypoint). However, if the value set is too low, the agent will be stuck in a repath loop because it will constantly overshoot the distance to the target on each physics frame update. If set, a new navigation path from the current agent position to the [member target_position] is requested from the NavigationServer. @@ -218,7 +220,7 @@ - Notifies when a navigation link has been reached. + Signals that the agent reached a navigation link. Emitted when the agent moves within [member path_desired_distance] of the next position of the path when that position is a navigation link. The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: - [code]position[/code]: The start position of the link that was reached. - [code]type[/code]: Always [constant NavigationPathQueryResult2D.PATH_SEGMENT_TYPE_LINK]. @@ -230,7 +232,8 @@ - Emitted once per loaded path when the agent internal navigation path index reaches the last index of the loaded path array. The agent internal navigation path index can be received with [method get_current_navigation_path_index]. + Signals that the agent's navigation has finished. If the target is reachable, navigation ends when the target is reached. If the target is unreachable, navigation ends when the last waypoint of the path is reached. This signal is emitted only once per loaded path. + This signal will be emitted just after [signal target_reached] when the target is reachable. @@ -243,7 +246,9 @@ - Emitted once per loaded path when the agent's global position is the first time within [member target_desired_distance] to the [member target_position]. + Signals that the agent reached the target, i.e. the agent moved within [member target_desired_distance] of the [member target_position]. This signal is emitted only once per loaded path. + This signal will be emitted just before [signal navigation_finished] when the target is reachable. + It may not always be possible to reach the target but it should always be possible to reach the final position. See [method get_final_position]. @@ -255,7 +260,7 @@ - Notifies when a waypoint along the path has been reached. + Signals that the agent reached a waypoint. Emitted when the agent moves within [member path_desired_distance] of the next position of the path. The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: - [code]position[/code]: The position of the waypoint that was reached. - [code]type[/code]: The type of navigation primitive (region or link) that contains this waypoint. diff --git a/doc/classes/NavigationAgent3D.xml b/doc/classes/NavigationAgent3D.xml index ec9f679307e8..cbcf50bb2972 100644 --- a/doc/classes/NavigationAgent3D.xml +++ b/doc/classes/NavigationAgent3D.xml @@ -84,8 +84,8 @@ - Returns [code]true[/code] if the end of the currently loaded navigation path has been reached. - [b]Note:[/b] While true prefer to stop calling update functions like [method get_next_path_position]. This avoids jittering the standing agent due to calling repeated path updates. + Returns [code]true[/code] if the agent's navigation has finished. If the target is reachable, navigation ends when the target is reached. If the target is unreachable, navigation ends when the last waypoint of the path is reached. + [b]Note:[/b] While [code]true[/code] prefer to stop calling update functions like [method get_next_path_position]. This avoids jittering the standing agent due to calling repeated path updates. @@ -97,7 +97,7 @@ - Returns true if [member target_position] is reached. It may not always be possible to reach the target position. It should always be possible to reach the final position though. See [method get_final_position]. + Returns [code]true[/code] if the agent reached the target, i.e. the agent moved within [member target_desired_distance] of the [member target_position]. It may not always be possible to reach the target but it should always be possible to reach the final position. See [method get_final_position]. @@ -183,7 +183,7 @@ The distance to search for other agents. - The distance threshold before a path point is considered to be reached. This allows agents to not have to hit a path point on the path exactly, but only to reach its general area. If this value is set too high, the NavigationAgent will skip points on the path, which can lead to leaving the navigation mesh. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot or undershoot the distance to the next point on each physics frame update. + The distance threshold before a path point is considered to be reached. This allows agents to not have to hit a path point on the path exactly, but only to reach its general area. If this value is set too high, the NavigationAgent will skip points on the path, which can lead to it leaving the navigation mesh. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot the distance to the next point on each physics frame update. The height offset is subtracted from the y-axis value of any vector path position for this NavigationAgent. The NavigationAgent height offset does not change or influence the navigation mesh or pathfinding query result. Additional navigation maps that use regions with navigation meshes that the developer baked with appropriate agent radius or height values are required to support different-sized agents. @@ -205,7 +205,9 @@ 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. - The distance threshold before the final target point is considered to be reached. This allows agents to not have to hit the point of the final target exactly, but only to reach its general area. If this value is set too low, the NavigationAgent will be stuck in a repath loop because it will constantly overshoot or undershoot the distance to the final target point on each physics frame update. + The distance threshold before the target is considered to be reached. On reaching the target, [signal target_reached] is emitted and navigation ends (see [method is_navigation_finished] and [signal navigation_finished]). + You can make navigation end early by setting this property to a value greater than [member path_desired_distance] (navigation will end before reaching the last waypoint). + You can also make navigation end closer to the target than each individual path position by setting this property to a value lower than [member path_desired_distance] (navigation won't immediately end when reaching the last waypoint). However, if the value set is too low, the agent will be stuck in a repath loop because it will constantly overshoot the distance to the target on each physics frame update. If set, a new navigation path from the current agent position to the [member target_position] is requested from the NavigationServer. @@ -228,7 +230,7 @@ - Notifies when a navigation link has been reached. + Signals that the agent reached a navigation link. Emitted when the agent moves within [member path_desired_distance] of the next position of the path when that position is a navigation link. The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: - [code]position[/code]: The start position of the link that was reached. - [code]type[/code]: Always [constant NavigationPathQueryResult3D.PATH_SEGMENT_TYPE_LINK]. @@ -240,7 +242,8 @@ - Emitted once per loaded path when the agent internal navigation path index reaches the last index of the loaded path array. The agent internal navigation path index can be received with [method get_current_navigation_path_index]. + Signals that the agent's navigation has finished. If the target is reachable, navigation ends when the target is reached. If the target is unreachable, navigation ends when the last waypoint of the path is reached. This signal is emitted only once per loaded path. + This signal will be emitted just after [signal target_reached] when the target is reachable. @@ -253,7 +256,9 @@ - Emitted once per loaded path when the agent's global position is the first time within [member target_desired_distance] to the [member target_position]. + Signals that the agent reached the target, i.e. the agent moved within [member target_desired_distance] of the [member target_position]. This signal is emitted only once per loaded path. + This signal will be emitted just before [signal navigation_finished] when the target is reachable. + It may not always be possible to reach the target but it should always be possible to reach the final position. See [method get_final_position]. @@ -265,7 +270,7 @@ - Notifies when a waypoint along the path has been reached. + Signals that the agent reached a waypoint. Emitted when the agent moves within [member path_desired_distance] of the next position of the path. The details dictionary may contain the following keys depending on the value of [member path_metadata_flags]: - [code]position[/code]: The position of the waypoint that was reached. - [code]type[/code]: The type of navigation primitive (region or link) that contains this waypoint. diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp index 1b3b0bcef0be..87d5c9d74af2 100644 --- a/scene/2d/navigation_agent_2d.cpp +++ b/scene/2d/navigation_agent_2d.cpp @@ -274,7 +274,6 @@ void NavigationAgent2D::_notification(int p_what) { NavigationServer2D::get_singleton()->agent_set_velocity_forced(agent, velocity_forced); } } - _check_distance_to_target(); } #ifdef DEBUG_ENABLED if (debug_path_dirty) { @@ -556,7 +555,7 @@ Vector2 NavigationAgent2D::get_target_position() const { } Vector2 NavigationAgent2D::get_next_path_position() { - update_navigation(); + _update_navigation(); const Vector &navigation_path = navigation_result->get_path(); if (navigation_path.size() == 0) { @@ -577,17 +576,25 @@ bool NavigationAgent2D::is_target_reached() const { } bool NavigationAgent2D::is_target_reachable() { - return target_desired_distance >= get_final_position().distance_to(target_position); + _update_navigation(); + return _is_target_reachable(); +} + +bool NavigationAgent2D::_is_target_reachable() const { + return target_desired_distance >= _get_final_position().distance_to(target_position); } bool NavigationAgent2D::is_navigation_finished() { - update_navigation(); + _update_navigation(); return navigation_finished; } Vector2 NavigationAgent2D::get_final_position() { - update_navigation(); + _update_navigation(); + return _get_final_position(); +} +Vector2 NavigationAgent2D::_get_final_position() const { const Vector &navigation_path = navigation_result->get_path(); if (navigation_path.size() == 0) { return Vector2(); @@ -625,7 +632,7 @@ PackedStringArray NavigationAgent2D::get_configuration_warnings() const { return warnings; } -void NavigationAgent2D::update_navigation() { +void NavigationAgent2D::_update_navigation() { if (agent_parent == nullptr) { return; } @@ -679,6 +686,7 @@ void NavigationAgent2D::update_navigation() { debug_path_dirty = true; #endif // DEBUG_ENABLED navigation_finished = false; + last_waypoint_reached = false; navigation_path_index = 0; emit_signal(SNAME("path_changed")); } @@ -687,102 +695,145 @@ void NavigationAgent2D::update_navigation() { return; } - // Check if we can advance the navigation path - 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 TypedArray &navigation_path_rids = navigation_result->get_path_rids(); - const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + // Check if the navigation has already finished. + if (navigation_finished) { + return; + } - while (origin.distance_to(navigation_path[navigation_path_index]) < path_desired_distance) { - Dictionary details; + // Check if we reached the target. + if (_is_within_target_distance(origin)) { + // Emit waypoint_reached in case we also moved within distance of a waypoint. + _advance_waypoints(origin); + _transition_to_target_reached(); + _transition_to_navigation_finished(); + } else { + // Advance waypoints if possible. + _advance_waypoints(origin); + // Keep navigation running even after reaching the last waypoint if the target is reachable. + if (last_waypoint_reached && !_is_target_reachable()) { + _transition_to_navigation_finished(); + } + } +} - const Vector2 waypoint = navigation_path[navigation_path_index]; - details[SNAME("position")] = waypoint; +void NavigationAgent2D::_advance_waypoints(const Vector2 &p_origin) { + if (last_waypoint_reached) { + return; + } - int waypoint_type = -1; - if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) { - const NavigationPathQueryResult2D::PathSegmentType type = NavigationPathQueryResult2D::PathSegmentType(navigation_path_types[navigation_path_index]); + // Advance to the farthest possible waypoint. + while (_is_within_waypoint_distance(p_origin)) { + _trigger_waypoint_reached(); - details[SNAME("type")] = type; - waypoint_type = type; - } + if (_is_last_waypoint()) { + last_waypoint_reached = true; + break; + } - if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) { - details[SNAME("rid")] = navigation_path_rids[navigation_path_index]; - } + _move_to_next_waypoint(); + } +} + +void NavigationAgent2D::_request_repath() { + navigation_result->reset(); + target_reached = false; + navigation_finished = false; + last_waypoint_reached = false; + update_frame_id = 0; +} - if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) { - const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]); +bool NavigationAgent2D::_is_last_waypoint() const { + return navigation_path_index == navigation_result->get_path().size() - 1; +} - // Get a reference to the owning object. - Object *owner = nullptr; - if (waypoint_owner_id.is_valid()) { - owner = ObjectDB::get_instance(waypoint_owner_id); - } +void NavigationAgent2D::_move_to_next_waypoint() { + navigation_path_index += 1; +} - details[SNAME("owner")] = owner; - - if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) { - const NavigationLink2D *navlink = Object::cast_to(owner); - if (navlink) { - Vector2 link_global_start_position = navlink->get_global_start_position(); - Vector2 link_global_end_position = navlink->get_global_end_position(); - if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) { - details[SNAME("link_entry_position")] = link_global_start_position; - details[SNAME("link_exit_position")] = link_global_end_position; - } else { - details[SNAME("link_entry_position")] = link_global_end_position; - details[SNAME("link_exit_position")] = link_global_start_position; - } - } - } - } +bool NavigationAgent2D::_is_within_waypoint_distance(const Vector2 &p_origin) const { + const Vector &navigation_path = navigation_result->get_path(); + return p_origin.distance_to(navigation_path[navigation_path_index]) < path_desired_distance; +} - // Emit a signal for the waypoint - emit_signal(SNAME("waypoint_reached"), details); +bool NavigationAgent2D::_is_within_target_distance(const Vector2 &p_origin) const { + return p_origin.distance_to(target_position) < target_desired_distance; +} - // Emit a signal if we've reached a navigation link - if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) { - emit_signal(SNAME("link_reached"), details); - } +void NavigationAgent2D::_trigger_waypoint_reached() { + const Vector &navigation_path = navigation_result->get_path(); + const Vector &navigation_path_types = navigation_result->get_path_types(); + const TypedArray &navigation_path_rids = navigation_result->get_path_rids(); + const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + + Dictionary details; + + const Vector2 waypoint = navigation_path[navigation_path_index]; + details[SNAME("position")] = waypoint; + + int waypoint_type = -1; + if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) { + const NavigationPathQueryResult2D::PathSegmentType type = NavigationPathQueryResult2D::PathSegmentType(navigation_path_types[navigation_path_index]); + + details[SNAME("type")] = type; + waypoint_type = type; + } + + if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) { + details[SNAME("rid")] = navigation_path_rids[navigation_path_index]; + } + + if (path_metadata_flags.has_flag(NavigationPathQueryParameters2D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) { + const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]); + + // Get a reference to the owning object. + Object *owner = nullptr; + if (waypoint_owner_id.is_valid()) { + owner = ObjectDB::get_instance(waypoint_owner_id); + } - // Move to the next waypoint on the list - navigation_path_index += 1; - - // Check to see if we've finished our route - if (navigation_path_index == navigation_path.size()) { - _check_distance_to_target(); - navigation_path_index -= 1; - navigation_finished = true; - target_position_submitted = false; - if (avoidance_enabled) { - NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); - NavigationServer2D::get_singleton()->agent_set_velocity(agent, Vector2(0.0, 0.0)); - NavigationServer2D::get_singleton()->agent_set_velocity_forced(agent, Vector2(0.0, 0.0)); + details[SNAME("owner")] = owner; + + if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) { + const NavigationLink2D *navlink = Object::cast_to(owner); + if (navlink) { + Vector2 link_global_start_position = navlink->get_global_start_position(); + Vector2 link_global_end_position = navlink->get_global_end_position(); + if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) { + details[SNAME("link_entry_position")] = link_global_start_position; + details[SNAME("link_exit_position")] = link_global_end_position; + } else { + details[SNAME("link_entry_position")] = link_global_end_position; + details[SNAME("link_exit_position")] = link_global_start_position; } - emit_signal(SNAME("navigation_finished")); - break; } } } -} -void NavigationAgent2D::_request_repath() { - navigation_result->reset(); - target_reached = false; - navigation_finished = false; - update_frame_id = 0; + // Emit a signal for the waypoint. + emit_signal(SNAME("waypoint_reached"), details); + + // Emit a signal if we've reached a navigation link. + if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) { + emit_signal(SNAME("link_reached"), details); + } } -void NavigationAgent2D::_check_distance_to_target() { - if (!target_reached) { - if (distance_to_target() < target_desired_distance) { - target_reached = true; - emit_signal(SNAME("target_reached")); - } +void NavigationAgent2D::_transition_to_navigation_finished() { + navigation_finished = true; + target_position_submitted = false; + + if (avoidance_enabled) { + NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); + NavigationServer2D::get_singleton()->agent_set_velocity(agent, Vector2(0.0, 0.0)); + NavigationServer2D::get_singleton()->agent_set_velocity_forced(agent, Vector2(0.0, 0.0)); } + + emit_signal(SNAME("navigation_finished")); +} + +void NavigationAgent2D::_transition_to_target_reached() { + target_reached = true; + emit_signal(SNAME("target_reached")); } void NavigationAgent2D::set_avoidance_layers(uint32_t p_layers) { diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h index 10c703168b3c..0e46086a81d3 100644 --- a/scene/2d/navigation_agent_2d.h +++ b/scene/2d/navigation_agent_2d.h @@ -88,6 +88,7 @@ class NavigationAgent2D : public Node { bool target_reached = false; bool navigation_finished = true; + bool last_waypoint_reached = false; // No initialized on purpose uint32_t update_frame_id = 0; @@ -232,9 +233,21 @@ class NavigationAgent2D : public Node { float get_debug_path_custom_line_width() const; private: - void update_navigation(); + bool _is_target_reachable() const; + Vector2 _get_final_position() const; + + void _update_navigation(); + void _advance_waypoints(const Vector2 &p_origin); void _request_repath(); - void _check_distance_to_target(); + + bool _is_last_waypoint() const; + void _move_to_next_waypoint(); + bool _is_within_waypoint_distance(const Vector2 &p_origin) const; + bool _is_within_target_distance(const Vector2 &p_origin) const; + + void _trigger_waypoint_reached(); + void _transition_to_navigation_finished(); + void _transition_to_target_reached(); #ifdef DEBUG_ENABLED void _navigation_debug_changed(); diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp index b311495a7fe6..15ca0b472818 100644 --- a/scene/3d/navigation_agent_3d.cpp +++ b/scene/3d/navigation_agent_3d.cpp @@ -299,7 +299,6 @@ void NavigationAgent3D::_notification(int p_what) { NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, velocity_forced); } } - _check_distance_to_target(); } #ifdef DEBUG_ENABLED if (debug_path_dirty) { @@ -620,7 +619,7 @@ Vector3 NavigationAgent3D::get_target_position() const { } Vector3 NavigationAgent3D::get_next_path_position() { - update_navigation(); + _update_navigation(); const Vector &navigation_path = navigation_result->get_path(); if (navigation_path.size() == 0) { @@ -641,22 +640,30 @@ bool NavigationAgent3D::is_target_reached() const { } bool NavigationAgent3D::is_target_reachable() { - return target_desired_distance >= get_final_position().distance_to(target_position); + _update_navigation(); + return _is_target_reachable(); +} + +bool NavigationAgent3D::_is_target_reachable() const { + return target_desired_distance >= _get_final_position().distance_to(target_position); } bool NavigationAgent3D::is_navigation_finished() { - update_navigation(); + _update_navigation(); return navigation_finished; } Vector3 NavigationAgent3D::get_final_position() { - update_navigation(); + _update_navigation(); + return _get_final_position(); +} +Vector3 NavigationAgent3D::_get_final_position() const { const Vector &navigation_path = navigation_result->get_path(); if (navigation_path.size() == 0) { return Vector3(); } - return navigation_path[navigation_path.size() - 1]; + return navigation_path[navigation_path.size() - 1] - Vector3(0, path_height_offset, 0); } void NavigationAgent3D::set_velocity_forced(Vector3 p_velocity) { @@ -691,7 +698,7 @@ PackedStringArray NavigationAgent3D::get_configuration_warnings() const { return warnings; } -void NavigationAgent3D::update_navigation() { +void NavigationAgent3D::_update_navigation() { if (agent_parent == nullptr) { return; } @@ -747,6 +754,7 @@ void NavigationAgent3D::update_navigation() { debug_path_dirty = true; #endif // DEBUG_ENABLED navigation_finished = false; + last_waypoint_reached = false; navigation_path_index = 0; emit_signal(SNAME("path_changed")); } @@ -755,103 +763,147 @@ void NavigationAgent3D::update_navigation() { return; } - // Check if we can advance the navigation path - 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 TypedArray &navigation_path_rids = navigation_result->get_path_rids(); - const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + // Check if the navigation has already finished. + if (navigation_finished) { + return; + } - while (origin.distance_to(navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0)) < path_desired_distance) { - Dictionary details; + // Check if we reached the target. + if (_is_within_target_distance(origin)) { + // Emit waypoint_reached in case we also moved within distance of a waypoint. + _advance_waypoints(origin); + _transition_to_target_reached(); + _transition_to_navigation_finished(); + } else { + // Advance waypoints if possible. + _advance_waypoints(origin); + // Keep navigation running even after reaching the last waypoint if the target is reachable. + if (last_waypoint_reached && !_is_target_reachable()) { + _transition_to_navigation_finished(); + } + } +} - const Vector3 waypoint = navigation_path[navigation_path_index]; - details[SNAME("position")] = waypoint; +void NavigationAgent3D::_advance_waypoints(const Vector3 &p_origin) { + if (last_waypoint_reached) { + return; + } - int waypoint_type = -1; - if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) { - const NavigationPathQueryResult3D::PathSegmentType type = NavigationPathQueryResult3D::PathSegmentType(navigation_path_types[navigation_path_index]); + // Advance to the farthest possible waypoint. + while (_is_within_waypoint_distance(p_origin)) { + _trigger_waypoint_reached(); - details[SNAME("type")] = type; - waypoint_type = type; - } + if (_is_last_waypoint()) { + last_waypoint_reached = true; + break; + } - if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) { - details[SNAME("rid")] = navigation_path_rids[navigation_path_index]; - } + _move_to_next_waypoint(); + } +} - if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) { - const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]); +void NavigationAgent3D::_request_repath() { + navigation_result->reset(); + target_reached = false; + navigation_finished = false; + last_waypoint_reached = false; + update_frame_id = 0; +} - // Get a reference to the owning object. - Object *owner = nullptr; - if (waypoint_owner_id.is_valid()) { - owner = ObjectDB::get_instance(waypoint_owner_id); - } +bool NavigationAgent3D::_is_last_waypoint() const { + return navigation_path_index == navigation_result->get_path().size() - 1; +} - details[SNAME("owner")] = owner; - - if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) { - const NavigationLink3D *navlink = Object::cast_to(owner); - if (navlink) { - Vector3 link_global_start_position = navlink->get_global_start_position(); - Vector3 link_global_end_position = navlink->get_global_end_position(); - if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) { - details[SNAME("link_entry_position")] = link_global_start_position; - details[SNAME("link_exit_position")] = link_global_end_position; - } else { - details[SNAME("link_entry_position")] = link_global_end_position; - details[SNAME("link_exit_position")] = link_global_start_position; - } - } - } - } +void NavigationAgent3D::_move_to_next_waypoint() { + navigation_path_index += 1; +} - // Emit a signal for the waypoint - emit_signal(SNAME("waypoint_reached"), details); +bool NavigationAgent3D::_is_within_waypoint_distance(const Vector3 &p_origin) const { + const Vector &navigation_path = navigation_result->get_path(); + Vector3 waypoint = navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0); + return p_origin.distance_to(waypoint) < path_desired_distance; +} - // Emit a signal if we've reached a navigation link - if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) { - emit_signal(SNAME("link_reached"), details); - } +bool NavigationAgent3D::_is_within_target_distance(const Vector3 &p_origin) const { + return p_origin.distance_to(target_position) < target_desired_distance; +} + +void NavigationAgent3D::_trigger_waypoint_reached() { + const Vector &navigation_path = navigation_result->get_path(); + const Vector &navigation_path_types = navigation_result->get_path_types(); + const TypedArray &navigation_path_rids = navigation_result->get_path_rids(); + const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); + + Dictionary details; + + const Vector3 waypoint = navigation_path[navigation_path_index]; + details[SNAME("position")] = waypoint; + + int waypoint_type = -1; + if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_TYPES)) { + const NavigationPathQueryResult3D::PathSegmentType type = NavigationPathQueryResult3D::PathSegmentType(navigation_path_types[navigation_path_index]); + + details[SNAME("type")] = type; + waypoint_type = type; + } + + if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_RIDS)) { + details[SNAME("rid")] = navigation_path_rids[navigation_path_index]; + } + + if (path_metadata_flags.has_flag(NavigationPathQueryParameters3D::PathMetadataFlags::PATH_METADATA_INCLUDE_OWNERS)) { + const ObjectID waypoint_owner_id = ObjectID(navigation_path_owners[navigation_path_index]); + + // Get a reference to the owning object. + Object *owner = nullptr; + if (waypoint_owner_id.is_valid()) { + owner = ObjectDB::get_instance(waypoint_owner_id); + } - // Move to the next waypoint on the list - navigation_path_index += 1; - - // Check to see if we've finished our route - if (navigation_path_index == navigation_path.size()) { - _check_distance_to_target(); - navigation_path_index -= 1; - navigation_finished = true; - target_position_submitted = false; - if (avoidance_enabled) { - NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin); - NavigationServer3D::get_singleton()->agent_set_velocity(agent, Vector3(0.0, 0.0, 0.0)); - NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, Vector3(0.0, 0.0, 0.0)); - stored_y_velocity = 0.0; + details[SNAME("owner")] = owner; + + if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) { + const NavigationLink3D *navlink = Object::cast_to(owner); + if (navlink) { + Vector3 link_global_start_position = navlink->get_global_start_position(); + Vector3 link_global_end_position = navlink->get_global_end_position(); + if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) { + details[SNAME("link_entry_position")] = link_global_start_position; + details[SNAME("link_exit_position")] = link_global_end_position; + } else { + details[SNAME("link_entry_position")] = link_global_end_position; + details[SNAME("link_exit_position")] = link_global_start_position; } - emit_signal(SNAME("navigation_finished")); - break; } } } -} -void NavigationAgent3D::_request_repath() { - navigation_result->reset(); - target_reached = false; - navigation_finished = false; - update_frame_id = 0; + // Emit a signal for the waypoint. + emit_signal(SNAME("waypoint_reached"), details); + + // Emit a signal if we've reached a navigation link. + if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) { + emit_signal(SNAME("link_reached"), details); + } } -void NavigationAgent3D::_check_distance_to_target() { - if (!target_reached) { - if (distance_to_target() < target_desired_distance) { - target_reached = true; - emit_signal(SNAME("target_reached")); - } +void NavigationAgent3D::_transition_to_navigation_finished() { + navigation_finished = true; + target_position_submitted = false; + + if (avoidance_enabled) { + NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin); + NavigationServer3D::get_singleton()->agent_set_velocity(agent, Vector3(0.0, 0.0, 0.0)); + NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, Vector3(0.0, 0.0, 0.0)); + stored_y_velocity = 0.0; } + + emit_signal(SNAME("navigation_finished")); +} + +void NavigationAgent3D::_transition_to_target_reached() { + target_reached = true; + emit_signal(SNAME("target_reached")); } void NavigationAgent3D::set_avoidance_layers(uint32_t p_layers) { diff --git a/scene/3d/navigation_agent_3d.h b/scene/3d/navigation_agent_3d.h index ff0498a2a8f8..4eaed8314929 100644 --- a/scene/3d/navigation_agent_3d.h +++ b/scene/3d/navigation_agent_3d.h @@ -95,6 +95,7 @@ class NavigationAgent3D : public Node { bool target_position_submitted = false; bool target_reached = false; bool navigation_finished = true; + bool last_waypoint_reached = false; // No initialized on purpose uint32_t update_frame_id = 0; @@ -250,9 +251,21 @@ class NavigationAgent3D : public Node { float get_debug_path_custom_point_size() const; private: - void update_navigation(); + bool _is_target_reachable() const; + Vector3 _get_final_position() const; + + void _update_navigation(); + void _advance_waypoints(const Vector3 &p_origin); void _request_repath(); - void _check_distance_to_target(); + + bool _is_last_waypoint() const; + void _move_to_next_waypoint(); + bool _is_within_waypoint_distance(const Vector3 &p_origin) const; + bool _is_within_target_distance(const Vector3 &p_origin) const; + + void _trigger_waypoint_reached(); + void _transition_to_navigation_finished(); + void _transition_to_target_reached(); #ifdef DEBUG_ENABLED void _navigation_debug_changed();