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);
}
