Broken A*, but implemented

Game1/Game1.cs

@@ -103,10 +103,10 @@ namespace Game1
             spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getTargetPosition().X / input.getSpacingTile() * (input.getTileSize() + input.getSpacing()) + input.getTileSize() / 4, (int)tractorUnit.getTargetPosition().Y / input.getSpacingTile() * (input.getTileSize() + input.getSpacing()) + input.getTileSize() / 4, input.getTileSize()/2, input.getTileSize()/2), Color.Green);
             for (int i = 0; i < tractorUnit.getPath().getCount() + 1; i++)
             {
-                spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getPath().getByIndex(i).getVector().X * (input.getSpacingTile()) + input.getTileSize() / 4, (int)tractorUnit.getPath().getByIndex(i).getVector().Y * (input.getSpacingTile()) + input.getTileSize() / 4, input.getTileSize()/2, input.getTileSize()/2), Color.Green);
+                spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getPath().getByIndex(i).getCords().X * (input.getSpacingTile()) + input.getTileSize() / 4, (int)tractorUnit.getPath().getByIndex(i).getCords().Y * (input.getSpacingTile()) + input.getTileSize() / 4, input.getTileSize()/2, input.getTileSize()/2), Color.Green);
             }
 
-            spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getPath().getFinalDest().getVector().X * (input.getSpacingTile()) + Convert.ToInt32(input.getTileSize() / 6), (int)tractorUnit.getPath().getFinalDest().getVector().Y * (input.getSpacingTile()) + Convert.ToInt32(input.getTileSize() / 6), Convert.ToInt32(input.getTileSize()/1.5), Convert.ToInt32(input.getTileSize()/1.5)) , Color.Red);                         //Draws the current target of the tractor
+            spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getPath().getFinalDest().getCords().X * (input.getSpacingTile()) + Convert.ToInt32(input.getTileSize() / 6), (int)tractorUnit.getPath().getFinalDest().getCords().Y * (input.getSpacingTile()) + Convert.ToInt32(input.getTileSize() / 6), Convert.ToInt32(input.getTileSize()/1.5), Convert.ToInt32(input.getTileSize()/1.5)) , Color.Red);                         //Draws the current target of the tractor
             spriteBatch.Draw(tractor, new Rectangle((int)tractorUnit.getPos().X, (int)tractorUnit.getPos().Y, input.getTileSize(), input.getTileSize()), Color.White);
             spriteBatch.Draw(house, houseUnit.GetRectangle(), Color.White);
             spriteBatch.DrawString(Bold, "Speed:" + tractorUnit.getSpeed().ToString(), new Vector2(10, input.getSize().Y * (input.getTileSize() + input.getSpacing()) + 20) , Color.White);                                                //Draws the speed value

Game1/Sources/Objects/Tractor.cs

@@ -111,12 +111,12 @@ class Tractor
                         
             }
 
-            TargetPosition = path.Reduce().getVector() * (sizeTile + Spacing);
+            TargetPosition = path.Reduce().getCords() * (sizeTile + Spacing);
             updateDirection(Size, newPosition);
         }
         else
         {
-            TargetPosition = path.Reduce().getVector() * (sizeTile + Spacing);
+            TargetPosition = path.Reduce().getCords() * (sizeTile + Spacing);
             updateDirection(Size, newPosition);
         }
     }

Game1/Sources/Pathing/A-Star/Astar.cs

@@ -13,23 +13,97 @@ class Astar
     private Crops[,] crops;
     private Vector2 Size;
     private PriorityQueue allPaths;
+    private Vector2 targetPos;
 
-    public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos)
+    public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newtargetPos)
     {
         tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
         housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
+        targetPos = newtargetPos;
         crops = newCrops;
         Size = newSize;
 
     }
 
-    public void findPath()
-    {
-        int i = 1;
-    }
-
-    public Path getOptimalPath()
+    public Nodes getOptimalPath()
     {
         return allPaths.Peek();
     }
+
+    private List <Nodes> GetAdjacentNodes(Vector2 currentPos)
+    {
+        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)),
+        };
+
+        return adjacentNodes.Where(
+            item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
+    }
+
+    public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
+    {
+        return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
+    }
+
+    public Path FindPath()
+    {
+        Path path = null;
+        PriorityQueue openList = new PriorityQueue();
+        PriorityQueue closedList = new PriorityQueue();
+        Nodes startPos = new Nodes (tractorPos);
+        Nodes current = null;
+        int g = 0;
+
+        openList.Enqueue(startPos);
+
+        while (openList.Count > 0)
+        {
+            current = openList.Peek();
+            closedList.Enqueue(current);
+            openList.Dequeue();
+
+            if (closedList.Peek().getCords() == targetPos)
+                break;
+
+            var adjacentNodes = GetAdjacentNodes(current.getCords());
+            g++;
+
+            foreach(var adjacentNode in adjacentNodes)
+            {
+                if (closedList.Exists(adjacentNode.getCords()))
+                    continue;
+                if (!(openList.Exists(adjacentNode.getCords())))
+                {
+                    adjacentNode.setG(g);
+                    adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), targetPos));
+                    adjacentNode.calculateF();
+                    adjacentNode.setParent(current);
+                    openList.Enqueue(adjacentNode);
+                }
+                else
+                {
+                    if(g + adjacentNode.getH() < adjacentNode.getF())
+                    {
+                        adjacentNode.setG(g);
+                        adjacentNode.calculateF();
+                        adjacentNode.setParent(current);
+                    }
+                }
+            }
+        }
+
+        while (current.getParent() != null)
+        {
+            path.AddNode(current);
+            current = current.getParent();
+        }
+
+        return path;
+    }
+
 }   

Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs

@@ -9,22 +9,77 @@ using Microsoft.Xna.Framework.Graphics;
 
 class Nodes
 {
-    private int Cost;
-    private Vector2 Node;
+    private int F = 0;
+    private int G = 0;
+    private int H = 0;
+    private Vector2 Coordinates;
+    private Nodes Parent = null;
 
-    public Nodes(int cost, Vector2 node)
+    public Nodes(Vector2 coordinates)
     {
-        Cost = cost;
-        Node = node;
+        Coordinates = coordinates;
     }
 
-    public Vector2 getVector()
+    public Nodes (Nodes node)
     {
-        return Node;
+        F = node.F;
+        G = node.G;
+        H = node.H;
+        Coordinates = node.Coordinates;
+        Parent = node.Parent;
     }
 
-    public int getCost()
+    public Nodes(int f, int g, int h, Vector2 coordinates, Nodes parent)
     {
-        return Cost;
+        F = f;
+        G = g;
+        H = h;
+        Coordinates = coordinates;
+        Parent = parent;
+    }
+
+    public Vector2 getCords()
+    {
+        return Coordinates;
+    }
+
+    public int getF()
+    {
+        return F;
+    }
+
+    public int getG()
+    {
+        return G;
+    }
+
+    public int getH()
+    {
+        return H;
+    }
+
+    public void calculateF()
+    {
+        F = G + H;
+    }
+
+    public void setG(int g)
+    {
+        G = g;
+    }
+
+    public void setH(int h)
+    {
+        H = h;
+    }
+
+    public Nodes getParent()
+    {
+        return Parent;
+    }
+
+    public void setParent(Nodes parent)
+    {
+        Parent = parent;
     }
 }

Game1/Sources/Pathing/A-Star/PathSaver/Path.cs

@@ -11,6 +11,55 @@ class Path
 {
     private Nodes[] nodes = new Nodes[512];
     private int Count = 0;
+
+    public void AddNode(Nodes node)
+    {
+        nodes[Count] = new Nodes(node);
+        Count++;
+    }
+
+    public Nodes Reduce()
+    {
+        Count--;
+        Nodes temp = nodes[0];
+
+        for (int i = 0; i < Count; i++)
+        {
+            nodes[i] = nodes[i + 1];
+        }
+
+        return temp;
+    }
+
+    public void FlipArray()
+    {
+        int j = Count;
+        Nodes[] temp = nodes;
+        for (int i = 0; i < Count; i++)
+        {
+            nodes[i] = temp[j];
+            j--;
+        }
+    }
+
+    public Nodes getFinalDest()
+    {
+        return nodes[Count];
+    }
+
+    public int getCount()
+    {
+        return Count;
+    }
+
+    public Nodes getByIndex(int i)
+    {
+        return nodes[i];
+    }
+
+
+    /* 
+    private int Count = 0;
     private int Cost = 0;
     private int Efficency;
 
@@ -83,4 +132,5 @@ class Path
 
     }
 
+    */
 }

Game1/Sources/Pathing/A-Star/PathSaver/PriorityQueue.cs

@@ -6,21 +6,21 @@ using Microsoft.Xna.Framework.Graphics;
 
 class PriorityQueue
 {
-    public List<Path> list;
+    public List<Nodes> list;
     public int Count { get { return list.Count; } }
 
     public PriorityQueue()
     {
-        list = new List<Path>();
+        list = new List<Nodes>();
     }
 
     public PriorityQueue(int count)
     {
-        list = new List<Path>(count);
+        list = new List<Nodes>(count);
     }
 
 
-    public void Enqueue(Path x)
+    public void Enqueue(Nodes x)
     {
         list.Add(x);
         int i = Count - 1;
@@ -28,7 +28,7 @@ class PriorityQueue
         while (i > 0)
         {
             int p = (i - 1) / 2;
-            if (list[p].getEfficency() <= x.getEfficency()) break;
+            if (list[p].getF() <= x.getF()) break;
 
             list[i] = list[p];
             i = p;
@@ -39,8 +39,8 @@ class PriorityQueue
 
     public void Dequeue()
     {
-        Path min = Peek();
-        Path root = list[Count - 1];
+        Nodes min = Peek();
+        Nodes root = list[Count - 1];
         list.RemoveAt(Count - 1);
 
         int i = 0;
@@ -48,9 +48,9 @@ class PriorityQueue
         {
             int a = i * 2 + 1;
             int b = i * 2 + 2;
-            int c = b < Count && list[b].getEfficency() < list[a].getEfficency() ? b : a;
+            int c = b < Count && list[b].getF() < list[a].getF() ? b : a;
 
-            if (list[c].getEfficency() >= root.getEfficency()) break;
+            if (list[c].getF() >= root.getF()) break;
             list[i] = list[c];
             i = c;
         }
@@ -58,12 +58,25 @@ class PriorityQueue
         if (Count > 0) list[i] = root;
     }
 
-    public Path Peek()
+    public Nodes Peek()
     {
         if (Count == 0) throw new InvalidOperationException("Queue is empty.");
         return list[0];
     }
 
+    public Boolean Exists(Vector2 coordinates)
+    {
+        if (Count == 0)
+            return false;
+        foreach(Nodes node in list)
+        {
+            if (node.getCords() == coordinates)
+                return true;
+        }
+        return false;
+    }
+
+
     public void Clear()
     {
         list.Clear();

Game1/Sources/Smart/SmartTractor.cs

@@ -20,7 +20,7 @@ class SmartTractor
     //What to do next
     public Path returnChoice(int task)
     {
-        astar.update(crops, Size, tractorPos / (tileSize + Spacing), housePos / (tileSize + Spacing));
+        astar.update(crops, Size, tractorPos / (tileSize + Spacing), housePos / (tileSize + Spacing), Target);
         if (task == 0)  
         {
             //To the house
@@ -31,8 +31,7 @@ class SmartTractor
             //To the fields
             getTargetPosition(r.Next(0, (int)Size.X), r.Next(0, (int)Size.Y));
         }
-        astar.findPath();
-        createPath();
+        astar.FindPath();
         return path;
     }
 
@@ -41,13 +40,6 @@ class SmartTractor
 
 
 
-
-
-
-
-
-
-
     //Updates the variables every frame
     public void updateMap(Vector2 newTractorPos, Vector2 newHousePos, Crops[,] newCropsStatus, Vector2 newSize, int newTileSize, int newSpacing, int newScore)
     {
@@ -72,7 +64,7 @@ class SmartTractor
 
 
 
-
+    /*
     //Only for testing without obstacles
     private void createPath()
     {
@@ -93,26 +85,35 @@ class SmartTractor
                 else if (currentPath.Y < targetPos.Y)
                 {
                     currentPath = new Vector2(currentPath.X, currentPath.Y + 1);
-                    path.setNode(currentPath);
+                    path.setNode(currentPath, crops);
                 }
                 else if (currentPath.Y > targetPos.Y)
                 {
                     currentPath = new Vector2(currentPath.X, currentPath.Y - 1);
-                    path.setNode(currentPath);
+                    path.setNode(currentPath, crops);
                 }
             }
             else if (currentPath.X < targetPos.X)
             {
                 currentPath = new Vector2(currentPath.X + 1, currentPath.Y);
-                path.setNode(currentPath);
+                path.setNode(currentPath, crops);
             }
             else if (currentPath.X > targetPos.X)
             {
                 currentPath = new Vector2(currentPath.X - 1, currentPath.Y);
-                path.setNode(currentPath);
+                path.setNode(currentPath, crops);
             }
         } while (currentPath != targetPos);
 
 
     }
+
+    public void setNode(Vector2 newNode, Crops[,] Crop)
+    {
+
+        nodes[Count] = new Nodes(Crop[(int)newNode.X, (int)newNode.Y].getCostOnMovement(), newNode);
+        Count++;
+    }
+
+    */
 }