diff --git a/scene/2d/navigation/navigation_agent_2d.cpp b/scene/2d/navigation/navigation_agent_2d.cpp
index 32f1d373d180..7d78e4ab3624 100644
--- a/scene/2d/navigation/navigation_agent_2d.cpp
+++ b/scene/2d/navigation/navigation_agent_2d.cpp
@@ -231,8 +231,11 @@ void NavigationAgent2D::_notification(int p_what) {
 			set_agent_parent(get_parent());
 			set_physics_process_internal(true);
 
-			if (agent_parent && avoidance_enabled) {
-				NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position());
+			if (agent_parent) {
+				previous_origin = agent_parent->get_global_position();
+				if (avoidance_enabled) {
+					NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position());
+				}
 			}
 
 #ifdef DEBUG_ENABLED
@@ -808,6 +811,8 @@ void NavigationAgent2D::_update_navigation() {
 			_transition_to_navigation_finished();
 		}
 	}
+
+	previous_origin = origin;
 }
 
 void NavigationAgent2D::_advance_waypoints(const Vector2 &p_origin) {
@@ -845,11 +850,14 @@ void NavigationAgent2D::_move_to_next_waypoint() {
 
 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;
+	Vector2 waypoint = navigation_path[navigation_path_index];
+	return p_origin.distance_to(waypoint) < path_desired_distance ||
+			waypoint.distance_squared_to(Geometry2D::get_closest_point_to_segment(waypoint, previous_origin, p_origin)) < path_desired_distance * path_desired_distance;
 }
 
 bool NavigationAgent2D::_is_within_target_distance(const Vector2 &p_origin) const {
-	return p_origin.distance_to(target_position) < target_desired_distance;
+	return p_origin.distance_to(target_position) < target_desired_distance ||
+			target_position.distance_squared_to(Geometry2D::get_closest_point_to_segment(target_position, previous_origin, p_origin)) < target_desired_distance * target_desired_distance;
 }
 
 void NavigationAgent2D::_trigger_waypoint_reached() {
diff --git a/scene/2d/navigation/navigation_agent_2d.h b/scene/2d/navigation/navigation_agent_2d.h
index b178d70bbbab..c417848df79e 100644
--- a/scene/2d/navigation/navigation_agent_2d.h
+++ b/scene/2d/navigation/navigation_agent_2d.h
@@ -72,6 +72,8 @@ class NavigationAgent2D : public Node {
 
 	Vector2 target_position;
 
+	Vector2 previous_origin;
+
 	Ref<NavigationPathQueryParameters2D> navigation_query;
 	Ref<NavigationPathQueryResult2D> navigation_result;
 	int navigation_path_index = 0;
diff --git a/scene/3d/navigation/navigation_agent_3d.cpp b/scene/3d/navigation/navigation_agent_3d.cpp
index f21a3e2c16df..8aa28358117f 100644
--- a/scene/3d/navigation/navigation_agent_3d.cpp
+++ b/scene/3d/navigation/navigation_agent_3d.cpp
@@ -251,8 +251,11 @@ void NavigationAgent3D::_notification(int p_what) {
 			set_agent_parent(get_parent());
 			set_physics_process_internal(true);
 
-			if (agent_parent && avoidance_enabled) {
-				NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
+			if (agent_parent) {
+				previous_origin = agent_parent->get_global_position();
+				if (avoidance_enabled) {
+					NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
+				}
 			}
 
 #ifdef DEBUG_ENABLED
@@ -878,6 +881,8 @@ void NavigationAgent3D::_update_navigation() {
 			_transition_to_navigation_finished();
 		}
 	}
+
+	previous_origin = origin;
 }
 
 void NavigationAgent3D::_advance_waypoints(const Vector3 &p_origin) {
@@ -916,11 +921,17 @@ void NavigationAgent3D::_move_to_next_waypoint() {
 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;
+	Vector3 flattened_origin = p_origin - Vector3(0, path_height_offset, 0);
+	Vector3 flattened_prev_origin = previous_origin - Vector3(0, path_height_offset, 0);
+	return p_origin.distance_to(waypoint) < path_desired_distance ||
+			waypoint.distance_squared_to(Geometry3D::get_closest_point_to_segment(waypoint, flattened_prev_origin, flattened_origin)) < path_desired_distance * path_desired_distance;
 }
 
 bool NavigationAgent3D::_is_within_target_distance(const Vector3 &p_origin) const {
-	return p_origin.distance_to(target_position) < target_desired_distance;
+	Vector3 flattened_origin = p_origin - Vector3(0, path_height_offset, 0);
+	Vector3 flattened_prev_origin = previous_origin - Vector3(0, path_height_offset, 0);
+	return p_origin.distance_to(target_position) < target_desired_distance ||
+			target_position.distance_squared_to(Geometry3D::get_closest_point_to_segment(target_position, flattened_prev_origin, flattened_origin)) < target_desired_distance * target_desired_distance;
 }
 
 void NavigationAgent3D::_trigger_waypoint_reached() {
diff --git a/scene/3d/navigation/navigation_agent_3d.h b/scene/3d/navigation/navigation_agent_3d.h
index 03a92b7bc73f..f4bf88fa8c9d 100644
--- a/scene/3d/navigation/navigation_agent_3d.h
+++ b/scene/3d/navigation/navigation_agent_3d.h
@@ -76,6 +76,8 @@ class NavigationAgent3D : public Node {
 
 	Vector3 target_position;
 
+	Vector3 previous_origin;
+
 	Ref<NavigationPathQueryParameters3D> navigation_query;
 	Ref<NavigationPathQueryResult3D> navigation_result;
 	int navigation_path_index = 0;