Broken A*, but implemented

2020-05-03 16:35:46 +02:00 · 2020-05-03 16:35:46 +02:00 · 6e4199eb47
commit 6e4199eb47
parent 8bf82380ab
7 changed files with 239 additions and 46 deletions
--- a/Game1/Game1.cs
+++ b/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
--- a/Game1/Sources/Objects/Tractor.cs
+++ b/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);
        }
    }
--- a/Game1/Sources/Pathing/A-Star/Astar.cs
+++ b/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;
+    }
+
+}   
--- a/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs
+++ b/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;
    }
 }
--- a/Game1/Sources/Pathing/A-Star/PathSaver/Path.cs
+++ b/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

    }

+    */
 }
--- a/Game1/Sources/Pathing/A-Star/PathSaver/PriorityQueue.cs
+++ b/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();
--- a/Game1/Sources/Smart/SmartTractor.cs
+++ b/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++;
+    }
+
+    */
 }