public void AStarSearch(Vector2 sourcePoint, Vector2 destinationPoint, out Vector2 nextNode) { //Console.WriteLine("Going from (" + sourcePoint.X + ", " + sourcePoint.Y + ") to (" + destinationPoint.X + ", " + destinationPoint.Y + ")"); Node source = adjacencyList[(int)(columns * sourcePoint.X + sourcePoint.Y)]; Node destination = adjacencyList[(int)(columns * destinationPoint.X + destinationPoint.Y)]; PriorityQueue <Node> Q = new PriorityQueue<Node>(); source.Distance = 0; Q.Add(source.Priority, source); while (Q.Count > 0) { Node current = Q.RemoveMin(); if (current == destination) { Node pathParent = current.Parent; Stack<Node> path = new Stack<Node>(); path.Push(current); while (pathParent != null) { path.Push(pathParent); pathParent = pathParent.Parent; } Thread t = new Thread(new ParameterizedThreadStart(DrawPath)); t.Start(path); path.Pop(); nextNode = new Vector2(path.Peek().X * 32, path.Peek().Y * 32); return; } List<Node> allAdjacent = new List<Node>(); allAdjacent.AddRange(current.AdjacentNodes); allAdjacent.AddRange(current.CornerNodes); foreach (Node n in allAdjacent) { Node node = adjacencyList[columns * n.X + n.Y]; if (node.Distance == int.MaxValue) { node.Distance = current.Distance + 1; node.Parent = current; node.Visited = true; node.Priority = (int)(destinationPoint - sourcePoint).Length() + node.Distance; Q.Add(node.Priority, node); } } } nextNode = new Vector2(-1); }
public static Path findPath(Node startNode, Node destination) { List<Node> closedList = new List<Node>(); PriorityQueue<Node> openList = new PriorityQueue<Node>(); Dictionary<Node, int> gScore = new Dictionary<Node,int>(); gScore[startNode] = 0; Dictionary<Node, int> fScore = new Dictionary<Node,int>(); fScore[startNode] = startNode.HeuristicDistance(destination); Dictionary<Node, Node> prevNode = new Dictionary<Node, Node>(); openList.Add(fScore[startNode], startNode); while (openList.Count > 0) { Node current = openList.RemoveMin(); if (current.Equals(destination)) { return getPath(prevNode, destination); } else { closedList.Add(current); Node[] neighbours = current.GetNeighbours(); foreach (Node next in neighbours) { if (closedList.Contains(next)) continue; int newGScore = gScore[current] + current.distanceBetween(next); if (!openList.Contains(next) || newGScore < gScore[next]) { prevNode[next] = current; gScore[next] = newGScore; fScore[next] = gScore[next] + next.HeuristicDistance(destination); if (!openList.Contains(next)) openList.Add(fScore[next], next); } } } } return null; }