public static string Visualize <TK, TV>(this IReadableGraph <TK, TV> graph)
{
var sb = "";
foreach (var keyValuePair in graph.ConnectionsOf)
{
sb += $"{keyValuePair.Key.Key}\n";
sb = keyValuePair.Value.Aggregate(sb, (current, valuePair) => current + $" {valuePair.Key}\n");
}
return(sb);
}
public static IEnumerable <GraphNode <TK, TV> > AStar <TK, TV>(
this IReadableGraph <TK, TV> graph,
TK from,
TK to,
HeuristicFunction <TK, TV> heuristicFunction
)
where TK : IEquatable <TK>
{
var fromNode = graph[from];
var toNode = graph[to];
var closedSet = new HashSet <GraphNode <TK, TV> >();
var openSet = new HashSet <GraphNode <TK, TV> > {
fromNode
};
var cameFrom = new Dictionary <GraphNode <TK, TV>, GraphNode <TK, TV> >();
var gScore = new Dictionary <GraphNode <TK, TV>, double>();
var fScore = new Dictionary <GraphNode <TK, TV>, double>();
foreach (var node in graph.Nodes)
{
cameFrom[node] = null;
gScore[node] = PositiveInfinity;
fScore[node] = PositiveInfinity;
}
gScore[fromNode] = 0;
fScore[fromNode] = heuristicFunction(graph, fromNode, toNode);
while (openSet.Count > 0)
{
var current = openSet.First(x => x.Equals(fScore.MinBy(kv => kv.Value).Key));
if (current.Equals(toNode))
{
return(ReconstructPath(cameFrom, current));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var connection in graph.ConnectionsOf[current])
{
if (closedSet.Contains(connection.Key))
{
continue;
}
var tentativeGScore = gScore[current] + connection.Value;
if (!openSet.Contains(connection.Key))
{
openSet.Add(connection.Key);
}
else if (tentativeGScore >= gScore[connection.Key])
{
continue;
}
cameFrom[connection.Key] = current;
gScore[connection.Key] = tentativeGScore;
fScore[connection.Key] = gScore[connection.Key] + heuristicFunction(graph, connection.Key, toNode);
}
}
return(new GraphNode <TK, TV> [0]);
}
public static void VisualizeToConsole <TK, TV>(this IReadableGraph <TK, TV> graph)
{
Console.WriteLine(graph.Visualize());
}
public static IEnumerable <GraphNode <TK, TV> > Dijkstra <TK, TV>(this IReadableGraph <TK, TV> graph, TK from, TK to)
where TK : IEquatable <TK>
{
if (graph[from] is null)
{
throw new ArgumentException($"Invalid from key {from}");
}
if (graph[to] is null)
{
throw new ArgumentException($"Invalid from key {to}");
}
var distance = new Dictionary <GraphNode <TK, TV>, double> {
[graph[from]] = 0
};
var previous = new Dictionary <GraphNode <TK, TV>, GraphNode <TK, TV> >();
var queue = new PriorityQueue <double, GraphNode <TK, TV> >();
foreach (var node in graph.Nodes)
{
if (!node.Key.Equals(from))
{
distance[node] = double.MaxValue;
}
previous[node] = null;
queue.Add(node, distance[node]);
}
while (queue.Count > 0)
{
var node = queue.PollLowest();
if (node.Equals(to))
{
IList <GraphNode <TK, TV> > list = new List <GraphNode <TK, TV> >();
var node = graph[to];
while (node != null)
{
list.Insert(0, node);
node = previous[node];
}
return(list);
}
foreach (var neighbor in graph.ConnectionsOf[node.Key])
{
var newDistance = distance[node] + neighbor.Value;
if (!(newDistance < distance[neighbor.Key.Key]))
{
continue;
}
distance[neighbor.Key.Key] = newDistance;
previous[neighbor.Key.Key] = graph[node];
queue.UpdatePriority(neighbor.Key.Key, newDistance);
}
}
return(null);
}
