public IEnumerable <T> Search(T start, T goal)
{
var closedSet = new HashSet <T>();
var openSet = new HashSet <T> {
start
};
var cameFrom = new Dictionary <T, T>();
var gScore = new MapWithDefaultValueOfInfinity {
[start] = 0
};
var fScore = new MapWithDefaultValueOfInfinity();
while (openSet.Any())
{
var current = NodeHavingTheLowestValue(openSet, fScore);
if (current.Equals(goal))
{
return(ReconstructPath(current, cameFrom));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var neighbor in _neighborsProvider.Get(current))
{
if (closedSet.Contains(neighbor))
{
continue;
}
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
var tentativeGScore = gScore[current] + _distanceBetweenProvider.Get(current, neighbor);
if (tentativeGScore > gScore[neighbor])
{
continue;
}
cameFrom[neighbor] = current;
gScore[neighbor] = tentativeGScore;
fScore[neighbor] = gScore[neighbor] + _heuristicCostEstimateProvider.Get(neighbor, goal);
}
}
return(null);
}
private static T NodeHavingTheLowestValue(IEnumerable <T> nodes, MapWithDefaultValueOfInfinity values)
{
T result = null;
foreach (var node in nodes)
{
if (result == null || values[node] < values[result])
{
result = node;
}
}
if (result == null)
{
throw new Exception("Something goes wrong.");
}
return(result);
}
