From d70d9cd6821f87e82c355c414afb3081429aa76f Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Fri, 27 Oct 2023 22:11:45 +0200
Subject: [PATCH] For planned paths use the current position as the first
 NavigationPoint.

---
 components/NavigationComponent.cs | 1 +
 1 file changed, 1 insertion(+)

diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs
index f029a1a..add6a67 100644
--- a/components/NavigationComponent.cs
+++ b/components/NavigationComponent.cs
@@ -160,6 +160,7 @@ public class NavigationComponent : Spatial
         
         // Perform smoothing
         _planningPathSmoothedWorldNavigationPoints = SmoothPath(body, _planningPathWorldNavigationPoints);
+        _planningPathSmoothedWorldNavigationPoints[0] = new NavigationPoint(fromPositionWorld);
     }
 
     public void FindPath(KinematicBody body, Vector3 fromPositionWorld, NavigationPoint navigationPoint)