diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs
index 56f487c..7478462 100644
--- a/components/NavigationComponent.cs
+++ b/components/NavigationComponent.cs
@@ -163,7 +163,12 @@ public class NavigationComponent : Spatial
         
         // Perform smoothing
         _planningPathSmoothedWorldNavigationPoints = SmoothPath(body, _planningPathWorldNavigationPoints);
-        _planningPathSmoothedWorldNavigationPoints[0] = new NavigationPoint(fromPositionWorld);
+
+        // Ensure starting point is the current position
+        if (_planningPathSmoothedWorldNavigationPoints.Count > 0)
+        {
+            _planningPathSmoothedWorldNavigationPoints[0] = new NavigationPoint(fromPositionWorld);
+        }
     }
 
     public void FindPath(KinematicBody body, Vector3 fromPositionWorld, NavigationPoint navigationPoint)