From 6e4199eb47626b730bb233d13045290898685874 Mon Sep 17 00:00:00 2001 From: BOTLester <58360400+BOTLester@users.noreply.github.com> Date: Sun, 3 May 2020 16:35:46 +0200 Subject: [PATCH] Broken A*, but implemented --- Game1/Game1.cs | 4 +- Game1/Sources/Objects/Tractor.cs | 4 +- Game1/Sources/Pathing/A-Star/Astar.cs | 90 +++++++++++++++++-- .../Sources/Pathing/A-Star/PathSaver/Nodes.cs | 73 +++++++++++++-- .../Sources/Pathing/A-Star/PathSaver/Path.cs | 50 +++++++++++ .../Pathing/A-Star/PathSaver/PriorityQueue.cs | 33 ++++--- Game1/Sources/Smart/SmartTractor.cs | 31 +++---- 7 files changed, 239 insertions(+), 46 deletions(-) diff --git a/Game1/Game1.cs b/Game1/Game1.cs index 45cbbf8..af08189 100644 --- 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 diff --git a/Game1/Sources/Objects/Tractor.cs b/Game1/Sources/Objects/Tractor.cs index 86bf81f..6e4ae27 100644 --- 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); } } diff --git a/Game1/Sources/Pathing/A-Star/Astar.cs b/Game1/Sources/Pathing/A-Star/Astar.cs index 8b93c83..2dff97c 100644 --- 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(); } -} \ No newline at end of file + + private List GetAdjacentNodes(Vector2 currentPos) + { + var adjacentNodes = new List() + + { + 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; + } + +} \ No newline at end of file diff --git a/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs b/Game1/Sources/Pathing/A-Star/PathSaver/Nodes.cs index c898cb9..2789c18 100644 --- 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; } } diff --git a/Game1/Sources/Pathing/A-Star/PathSaver/Path.cs b/Game1/Sources/Pathing/A-Star/PathSaver/Path.cs index d2c9e25..86e17f7 100644 --- 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 } + */ } \ No newline at end of file diff --git a/Game1/Sources/Pathing/A-Star/PathSaver/PriorityQueue.cs b/Game1/Sources/Pathing/A-Star/PathSaver/PriorityQueue.cs index 8043950..603d552 100644 --- 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 list; + public List list; public int Count { get { return list.Count; } } public PriorityQueue() { - list = new List(); + list = new List(); } public PriorityQueue(int count) { - list = new List(count); + list = new List(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(); diff --git a/Game1/Sources/Smart/SmartTractor.cs b/Game1/Sources/Smart/SmartTractor.cs index 6b4102c..690571f 100644 --- 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++; + } + + */ }