From 8433934bcd67f4ac569f4314af94d92d7e96ca8f Mon Sep 17 00:00:00 2001
From: BOTLester <58360400+BOTLester@users.noreply.github.com>
Date: Sun, 3 May 2020 23:40:40 +0200
Subject: [PATCH] implement direction cost

---
 Game1/Sources/Pathing/A-Star/Astar.cs         | 28 +++++++++++++++----
 .../Sources/Pathing/A-Star/PathSaver/Nodes.cs | 12 ++++++++
 2 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/Game1/Sources/Pathing/A-Star/Astar.cs b/Game1/Sources/Pathing/A-Star/Astar.cs
index 77b4aea..a089a44 100644
--- a/Game1/Sources/Pathing/A-Star/Astar.cs
+++ b/Game1/Sources/Pathing/A-Star/Astar.cs
@@ -35,12 +35,13 @@ class Astar
         var adjacentNodes = new List<Nodes>()
 
         {
-            new Nodes(new Vector2(currentPos.X, currentPos.Y+1)),
-            new Nodes(new Vector2(currentPos.X + 1, currentPos.Y)),
-            new Nodes(new Vector2(currentPos.X, currentPos.Y - 1)),
-            new Nodes(new Vector2(currentPos.X - 1, currentPos.Y)),
+            new Nodes(new Vector2(currentPos.X, currentPos.Y+1), 0),
+            new Nodes(new Vector2(currentPos.X + 1, currentPos.Y), 1),
+            new Nodes(new Vector2(currentPos.X, currentPos.Y - 1), 2),
+            new Nodes(new Vector2(currentPos.X - 1, currentPos.Y), -1),
         };
 
+        //check if out of range
         for (int i = 3; i >=0; i--)
         {
             if (adjacentNodes[i].getCords().X < 0 || adjacentNodes[i].getCords().Y < 0)
@@ -56,12 +57,23 @@ class Astar
             item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
     }
 
-
+    // Heuristic function, Manhattan method.
     public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
     {
         return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
     }
 
+    public int CalculateRotationCost(int currDir, int newDir)
+    {
+        if (currDir == newDir)
+            return 0;
+        else if (Math.Abs(currDir - newDir) == 1 || Math.Abs(currDir - newDir) == 3)
+            return 1;
+        else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2) //its for turning back, so it never should happen anyway, remove this check then?
+            return 10;
+        return 0;
+    }
+
     public Path FindPath()
     {
         Path path = new Path();
@@ -72,6 +84,7 @@ class Astar
         Nodes target = new Nodes(targetPos);
         Nodes startPos = new Nodes (tractorPos);
         Nodes current = null;
+        int direction = 6;
         int g = 0;
 
         //openList.Enqueue(startPos);
@@ -87,6 +100,7 @@ class Astar
             //closedList.Enqueue(current);
             openList.removeMin();
             //openList.Dequeue();
+            direction = current.getDirection();
 
             if (current.getCords() == target.getCords())
                break;
@@ -99,9 +113,9 @@ class Astar
             //g++;
             foreach(var adjacentNode in adjacentNodes)
             {
-                g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement();
                 if (closedList.Exists(adjacentNode.getCords()))
                     continue;
+                g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection());
                 if (!(openList.Exists(adjacentNode.getCords())))
                 {
                     
@@ -121,7 +135,9 @@ class Astar
                         adjacentNode.setParent(current);
                     }
                 }
+
             }
+
             //openList.BuildMinHeap(openList.GetList());
         }
 
diff --git a/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs b/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs
index 2789c18..7413965 100644
--- a/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs
+++ b/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs
@@ -14,12 +14,19 @@ class Nodes
     private int H = 0;
     private Vector2 Coordinates;
     private Nodes Parent = null;
+    private int Direction = 0;
 
     public Nodes(Vector2 coordinates)
     {
         Coordinates = coordinates;
     }
 
+    public Nodes(Vector2 coordinates, int direction)
+    {
+        Coordinates = coordinates;
+        Direction = direction;
+    }
+
     public Nodes (Nodes node)
     {
         F = node.F;
@@ -82,4 +89,9 @@ class Nodes
     {
         Parent = parent;
     }
+
+    public int getDirection()
+    {
+        return Direction;
+    }
 }