private static IEnumerable <Point> FindPathInternal(DijkstraTile[,] dijkstra, Point end)
{
DijkstraTile dTile = dijkstra[end.X, end.Y];
while (dTile.Previous != null)
{
yield return(dTile.Tile);
dTile = dTile.Previous;
}
}
public static Point FindStart(this DijkstraTile[,] dijkstra, Point end)
{
DijkstraTile dTile = dijkstra[end.X, end.Y];
while (dTile.Previous != null)
{
dTile = dTile.Previous;
}
return(dTile.Tile);
}
public static DijkstraTile[,] Dijkstra(IEnumerable <Point> start, int width, int height, double maxDist, Func <Point, Point, double> length, Func <Point, IEnumerable <Point> > neighbors)
{
var dijkstraMap = new DijkstraTile[width, height];
var nodeMap = new FibonacciHeapNode <DijkstraTile, double> [width, height];
var heap = new FibonacciHeap <DijkstraTile, double>(0);
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
Point tile = new Point(x, y);
bool isStart = start.Contains(tile);
DijkstraTile dTile = new DijkstraTile(tile, isStart ? 0 : double.PositiveInfinity, isStart ? 0 : double.PositiveInfinity);
var node = new FibonacciHeapNode <DijkstraTile, double>(dTile, dTile.Distance);
dijkstraMap[x, y] = dTile;
nodeMap[x, y] = node;
heap.Insert(node);
}
}
while (!heap.IsEmpty())
{
var node = heap.RemoveMin();
var dTile = node.Data;
if (dTile.Distance >= maxDist)
{
break;
}
foreach (var neighbor in neighbors(dTile.Tile))
{
if (neighbor.X < 0 || neighbor.Y < 0 || neighbor.X >= width || neighbor.Y >= height)
{
continue;
}
var nodeNeighbor = nodeMap[neighbor.X, neighbor.Y];
var dNeighbor = nodeNeighbor.Data;
double newDist = dTile.Distance + length(dTile.Tile, dNeighbor.Tile);
if (newDist < dNeighbor.Distance)
{
dNeighbor.Distance = newDist;
dNeighbor.Previous = dTile;
dNeighbor.MoveDistance = dTile.MoveDistance + 1;
heap.DecreaseKey(nodeNeighbor, dNeighbor.Distance);
}
}
}
return(dijkstraMap);
}
public static bool Reachable(this DijkstraTile[,] dijkstra, Tile end)
{
DijkstraTile dTile = dijkstra[end.X, end.Y];
return(dTile.Previous != null);
}
