public bool Check(EdgeWeightedDigraph graph, int sourceVertex)
{
if (graph == null)
{
throw new ArgumentNullException("graph", "EdgeWeightedDigraph cannot be null");
}
if (_distanceTo[sourceVertex] != 0.0 || _edgeTo[sourceVertex] != null)
{
return(false);
}
for (int v = 0; v < graph.NumberOfVertices; v++)
{
if (v == sourceVertex)
{
continue;
}
if (_edgeTo[v] == null && _distanceTo[v] != double.PositiveInfinity)
{
return(false);
}
}
for (int v = 0; v < graph.NumberOfVertices; v++)
{
foreach (DirectedEdge edge in graph.Adjacent(v))
{
int w = edge.To;
if (_distanceTo[v] + edge.Weight < _distanceTo[w])
{
return(false);
}
}
}
for (int w = 0; w < graph.NumberOfVertices; w++)
{
if (_edgeTo[w] == null)
{
continue;
}
DirectedEdge edge = _edgeTo[w];
int v = edge.From;
if (w != edge.To)
{
return(false);
}
if (_distanceTo[v] + edge.Weight != _distanceTo[w])
{
return(false);
}
}
return(true);
}
public PathFinder(IMap map, double cardinalCost, double diagonalCost)
{
_map = map ?? throw new ArgumentNullException("map");
_graph = new EdgeWeightedDigraph(_map.Width * _map.Height);
foreach (Cell cell in _map.GetAllCells())
{
if (!cell.IsWalkable)
{
continue;
}
int cellIndex = IndexFor(cell.Point);
var neighborCoords = new List <(Point, double)>
{
(new Point(cell.Point.X + 0, cell.Point.Y - 1), cardinalCost),
(new Point(cell.Point.X + 0, cell.Point.Y + 1), cardinalCost),
(new Point(cell.Point.X + 1, cell.Point.Y + 0), cardinalCost),
(new Point(cell.Point.X - 1, cell.Point.Y + 0), cardinalCost),
(new Point(cell.Point.X - 1, cell.Point.Y - 1), diagonalCost),
(new Point(cell.Point.X - 1, cell.Point.Y + 1), diagonalCost),
(new Point(cell.Point.X + 1, cell.Point.Y - 1), diagonalCost),
(new Point(cell.Point.X + 1, cell.Point.Y + 1), diagonalCost),
};
foreach (var(coord, cost) in neighborCoords)
{
if (!map.IsWithinMap(coord))
{
continue;
}
if (!map.IsWalkable(coord))
{
continue;
}
int neighborIndex = IndexFor(coord);
_graph.AddEdge(new DirectedEdge(cellIndex, neighborIndex, cost));
}
}
}
private DijkstraShortestPath(EdgeWeightedDigraph graph, int sourceVertex, int?destinationVertex)
{
if (graph == null)
{
throw new ArgumentNullException("graph", "EdgeWeightedDigraph cannot be null");
}
foreach (DirectedEdge edge in graph.Edges())
{
if (edge.Weight < 0)
{
throw new ArgumentOutOfRangeException(string.Format("Edge: '{0}' has negative weight", edge));
}
}
_distanceTo = new double[graph.NumberOfVertices];
_edgeTo = new DirectedEdge[graph.NumberOfVertices];
for (int v = 0; v < graph.NumberOfVertices; v++)
{
_distanceTo[v] = Double.PositiveInfinity;
}
_distanceTo[sourceVertex] = 0.0;
_priorityQueue = new IndexMinPriorityQueue <double>(graph.NumberOfVertices);
_priorityQueue.Insert(sourceVertex, _distanceTo[sourceVertex]);
while (!_priorityQueue.IsEmpty())
{
int v = _priorityQueue.DeleteMin();
if (destinationVertex.HasValue && v == destinationVertex.Value)
{
return;
}
foreach (DirectedEdge edge in graph.Adjacent(v))
{
Relax(edge);
}
}
}
public PathFinder(IMap map)
{
_map = map ?? throw new ArgumentNullException("map", "Map cannot be null");
_graph = new EdgeWeightedDigraph(_map.Width * _map.Height);
foreach (Cell cell in _map.GetAllCells())
{
if (cell.IsWalkable)
{
int v = IndexFor(cell.Point);
foreach (Cell neighbor in _map.GetBorderCellsInDiamond(cell.Point, 1))
{
if (neighbor.IsWalkable)
{
int w = IndexFor(neighbor.Point);
_graph.AddEdge(new DirectedEdge(v, w, 1.0));
_graph.AddEdge(new DirectedEdge(w, v, 1.0));
}
}
}
}
}
public static IEnumerable <DirectedEdge> FindPath(EdgeWeightedDigraph graph, int sourceVertex, int destinationVertex)
{
var dijkstraShortestPath = new DijkstraShortestPath(graph, sourceVertex, destinationVertex);
return(dijkstraShortestPath.PathTo(destinationVertex));
}
public DijkstraShortestPath(EdgeWeightedDigraph graph, int sourceVertex)
: this(graph, sourceVertex, null)
{
}