public IEnumerable <Vertex <T> > Dijkstra(Vertex <T> start, Vertex <T> end)
{
if (!(vertices.Contains(start) && vertices.Contains(end)))
{
return(null);
}
var info = new Dictionary <Vertex <T>, (Vertex <T> founder, float distance)>();
var queue = new PriorityQue <Vertex <T> >(Comparer <Vertex <T> > .Create((a, b) => info[a].distance.CompareTo(info[b].distance)));
for (int i = 0; i < Count; i++)
{
vertices[i].IsVisited = false;
info.Add(vertices[i], (null, int.MaxValue));
}
info[start] = (null, 0);
queue.Enqueu(start);
while (!queue.IsEmpty())
{
var current = queue.Deqeue();
current.IsVisited = true;
foreach (var edge in current.Neighbors)
{
var neighbor = edge.EndingPoint;
float tentative = edge.Distance + info[current].distance;
if (tentative < info[neighbor].distance)
{
info[neighbor] = (current, tentative);
neighbor.IsVisited = false;
}
if (!queue.Contains(neighbor) && !neighbor.IsVisited)
{
queue.Enqueu(neighbor);
}
}
if (current == end)
{
break;
}
}
var path = new Stack <Vertex <T> >();
var currentV = info.Where(x => x.Key == end).First();
while (true)
{
path.Push(currentV.Key);
if (currentV.Value.founder == null)
{
if (path.Count == 1)
{
return(null);
}
break;
}
currentV = new KeyValuePair <Vertex <T>, (Vertex <T> founder, float distance)>(currentV.Value.founder, info[currentV.Value.founder]);
}
return(path);
}
public static IEnumerable <Vertex <Point> > AStarAlgo(Graph <Point> graph, Vertex <Point> start, Vertex <Point> end, Heuristics heuristic = Heuristics.Manhattan)
{
if (!(graph.vertices.Contains(start) && graph.vertices.Contains(end)))
{
return(null);
}
var info = new Dictionary <Vertex <Point>, (Vertex <Point> founder, float distance, float finalDistance)>();
var queu = new PriorityQue <Vertex <Point> >(Comparer <Vertex <Point> > .Create((a, b) => info[a].finalDistance.CompareTo(info[b].finalDistance)));
foreach (var vertex in graph.vertices)
{
vertex.IsVisited = false;
info.Add(vertex, (null, float.PositiveInfinity, float.PositiveInfinity));
}
info[start] = (null, 0, HeuristicsFunc(heuristic, start.Value, end.Value));
queu.Enqueu(start);
while (queu.Count > 0)
{
var current = queu.Deqeue();
current.IsVisited = false;
foreach (var edge in current.Neighbors)
{
var neighbor = edge.EndingPoint;
float tentative = edge.Distance + info[start].distance;
if (tentative < info[neighbor].distance)
{
neighbor.IsVisited = false;
info[neighbor] = (current, tentative, HeuristicsFunc(heuristic, current.Value, neighbor.Value));
}
if (!queu.Contains(neighbor) && !neighbor.IsVisited)
{
queu.Enqueu(neighbor);
}
}
if (current == end)
{
break;
}
}
var path = new Stack <Vertex <Point> >();
var currentV = info.Where(x => x.Key == end).First();
while (true)
{
path.Push(currentV.Key);
if (currentV.Value.founder == null)
{
if (path.Count == 1)
{
return(null);
}
break;
}
currentV = new KeyValuePair <Vertex <Point>, (Vertex <Point> founder, float distance, float finalDistance)>();
}
return(path);
}
