From c912df9d0591a7abcab9d0eab5fac80c28117e9f Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Thu, 29 Jun 2023 21:09:26 +0200
Subject: [PATCH] Fix NavigationAgent continues avoidance velocity

Fixes NavigationAgent continues avoidance velocity.
---
 scene/2d/navigation_agent_2d.cpp | 5 +++++
 scene/3d/navigation_agent_3d.cpp | 5 +++++
 2 files changed, 10 insertions(+)

diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
index 73a801c822c..9007681374e 100644
--- a/scene/2d/navigation_agent_2d.cpp
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -755,6 +755,11 @@ void NavigationAgent2D::update_navigation() {
 				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;
 			}
diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
index 63a2ff55349..ba11676225a 100644
--- a/scene/3d/navigation_agent_3d.cpp
+++ b/scene/3d/navigation_agent_3d.cpp
@@ -801,6 +801,11 @@ void NavigationAgent3D::update_navigation() {
 				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));
+				}
 				emit_signal(SNAME("navigation_finished"));
 				break;
 			}