public static bool AStar <V, E>(
this Graph <V, E> graph,
int from,
int to,
Heuristics heuristics,
WeightCalculator <E> calculator,
out GraphPath <V, E> path,
ExplorationCallbacks <V, E> callbacks = default
)
{
var gScore = new Dictionary <int, float> {
{ from, 0 }
};
var fScore = new Dictionary <int, float> {
{ from, heuristics(from, to) }
};
var open = new PriorityQueue <int, float>();
open.Enqueue(from, fScore[from]);
var history = new Dictionary <int, int>();
while (open.Count > 0)
{
var i = open.Dequeue();
if (i == to)
{
path = ReTrace(history, i, graph);
return(true);
}
var edges = graph.EdgesFrom(i).ToArray();
foreach (var(edge, neighborIndex) in edges)
{
callbacks.OnVisit?.Invoke(i, neighborIndex, edge);
var found = callbacks.EdgeFilter?.Invoke(i, neighborIndex, edge);
if (!found.GetValueOrDefault(true))
{
continue;
}
var attempt = gScore[i] + calculator(i, neighborIndex, edge);
if (!gScore.ContainsKey(neighborIndex) || attempt < gScore[neighborIndex])
{
history[neighborIndex] = i;
gScore[neighborIndex] = attempt;
float neighborFScore = attempt + heuristics(neighborIndex, to);
fScore[neighborIndex] = neighborFScore;
callbacks.OnSelected?.Invoke(i, neighborIndex, edge);
if (!open.Contains(neighborIndex))
{
open.Enqueue(neighborIndex, neighborFScore);
}
}
}
}
path = null;
return(false);
}
public static bool AStar <V>(
this Graph <V, double> graph,
int from,
int to,
Heuristics heuristics,
out GraphPath <V, double> path,
ExplorationCallbacks <V, double> callbacks = default
)
{
return(graph.AStar(from, to, heuristics, (i, to1, edge) => (float)edge, out path, callbacks));
}
public void Reload(GraphPath <V, E> path)
{
if (path == currentPath)
{
return;
}
currentPath = path;
Current = 0;
OnReloaded.Invoke();
}
public static bool AStar <V, E>(
this Graph <V, E> graph,
int from,
int to,
Heuristics heuristics,
WeightCalculator <E> calculator,
out GraphPath <V, E> path,
ExplorationCallbacks <V, E> callbacks = default
)
{
var gScore = new Dictionary <int, float> {
{ from, 0 }
};
var fScore = new Dictionary <int, float> {
{ from, heuristics(from, to) }
};
var open = new SortedSet <int>(Comparer <int> .Create((first, second) => {
var a = fScore[first];
var b = fScore[second];
return(a.CompareTo(b));
}))
{
from
};
var history = new Dictionary <int, int>();
while (!open.IsEmpty())
{
var i = open.First();
if (i == to)
{
path = ReTrace(history, i, graph);
return(true);
}
open.Remove(i);
var edges = graph.EdgesFrom(i).ToArray();
foreach (var(edge, neighborIndex) in edges)
{
callbacks.OnVisit?.Invoke(i, neighborIndex, edge);
var found = callbacks.EdgeFilter?.Invoke(i, neighborIndex, edge);
if (!found.GetValueOrDefault(true))
{
continue;
}
var attempt = gScore[i] + calculator(i, neighborIndex, edge);
if (!gScore.ContainsKey(neighborIndex) || attempt < gScore[neighborIndex])
{
history[neighborIndex] = i;
gScore[neighborIndex] = attempt;
fScore[neighborIndex] = attempt + heuristics(neighborIndex, to);
callbacks.OnSelected?.Invoke(i, neighborIndex, edge);
if (!open.Contains(neighborIndex))
{
open.Add(neighborIndex);
}
}
}
}
path = null;
return(false);
}
public static bool AStar <V, E>(
this Graph <V, E> graph,
int from,
int to,
Heuristics <V, E> heuristics,
out GraphPath <V, E> path,
AStarCallbacks <V, E> callbacks = default
) where E : IWeighted <float>
{
var gScore = new Dictionary <int, float> {
{ from, 0 }
};
var fScore = new Dictionary <int, float> {
{ from, heuristics(graph, from, to) }
};
var open = new List <int> {
from
};
var history = new Dictionary <int, int>();
while (!open.IsEmpty())
{
var i = open.MinBy(candidate => fScore[candidate]);
if (i == to)
{
path = ReTrace(history, i, graph);
return(true);
}
open.Remove(i);
var edges = graph.EdgesFrom(i).ToArray();
foreach (var t in edges)
{
var edge = t.Item1;
var ni = t.Item2;
callbacks.onVisit?.Invoke(graph, i, ni, edge);
var found = callbacks.edgeFilter?.Invoke(graph, i, ni, edge);
if (!found.GetValueOrDefault(true))
{
continue;
}
var attempt = gScore[i] + edge.Weight;
if (!gScore.ContainsKey(ni) || attempt < gScore[ni])
{
history[ni] = i;
gScore[ni] = attempt;
fScore[ni] = attempt + heuristics(graph, ni, to);
callbacks.onSelected?.Invoke(graph, i, ni, edge);
if (!open.Contains(ni))
{
open.Add(ni);
}
}
}
}
path = null;
return(false);
}
public GraphPlan(GraphPath <V, E> currentPath, int current = 0)
{
this.currentPath = currentPath;
this.current = current;
}