1 files changed, 137 insertions, 85 deletions

diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
index b311495a7f..15ca0b4728 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<Vector3> &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<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) {
@@ -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<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;
+	// 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<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;
-						}
-					}
-				}
-			}
+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<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;
+}
 
-			// 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<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);
+		}
 
-			// 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<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_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) {