public List <Coordinates> AStar(Coordinates start, Coordinates target) { FastPriorityQueue <BreadCrump> nodesQueue = new FastPriorityQueue <BreadCrump>(VerticeAmount); Dictionary <Coordinates, BreadCrump> guide = new Dictionary <Coordinates, BreadCrump>(); BreadCrump ariadnasThread = new BreadCrump(target, 0); nodesQueue.Enqueue(ariadnasThread, 0); guide.Add(target, ariadnasThread); while (nodesQueue.IsAny()) { BreadCrump current = nodesQueue.Dequeue(); if (current.Equals(start)) { ariadnasThread = current; break; } foreach (var neighbour in Neighbours(current)) { int newCost = 1 + current.PathCost; if (!guide.ContainsKey(neighbour) || newCost < guide[neighbour].PathCost) { BreadCrump next = new BreadCrump(neighbour, newCost); guide[next] = current; int priority = newCost + Heuristic(neighbour, start); nodesQueue.Enqueue(next, priority); } } } if (ariadnasThread.Equals(start)) { List <Coordinates> path = new List <Coordinates>(); path.Add(ariadnasThread); while (!ariadnasThread.Equals(target)) { ariadnasThread = guide[ariadnasThread]; path.Add(ariadnasThread); } return(path); } return(null); }