From 1ae17ed63b128a290869a60221f7056a731dc529 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Wed, 3 May 2023 12:10:01 +0200
Subject: [PATCH] Pathplanning now ignores grid.

---
 components/NavigationComponent.cs | 45 ++++++++++++++++++++++++++-----
 entities/Player.cs                |  2 +-
 2 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs
index b068d3a..f06f17f 100644
--- a/components/NavigationComponent.cs
+++ b/components/NavigationComponent.cs
@@ -97,7 +97,7 @@ public class NavigationComponent : Node
     }
 
 
-    public void Plan(Vector3 fromPositionWorld, Vector3 toPositionWorld)
+    public void PlanGridPath(Vector3 fromPositionWorld, Vector3 toPositionWorld)
     {
         Vector2 fromPositionOffset = TileWorld.WorldToOffsetCoords(fromPositionWorld);
         Vector2 toPositionOffset = TileWorld.WorldToOffsetCoords(toPositionWorld);
@@ -134,23 +134,56 @@ public class NavigationComponent : Node
     }
 
 
-    public void Plan(Vector3 fromPositionWorld, Vector3 toPositionWorld, Quat toWorldOrientation)
+    public void PlanGridPath(Vector3 fromPositionWorld, Vector3 toPositionWorld, Quat toWorldOrientation)
     {
-        Plan(fromPositionWorld, toPositionWorld);
+        PlanGridPath(fromPositionWorld, toPositionWorld);
+
+        _pathWorldNavigationPoints.Add(new NavigationPoint(toWorldOrientation));
+    }
+    
+    
+    public void PlanGridPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
+    {
+        if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
+            && navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
+        {
+            PlanGridPath(fromTransformWorld.origin, navigationPoint.WorldPosition, navigationPoint.WorldOrientation);
+        } else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
+        {
+            PlanGridPath(fromTransformWorld.origin, navigationPoint.WorldPosition);
+        } else
+        {
+            throw new NotImplementedException();
+        }
+    }
+    
+    
+    public void PlanDirectPath(Vector3 fromPositionWorld, Vector3 toPositionWorld)
+    {
+        _pathWorldNavigationPoints.Clear();
+        _pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));
+        
+        UpdateCurrentGoal();
+    }
+
+    
+    public void PlanDirectPath(Vector3 fromPositionWorld, Vector3 toPositionWorld, Quat toWorldOrientation)
+    {
+        PlanDirectPath(fromPositionWorld, toPositionWorld);
 
         _pathWorldNavigationPoints.Add(new NavigationPoint(toWorldOrientation));
     }
 
 
-    public void Plan(Transform fromTransformWorld, NavigationPoint navigationPoint)
+    public void PlanDirectPath(Transform fromTransformWorld, NavigationPoint navigationPoint)
     {
         if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
             && navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
         {
-            Plan(fromTransformWorld.origin, navigationPoint.WorldPosition, navigationPoint.WorldOrientation);
+            PlanDirectPath(fromTransformWorld.origin, navigationPoint.WorldPosition, navigationPoint.WorldOrientation);
         } else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
         {
-            Plan(fromTransformWorld.origin, navigationPoint.WorldPosition);
+            PlanDirectPath(fromTransformWorld.origin, navigationPoint.WorldPosition);
         } else
         {
             throw new NotImplementedException();
diff --git a/entities/Player.cs b/entities/Player.cs
index 34f82b3..2317ed0 100644
--- a/entities/Player.cs
+++ b/entities/Player.cs
@@ -76,7 +76,7 @@ public class Player : Entity
                 if (currentTask is TaskQueueComponent.NavigationTask)
                 {
                     TaskQueueComponent.NavigationTask navigationTask = (TaskQueueComponent.NavigationTask)currentTask;
-                    _navigationComponent.Plan(GlobalTransform, navigationTask.NavigationPoint);
+                    _navigationComponent.PlanDirectPath(GlobalTransform, navigationTask.NavigationPoint);
                 }
             }
         }