public static Path FindPath(Node start, Node end)
{
Dictionary <Node, double> g = new Dictionary <Node, double>();
Dictionary <Node, double> f = new Dictionary <Node, double>();
Dictionary <Node, Node> parent = new Dictionary <Node, Node>();
SortedList <double, Node> Q = new SortedList <double, Node>();
List <Node> U = new List <Node>();
g[start] = 0;
f[start] = g[start] + Distance(start, end);
Q.Add(f[start], start);
while (Q.Count > 0)
{
Node current = Q.First().Value;
if (current == end)
{
List <LinkNode> result = new List <LinkNode>();
while (parent.ContainsKey(current))
{
result.Add(LinkNode.NewLink(parent[current], current));
current = parent[current];
}
return(new Path(start, end, result));
}
Q.RemoveAt(0);
U.Add(current);
foreach (LinkNode outLink in current.OutLinks)
{
double tentativeScore = g[current] + outLink.Lenght;
if (U.Contains(outLink.NodeEnd) &&
tentativeScore >= (g.ContainsKey(current) ? g[current] : double.PositiveInfinity))
{
continue;
}
if (!U.Contains(outLink.NodeEnd) ||
tentativeScore < (g.ContainsKey(current) ? g[current] : double.PositiveInfinity))
{
parent[outLink.NodeEnd] = current;
g[outLink.NodeEnd] = tentativeScore;
f[outLink.NodeEnd] = g[outLink.NodeEnd] + Distance(outLink.NodeEnd, end);
if (!Q.ContainsValue(outLink.NodeEnd))
{
Q.Add(f[outLink.NodeEnd], outLink.NodeEnd);
}
}
}
}
return(null);
}
public void AddLink(Node start, Node end)
{
LinkNode.NewLink(start, end);
}
