1 files changed, 134 insertions, 83 deletions

diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
index 1b3b0bcef0..87d5c9d74a 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<Vector2> &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<Vector2> &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<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;
+	// 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<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;
-						}
-					}
-				}
-			}
+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;
+}
 
-			// 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<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);
+		}
 
-			// 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<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_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) {