static void Main(string[] args)
{
var endGoalFound = false;
var costs = new[]
{
//1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1
0, 0, 1, 0, 1, 1, 1, 0, 1
};
var map = new Map(3, 3, costs);
var start = new Node(2, 0);
var end = new Node(0, 2);
var cameFrom = new Dictionary <Node, Node>();
cameFrom.Add(start, null);
var costSoFar = new Dictionary <Node, int>();
costSoFar.Add(start, 0);
var frontier = new List <Node>();
frontier.Add(start);
while (frontier.Count > 0)
{
var current = frontier.OrderBy(x => x.Priority).First();
frontier.Remove(current);
if (Equals(current, end))
{
endGoalFound = true;
break;
}
var neighbours = map.GetNeighbours(current);
foreach (var neighbour in neighbours)
{
var newCost = costSoFar[current] + map.Cost(current, neighbour);
if (costSoFar.ContainsKey(neighbour) == false || costSoFar[neighbour] > newCost)
{
costSoFar[neighbour] = newCost;
neighbour.Priority = newCost + EstimateDistance(end, neighbour);
frontier.Add(neighbour);
cameFrom[neighbour] = current;
}
}
}
if (endGoalFound == false)
{
Console.WriteLine("No path to end");
return;
}
var path = new List <Node>();
var pathNode = end;
while (!Equals(pathNode, start))
{
path.Add(pathNode);
pathNode = cameFrom[pathNode];
}
path.Reverse();
foreach (var node in path)
{
Console.WriteLine($"{node.X}, {node.Y}");
//Console.WriteLine($"{map.Nodes.IndexOf(node)}");
}
Console.ReadKey();
}