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);
}
}
public void KeyAt_WhenIndexIsInQueue_WillReturnKeyAtIndex()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
double keyAtIndex = queue.KeyAt(3);
Assert.AreEqual(40.12, keyAtIndex);
}
public void MinIndex_WhenMultipleKeysAreInQueue_WillReturnIndexOfMinimumKey()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
int minIndex = queue.MinIndex();
Assert.AreEqual(7, minIndex);
}
public void MinKey_WhenMultipleKeysAreInQueue_WillReturnMinimumKey()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
double minKey = queue.MinKey();
Assert.AreEqual(4.3, minKey);
}
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);
}
public void Delete_WhenIndexIsInQueue_WillRemoveKeyFromQueue()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
queue.Delete(7);
double keyAtIndex = queue.KeyAt(7);
Assert.AreEqual(default(double), keyAtIndex);
}
/// <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);
}
}
public void ChangeKey_WhenIndexIsInQueue_WillChangeOldKeyToNewKey()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
double newKey = 5.54;
queue.ChangeKey(3, newKey);
double keyAtIndex = queue.KeyAt(3);
Assert.AreEqual(newKey, keyAtIndex);
}
private void Visit(EdgeWeightedGraph g, int v)
{
marked[v] = true;
foreach (var e in g.Adj(v))
{
int w = e.Other(v);
if (marked[w])
{
continue;
}
if (e.Weight < distTo[w])
{
edgeTo[w] = e;
distTo[w] = e.Weight;
if (pq.Contains(w))
{
pq.ChangeKey(w, distTo[w]);
}
else
{
pq.Insert(w, distTo[w]);
}
}
}
}
public void Contains_Index4AfterInsertingKeyAtIndex4_WillBeTrue()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
Assert.IsTrue(queue.Contains(4));
}
public void Size_AfterInserting1Key_WillBe1()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
Assert.AreEqual(1, queue.Size);
}
public void Reset()
{
EdgeTo = new Edge[_graph.Vertices];
DistTo = new int[_graph.Vertices];
Marked = new bool[_graph.Vertices];
_priorityQueue = new IndexMinPriorityQueue <int>(_graph.Vertices);
// Copy the int.maxValue to DistTo by copying the bytes - *should* be faster
Buffer.BlockCopy(_baseDistTo, Sizeofint, DistTo, Sizeofint, Sizeofint * (_graph.Vertices - 1));
_priorityQueue.Insert(0, 0);
}
public DijkstraShortestPath(EdgeWeightedDigraph g, int s)
{
edgeTo = new DirectedEdge[g.Vcount];
distTo = new double[g.Vcount];
pq = new IndexMinPriorityQueue <double>(g.Vcount);
for (int i = 0; i < g.Vcount; i++)
{
distTo[i] = double.PositiveInfinity;
}
distTo[0] = 0.0;
pq.Insert(s, 0.0);
while (!pq.IsEmpty())
{
Relax(g, pq.DelMin());
}
}
public PrimMinimumSpanTree(EdgeWeightedGraph g)
{
edgeTo = new Edge[g.Vcount];
distTo = new double[g.Vcount];
marked = new bool[g.Vcount];
for (int i = 0; i < g.Vcount; i++)
{
distTo[i] = double.PositiveInfinity;
}
pq = new IndexMinPriorityQueue <double>(g.Vcount);
distTo[0] = 0.0;
pq.Insert(0, 0.0);
while (!pq.IsEmpty())
{
Visit(g, pq.DelMin());
}
}
public void IndexMinPrivorityQueueTest()
{
string[] strings1 = { "it", "was", "the", "best", "of", "times", "it", "was", "the", "worst" };
string[] strings2 = new string[strings1.Length];
Array.Copy(strings1, strings2, strings1.Length);
var queue = new IndexMinPriorityQueue <string>(strings1.Length);
for (int i = 0; i < strings1.Length; i++)
{
queue.Insert(i, strings1[i]);
}
Quick <string> .SimpleSort(strings2);
for (int i = 0; i < strings1.Length; i++)
{
int index = queue.DelMin();
Assert.Equal(strings1[index], strings2[i]);
}
}
private void Relax(DirectedEdge edge)
{
uint v = edge.From;
uint w = edge.To;
if (_distanceTo[w] > _distanceTo[v] + edge.Weight)
{
_distanceTo[w] = _distanceTo[v] + edge.Weight;
_edgeTo[w] = edge;
if (_priorityQueue.Contains((int)w))
{
_priorityQueue.DecreaseKey((int)w, _distanceTo[w]);
}
else
{
_priorityQueue.Insert((int)w, _distanceTo[w]);
}
}
}
private void Relax(EdgeWeightedDigraph g, int v)
{
foreach (var e in g.Adj(v))
{
int w = e.To;
if (distTo[w] > distTo[v] + e.Weight)
{
distTo[w] = distTo[v] + e.Weight;
edgeTo[w] = e;
if (pq.Contains(w))
{
pq.ChangeKey(w, distTo[w]);
}
else
{
pq.Insert(w, distTo[w]);
}
}
}
}
private void Visit(EdgeWeightedGraph graph, int vertex, ISet <int> excludes)
{
Marked[vertex] = true;
foreach (var edge in graph.Edges[vertex].Where(edge => !excludes.Contains(edge.Id)))
{
var other = edge.OtherVertex(vertex);
if (Marked[other] || edge.Weight >= DistTo[other])
{
continue;
}
EdgeTo[other] = edge;
DistTo[other] = edge.Weight;
if (_priorityQueue.Contains(other))
{
_priorityQueue.Change(other, DistTo[other]);
}
else
{
_priorityQueue.Insert(other, DistTo[other]);
}
}
}
/// <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);
}