Started drafting path smoothing algorithm

components/NavigationComponent.cs

@@ -177,6 +177,48 @@ public class NavigationComponent : Node
     }
 
 
+    bool SweptSphereHasCollision(Vector3 fromPosition, Vector3 toPosition, float radius)
+    {
+        if ((fromPosition - toPosition).LengthSquared() < 0.001)
+        {
+            return false;
+        }
+        
+        Vector3 direction = (toPosition - fromPosition).Normalized();
+        
+        // TODO: Complete Implementation
+        Debug.Assert(false);
+        return true;
+    }
+
+    public List<NavigationPoint> SmoothPath(List<NavigationPoint> navigationPoints)
+    {
+        List<NavigationPoint> smoothedPath = new List<NavigationPoint>(); 
+            
+        int startIndex = 0;
+        smoothedPath.Add(navigationPoints[startIndex]);
+        
+        int endIndex = navigationPoints.Count > 1 ? 1 : 0;
+
+        while (endIndex != navigationPoints.Count - 1)
+        {
+            Vector3 startPoint = navigationPoints[startIndex].WorldPosition;
+            Vector3 endPoint = navigationPoints[endIndex].WorldPosition;
+
+            if (SweptSphereHasCollision(startPoint, endPoint, 0.25f))
+            {
+                smoothedPath.Add(navigationPoints[endIndex-1]);
+                smoothedPath.Add(navigationPoints[endIndex]);
+                startIndex = endIndex;
+                endIndex += 1;
+            }
+        }
+
+        smoothedPath.Add(navigationPoints[endIndex]);
+
+        return smoothedPath;
+    }
+
     public void PlanDirectPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
     {
         if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)