/// <summary>
/// Computes a shortest paths tree from the specified sourceVertex to every other vertex in the edge-weighted directed graph
/// </summary>
/// <param name="graph">The edge-weighted directed graph</param>
/// <param name="sourceVertex">The source vertex to compute the shortest paths tree from</param>
/// <exception cref="ArgumentOutOfRangeException">Throws an ArgumentOutOfRangeException if an edge weight is negative</exception>
/// <exception cref="ArgumentNullException">Thrown if EdgeWeightedDigraph is null</exception>
public DijkstraShortestPath(EdgeWeightedDigraph graph, int sourceVertex)
{
if (graph == null)
{
throw new ArgumentNullException("graph", "EdgeWeightedDigraph cannot be null");
}
foreach (DirectedEdge edge in graph.Edges())
{
if (edge.Weight < 0)
{
throw new ArgumentOutOfRangeException($"Edge: '{edge}' has negative weight");
}
}
_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();
foreach (DirectedEdge edge in graph.Adjacent(v))
{
Relax(edge);
}
}
}
/// <summary>
/// Computes a shortest paths tree from the specified sourceVertex to every other vertex in the edge-weighted directed graph
/// </summary>
/// <param name="graph">The edge-weighted directed graph</param>
/// <param name="sourceVertex">The source vertex to compute the shortest paths tree from</param>
/// <exception cref="ArgumentOutOfRangeException">Throws an ArgumentOutOfRangeException if an edge weight is negative</exception>
/// <exception cref="ArgumentNullException">Thrown if EdgeWeightedDigraph is null</exception>
public DijkstraShortestPath( EdgeWeightedDigraph graph, int sourceVertex )
{
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();
foreach ( DirectedEdge edge in graph.Adjacent( v ) )
{
Relax( edge );
}
}
}
public ShortestPathTree(EdgeWeightedDirectedGraph G, int s)
{
foreach (DirectedEdge e in G.Edges())
{
if (e.Weight < 0)
throw new ArgumentException("edge " + e + " has negative weight");
}
distTo = new double[G.VerticesCount];
edgeTo = new DirectedEdge[G.VerticesCount];
for (int v = 0; v < G.VerticesCount; v++)
distTo[v] = Double.PositiveInfinity;
distTo[s] = 0.0;
// relax vertices in order of distance from s
pq = new IndexMinPriorityQueue<Double>(G.VerticesCount);
pq.Insert(s, distTo[s]);
while (!pq.IsEmpty)
{
int v = pq.DeleteMin();
foreach (DirectedEdge e in G.AdjacentsOf(v))
Relax(e);
}
}
private void Prim(EdgeWeightedGraph G, int s)
{
_distTo[s] = 0.0;
_pq.Insert(s, _distTo[s]);
while (!_pq.IsEmpty())
{
int v = _pq.DeleteMin();
Scan(G, v);
}
}
List <List <int> > GetMST()
{
var size = nodes.NodeList.Count;
var parents = new int[size];
var weights = new float[size];
var marked = new bool[size];
var pq = new IndexMinPriorityQueue <float>(size);
for (var x = 1; x < size; x++)
{
weights[x] = float.MaxValue;
}
weights[0] = 0;
pq.Insert(0, 0);
while (!pq.IsEmpty())
{
int v = pq.DeleteMin();
marked[v] = true;
for (var x = 0; x < size; x++)
{
if (x != v && !marked[x])
{
var distance = Vector2.Distance(nodes.NodeList[v], nodes.NodeList[x]);
if (distance < weights[x])
{
weights[x] = distance;
parents[x] = v;
if (pq.Contains(x))
{
pq.ChangeKey(x, distance);
}
else
{
pq.Insert(x, distance);
}
}
}
}
}
var result = GetAdjacencyLists(size);
for (var x = 1; x < parents.Length; x++)
{
var target = parents[x];
result[x].Add(target);
result[target].Add(x);
}
return(result);
}
/// <summary>
/// Returns an List of Cells representing a shortest path from the specified source to the specified destination
/// </summary>
/// <param name="source">The source Cell to find a shortest path from</param>
/// <param name="destination">The destination Cell to find a shortest path to</param>
/// <param name="map">The Map on which to find the shortest path between Cells</param>
/// <returns>List of Cells representing a shortest path from the specified source to the specified destination</returns>
public List <TCell> FindPath(TCell source, TCell destination, IMap <TCell> map, Func <TCell, TCell, bool> ValidStep)
{
// OPEN = the set of nodes to be evaluated
IndexMinPriorityQueue <PathNode> openNodes = new IndexMinPriorityQueue <PathNode>(map.Height * map.Width);
// CLOSED = the set of nodes already evaluated
bool[] isNodeClosed = new bool[map.Height * map.Width];
// add the start node to OPEN
openNodes.Insert(map.IndexFor(source), new PathNode {
DistanceFromStart = 0,
HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
X = source.X,
Y = source.Y,
Parent = null
});
PathNode currentNode;
// loop
while (true)
{
// current = node in OPEN with the lowest f_cost
if (openNodes.Size < 1)
{
return(null);
}
currentNode = openNodes.MinKey();
// remove current from OPEN
int currentIndex = openNodes.DeleteMin();
// add current to CLOSED
isNodeClosed[currentIndex] = true;
ICell currentCell = map.CellFor(currentIndex);
// if current is the target node the path has been found
if (currentCell.Equals(destination))
{
break;
}
// foreach neighbor of the current node
bool includeDiagonals = _diagonalCost.HasValue;
foreach (TCell neighbor in map.GetAdjacentCells(currentCell.X, currentCell.Y, includeDiagonals))
{
int neighborIndex = map.IndexFor(neighbor);
// if neighbor is not walkable or neighbor is in CLOSED
if (!ValidStep(neighbor, destination) || isNodeClosed[neighborIndex])
{
// skip to the next neighbor
continue;
}
bool isNeighborInOpen = openNodes.Contains(neighborIndex);
// if neighbor is in OPEN
if (isNeighborInOpen)
{
// if new path to neighbor is shorter
PathNode neighborNode = openNodes.KeyAt(neighborIndex);
double newDistance = currentNode.DistanceFromStart + 1;
if (newDistance < neighborNode.DistanceFromStart)
{
// update neighbor distance
neighborNode.DistanceFromStart = newDistance;
// set parent of neighbor to current
neighborNode.Parent = currentNode;
}
}
else // if neighbor is not in OPEN
{
// set f_cost of neighbor
// set parent of neighbor to current
PathNode neighborNode = new PathNode {
DistanceFromStart = currentNode.DistanceFromStart + 1,
HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
X = neighbor.X,
Y = neighbor.Y,
Parent = currentNode
};
// add neighbor to OPEN
openNodes.Insert(neighborIndex, neighborNode);
}
}
}
List <TCell> path = new List <TCell>();
path.Add(map.GetCell(currentNode.X, currentNode.Y));
while (currentNode.Parent != null)
{
currentNode = currentNode.Parent;
path.Add(map.GetCell(currentNode.X, currentNode.Y));
}
path.Reverse();
return(path);
}