From 5cabf4ca28f566f8eabc6c33ec686777f45b65d0 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Wed, 1 Nov 2023 12:34:28 +0100
Subject: [PATCH] Fixed exception when planning results in an empty path.

---
 components/NavigationComponent.cs | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

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)