gaming/godot
gaming/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot synced 2024-09-19 15:18:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #82561 from ershn/fix_navigation_agents_is_target_reached_behavior

Browse source 
Make `target_desired_distance` affect the navigation of `NavigationAgent2D/3D`
This commit is contained in:
Yuri Sizov 2023-12-14 21:22:35 +01:00
parent 58e68590a3 fce16b6662
commit 302e41c264
6 changed files with 339 additions and 200 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
doc/classes/NavigationAgent2D.xml
View file

@ -84,8 +84,8 @@
<method name="is_navigation_finished">
<return type="bool" />
<description>
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.
</description>
</method>
<method name="is_target_reachable">
@ -97,7 +97,7 @@
<method name="is_target_reached" qualifiers="const">
<return type="bool" />
<description>
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].
</description>
</method>
<method name="set_avoidance_layer_value">
@ -180,7 +180,7 @@
The distance to search for other agents.
</member>
<member name="path_desired_distance" type="float" setter="set_path_desired_distance" getter="get_path_desired_distance" default="20.0">
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.
</member>
<member name="path_max_distance" type="float" setter="set_path_max_distance" getter="get_path_max_distance" default="100.0">
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.
</member>
<member name="target_desired_distance" type="float" setter="set_target_desired_distance" getter="get_target_desired_distance" default="10.0">
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.
</member>
<member name="target_position" type="Vector2" setter="set_target_position" getter="get_target_position" default="Vector2(0, 0)">
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 @@
<signal name="link_reached">
<param index="0" name="details" type="Dictionary" />
<description>
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 @@
</signal>
<signal name="navigation_finished">
<description>
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.
</description>
</signal>
<signal name="path_changed">
@ -243,7 +246,9 @@
</signal>
<signal name="target_reached">
<description>
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].
</description>
</signal>
<signal name="velocity_computed">
@ -255,7 +260,7 @@
<signal name="waypoint_reached">
<param index="0" name="details" type="Dictionary" />
<description>
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.

23
doc/classes/NavigationAgent3D.xml
View file

@ -84,8 +84,8 @@
<method name="is_navigation_finished">
<return type="bool" />
<description>
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.
</description>
</method>
<method name="is_target_reachable">
@ -97,7 +97,7 @@
<method name="is_target_reached" qualifiers="const">
<return type="bool" />
<description>
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].
</description>
</method>
<method name="set_avoidance_layer_value">
@ -183,7 +183,7 @@
The distance to search for other agents.
</member>
<member name="path_desired_distance" type="float" setter="set_path_desired_distance" getter="get_path_desired_distance" default="1.0">
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.
</member>
<member name="path_height_offset" type="float" setter="set_path_height_offset" getter="get_path_height_offset" default="0.0">
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.
</member>
<member name="target_desired_distance" type="float" setter="set_target_desired_distance" getter="get_target_desired_distance" default="1.0">
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.
</member>
<member name="target_position" type="Vector3" setter="set_target_position" getter="get_target_position" default="Vector3(0, 0, 0)">
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 @@
<signal name="link_reached">
<param index="0" name="details" type="Dictionary" />
<description>
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 @@
</signal>
<signal name="navigation_finished">
<description>
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.
</description>
</signal>
<signal name="path_changed">
@ -253,7 +256,9 @@
</signal>
<signal name="target_reached">
<description>
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].
</description>
</signal>
<signal name="velocity_computed">
@ -265,7 +270,7 @@
<signal name="waypoint_reached">
<param index="0" name="details" type="Dictionary" />
<description>
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.

227
scene/2d/navigation_agent_2d.cpp
View file

@ -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) {
@ -558,7 +557,7 @@ Vector2 NavigationAgent2D::get_target_position() const {
}
Vector2 NavigationAgent2D::get_next_path_position() {
update_navigation();
_update_navigation();
const Vector<Vector2> &navigation_path = navigation_result->get_path();
if (navigation_path.size() == 0) {
@ -579,17 +578,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<Vector2> &navigation_path = navigation_result->get_path();
if (navigation_path.size() == 0) {
return Vector2();
@ -627,7 +634,7 @@ PackedStringArray NavigationAgent2D::get_configuration_warnings() const {
return warnings;
}
void NavigationAgent2D::update_navigation() {
void NavigationAgent2D::_update_navigation() {
if (agent_parent == nullptr) {
return;
}
@ -681,6 +688,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"));
}
@ -689,102 +697,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<Vector2> &navigation_path = navigation_result->get_path();
const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
const Vector<int64_t> &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;
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);
}
details[SNAME("owner")] = owner;
if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) {
const NavigationLink2D *navlink = Object::cast_to<NavigationLink2D>(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 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);
}
// 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));
}
emit_signal(SNAME("navigation_finished"));
break;
}
// 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();
}
}
}
void NavigationAgent2D::_advance_waypoints(const Vector2 &p_origin) {
if (last_waypoint_reached) {
return;
}
// Advance to the farthest possible waypoint.
while (_is_within_waypoint_distance(p_origin)) {
_trigger_waypoint_reached();
if (_is_last_waypoint()) {
last_waypoint_reached = true;
break;
}
_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;
}
void NavigationAgent2D::_check_distance_to_target() {
if (!target_reached) {
if (distance_to_target() < target_desired_distance) {
target_reached = true;
emit_signal(SNAME("target_reached"));
bool NavigationAgent2D::_is_last_waypoint() const {
return navigation_path_index == navigation_result->get_path().size() - 1;
}
void NavigationAgent2D::_move_to_next_waypoint() {
navigation_path_index += 1;
}
bool NavigationAgent2D::_is_within_waypoint_distance(const Vector2 &p_origin) const {
const Vector<Vector2> &navigation_path = navigation_result->get_path();
return p_origin.distance_to(navigation_path[navigation_path_index]) < path_desired_distance;
}
bool NavigationAgent2D::_is_within_target_distance(const Vector2 &p_origin) const {
return p_origin.distance_to(target_position) < target_desired_distance;
}
void NavigationAgent2D::_trigger_waypoint_reached() {
const Vector<Vector2> &navigation_path = navigation_result->get_path();
const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
const Vector<int64_t> &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);
}
details[SNAME("owner")] = owner;
if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) {
const NavigationLink2D *navlink = Object::cast_to<NavigationLink2D>(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 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::_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) {

17
scene/2d/navigation_agent_2d.h
View file

@ -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 @@ public:
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();

232
scene/3d/navigation_agent_3d.cpp
View file

@ -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) {
@ -622,7 +621,7 @@ Vector3 NavigationAgent3D::get_target_position() const {
}
Vector3 NavigationAgent3D::get_next_path_position() {
update_navigation();
_update_navigation();
const Vector<Vector3> &navigation_path = navigation_result->get_path();
if (navigation_path.size() == 0) {
@ -643,22 +642,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<Vector3> &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) {
@ -693,7 +700,7 @@ PackedStringArray NavigationAgent3D::get_configuration_warnings() const {
return warnings;
}
void NavigationAgent3D::update_navigation() {
void NavigationAgent3D::_update_navigation() {
if (agent_parent == nullptr) {
return;
}
@ -749,6 +756,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"));
}
@ -757,103 +765,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<Vector3> &navigation_path = navigation_result->get_path();
const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
const Vector<int64_t> &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;
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);
}
details[SNAME("owner")] = owner;
if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
const NavigationLink3D *navlink = Object::cast_to<NavigationLink3D>(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 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);
}
// 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;
}
emit_signal(SNAME("navigation_finished"));
break;
}
// 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();
}
}
}
void NavigationAgent3D::_advance_waypoints(const Vector3 &p_origin) {
if (last_waypoint_reached) {
return;
}
// Advance to the farthest possible waypoint.
while (_is_within_waypoint_distance(p_origin)) {
_trigger_waypoint_reached();
if (_is_last_waypoint()) {
last_waypoint_reached = true;
break;
}
_move_to_next_waypoint();
}
}
void NavigationAgent3D::_request_repath() {
navigation_result->reset();
target_reached = false;
navigation_finished = false;
last_waypoint_reached = false;
update_frame_id = 0;
}
void NavigationAgent3D::_check_distance_to_target() {
if (!target_reached) {
if (distance_to_target() < target_desired_distance) {
target_reached = true;
emit_signal(SNAME("target_reached"));
bool NavigationAgent3D::_is_last_waypoint() const {
return navigation_path_index == navigation_result->get_path().size() - 1;
}
void NavigationAgent3D::_move_to_next_waypoint() {
navigation_path_index += 1;
}
bool NavigationAgent3D::_is_within_waypoint_distance(const Vector3 &p_origin) const {
const Vector<Vector3> &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;
}
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<Vector3> &navigation_path = navigation_result->get_path();
const Vector<int32_t> &navigation_path_types = navigation_result->get_path_types();
const TypedArray<RID> &navigation_path_rids = navigation_result->get_path_rids();
const Vector<int64_t> &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);
}
details[SNAME("owner")] = owner;
if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
const NavigationLink3D *navlink = Object::cast_to<NavigationLink3D>(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 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::_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) {

17
scene/3d/navigation_agent_3d.h
View file

@ -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 @@ public:
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();