public static List <DijkstraVertex> GetDijkstraVertices(bool[,] graph, Position from, double speed)
{
var start = new DijkstraVertex {
Position = from, SpeedLeft = speed, Previous = from
};
DijkstraVertices.Clear();
FindDijkstraVertices(graph, start);
return(DijkstraVertices);
}
private static void FindDijkstraVertices(bool[,] graph, DijkstraVertex start)
{
foreach (var offset in DiagonalOffset ? Offsets8 : Offsets4)
{
var currentPosition = start.Position;
var nextPosition = start.Position + offset;
if (nextPosition.X < 0 || nextPosition.X >= graph.GetLength(0) || nextPosition.Y < 0 || nextPosition.Y >= graph.GetLength(1))
{
continue;
}
if (!graph[currentPosition.X, currentPosition.Y] || !graph[nextPosition.X, nextPosition.Y])
{
continue;
}
var speedLeft = start.SpeedLeft - (DiagonalOffset ? DiagonalSpeedCost : 1);
if (speedLeft < 0)
{
continue;
}
var vertex = DijkstraVertices.FirstOrDefault(i => i.Position == nextPosition);
if (vertex == null)
{
vertex = new DijkstraVertex
{
Position = nextPosition.Copy(),
SpeedLeft = speedLeft,
Previous = currentPosition.Copy(),
};
DijkstraVertices.Add(vertex);
FindDijkstraVertices(graph, vertex);
}
else if (vertex.SpeedLeft < speedLeft)
{
vertex.SpeedLeft = speedLeft;
vertex.Previous = currentPosition.Copy();
FindDijkstraVertices(graph, vertex);
}
}
}