using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; using Microsoft.Xna.Framework; using C5; using System.Diagnostics; class Astar { private Vector2 tractorPos; private Vector2 housePos; private static Crops[,] crops; private Vector2 Size; private Vector2 targetPos; private int Rotation; public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, int rotation) { tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y); housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y); crops = newCrops; Size = newSize; Rotation = rotation; } public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newTargetPos, int rotation) { tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y); housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y); crops = newCrops; Size = newSize; Rotation = rotation; targetPos = newTargetPos; } // Get all adjacent nodes public List GetAdjacentNodes(Vector2 currentPos) { var adjacentNodes = new List() { 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) adjacentNodes.Remove(adjacentNodes[i]); else { if (adjacentNodes[i].getCords().X > Size.X - 1 || adjacentNodes[i].getCords().Y > Size.Y - 1) adjacentNodes.Remove(adjacentNodes[i]); } } // return if not an obstacle return adjacentNodes.Where( item => (crops[(int)item.getCords().X, (int)item.getCords().Y].getStatus()) != 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)); } // Rotation Cost 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 20; else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2) return 900; return 0; } // Convert rotation used by sprite, to get direction of first node in next path public int ConvertRotation() { int rotation = 0; if (Rotation > 135 && Rotation < 225) rotation = 0; else if (Rotation > 225 && Rotation < 315) rotation = 1; else if (Rotation > 315 && Rotation < 45) rotation = 2; else if (Rotation > 45 && Rotation < 135) rotation = -1; return rotation; } // Main function of A* algorithm public Path FindPath(bool flipArray) { int g = 0; int direction = ConvertRotation(); Path path = new Path(); MinHeap openList = new MinHeap(); MinHeap closedList = new MinHeap(); Nodes target = new Nodes(targetPos); Nodes startPos = new Nodes(tractorPos, direction); Nodes current = null; openList.Insert(startPos); while (openList.GetSize() > 0) { current = openList.getMin(); closedList.Insert(current); openList.removeMin(); direction = current.getDirection(); if (current.getCords() == target.getCords()) break; var adjacentNodes = GetAdjacentNodes(current.getCords()); foreach (var adjacentNode in adjacentNodes) { if (closedList.Exists(adjacentNode.getCords())) // check if adjacent node is on closed list, if it is, skip it continue; g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection()); // calculate g - cost from start point if (!(openList.Exists(adjacentNode.getCords()))) // if adjacent node is not on open list, add it { adjacentNode.setG(g); adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), target.getCords())); adjacentNode.calculateF(); adjacentNode.setParent(current); openList.Insert(adjacentNode); } else { if (g + adjacentNode.getH() < adjacentNode.getF()) // check if adjacent node is a better path than the current one { adjacentNode.setG(g); adjacentNode.calculateF(); adjacentNode.setParent(current); } } } } // backtrack to create path while (current != null) { path.AddNode(current); current = current.getParent(); } if (flipArray) path = path.FlipArray(); openList.deleteHeap(); closedList.deleteHeap(); return path; } }