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) { }