private void FindPath(IGraph <T> graph, T src, T dest, float greediness)
{
if (!graph.Accessible(src, dest))
{
return;
}
visited.Add(src, new NodeData(src, 0));
queue.Enqueue(src, 0);
while (!visited.ContainsKey(dest) && queue.Count > 0)
{
T node = queue.Dequeue();
float dist = visited[node].Dist;
// Visit all neighbours
foreach (T neighbour in graph.GetConnections(node, src, dest))
{
var g = dist + graph.GetCost(node, neighbour);
var closed = visited.TryGetValue(neighbour, out NodeData v);
if (!closed || v.Dist > g)
{
visited[neighbour] = new NodeData(node, g);
}
else
{
continue;
}
var f = (1 - greediness) * g + greediness * graph.EstimateCost(neighbour, dest);
queue.Enqueue(neighbour, f);
}
}
queue.Clear();
}
public List <T>?FindPath(IGraph <T> graph, T src, T dest)
{
if (!graph.Accessible(src, dest))
{
return(null);
}
visited.Add(src, new NodeData(src, 0));
queue.Enqueue(src, 0);
while (!visited.ContainsKey(dest) && queue.Count > 0)
{
T node = queue.Dequeue();
float dist = visited[node].Dist;
T cameFrom = visited[node].CameFrom;
// Visit all neighbours
foreach (T neighbour in graph.GetConnections(cameFrom, node))
{
var g = dist + graph.CalculateCost(node, neighbour);
visited.TryGetValue(neighbour, out NodeData? v);
if (v == null)
{
visited[neighbour] = new NodeData(node, g);
}
else if (v.Dist <= g)
{
continue;
}
else
{
v.Dist = g;
v.CameFrom = node;
}
var f = (1 - greed) * g + greed * graph.EstimateCost(neighbour, dest);
queue.Enqueue(neighbour, f);
}
}
var path = ReconstructPath(src, dest);
queue.Clear();
visited.Clear();
return(path);
}