public void AddNeighbour(NavNode node)
{
Neighbours.Add(node);
}
public static IEnumerable <NavNode> FindBestPath(NavNode source, NavNode target, IEnumerable <NavNode> graph)
{
var dist = new Dictionary <NavNode, int>();
var prev = new Dictionary <NavNode, NavNode>();
var unvisited = new List <NavNode>();
dist[source] = 0;
prev[source] = null;
foreach (var v in graph)
{
if (v != source)
{
dist[v] = int.MaxValue;
prev[v] = null;
}
unvisited.Add(v);
}
while (unvisited.Any())
{
NavNode u = null;
foreach (var un in unvisited)
{
if (u == null || dist[un] < dist[u])
{
u = un;
}
}
if (u == target)
{
break;
}
unvisited.Remove(u);
foreach (var n in u.Neighbours)
{
var alt = dist[u] + n.Coord.DistanceTo(u.Coord);
if (alt < dist[n])
{
dist[n] = alt;
prev[n] = u;
}
}
}
if (prev[target] != null)
{
var path = new List <NavNode>();
var node = target;
while (node != null)
{
path.Add(node);
node = prev[node];
}
path.Reverse();
return(path);
}
return(null);
}
