/// <summary>
/// The Dijkstra's algorithm.
/// </summary>
private void _dijkstra()
{
while (!_minPriorityQueue.IsEmpty)
{
var currentVertex = _minPriorityQueue.DequeueMin();
var currentVertexIndex = _nodesToIndices[currentVertex];
var outgoingEdges = _graph.OutgoingEdges(currentVertex);
foreach (var outgoingEdge in outgoingEdges)
{
var adjacentIndex = _nodesToIndices[outgoingEdge.Destination];
var delta = _distances[currentVertexIndex] != Infinity ? _distances[currentVertexIndex] + outgoingEdge.Weight : Infinity;
if (delta < _distances[adjacentIndex])
{
_distances[adjacentIndex] = delta;
_predecessors[adjacentIndex] = currentVertexIndex;
if (_minPriorityQueue.Contains(outgoingEdge.Destination))
{
_minPriorityQueue.UpdatePriority(outgoingEdge.Destination, delta);
}
else
{
_minPriorityQueue.Enqueue(outgoingEdge.Destination, delta);
}
}
}
}
}
private void Scan(IGraph g, int v)
{
_marked[v] = true;
foreach (var e in g.Neighbours(v))
{
var w = e.Target;
if (_marked[w])
{
continue;
}
if (e.Weight < _distTo[w])
{
_distTo[w] = e.Weight;
_edgeTo[w] = e;
if (_pq.Contains(w))
{
_pq.ChangeKey(w, _distTo[w]);
}
else
{
_pq.Insert(w, _distTo[w]);
}
}
}
}
private void Relax(Edge edge)
{
//get the source and target vertex of the edge
int v = edge.Source, w = edge.Target;
/*
* _distTo[w] contains the shortest path so far to the
* vertex w so we can compare it to the weight of of going through
* v, if it is less use the new path instead
* */
if (_distanceTo[w] > _distanceTo[v] + edge.Weight)
{
//set the distance to w to the new(lower) weight
_distanceTo[w] = _distanceTo[v] + edge.Weight;
//add the edge to the list of edges in our shortest paths
_edgeTo[w] = edge;
if (_priorityQueue.Contains(w))
{
/*
* if w is already in the priority que update its minimum path and re-order the que
* */
_priorityQueue.DecreaseKey(w, _distanceTo[w]);
}
else
{
_priorityQueue.Insert(w, _distanceTo[w]);
}
}
}
public void Test_Contains_Not()
{
PriorityQueue <double> q = null;
q = new MinPriorityQueue <double>();
q.Enqueue(3);
q.Enqueue(2);
q.Enqueue(1);
Assert.IsFalse(q.Contains(4));
}
// Implementation of Dijkstra's Algorithm
public List <Tile> FindPath(Tile start, Tile end)
{
MinPriorityQueue <Tile, int> queue = new MinPriorityQueue <Tile, int>();
// from prev tile to cur tile dictionary
Dictionary <Tile, Tile> prev = new Dictionary <Tile, Tile>();
foreach (Tile t in tiles)
{
if (!t.IsWalkable && t != start)
{
continue;
}
queue.Add(t, int.MaxValue);
prev.Add(t, null);
}
queue.Add(start, 0);
while (queue.Count > 0)
{
int uDist;
Tile u = queue.Pop(out uDist);
foreach (Tile v in u.neighborTiles)
{
if (!queue.Contains(v))
{
continue;
}
int vDist = uDist + 1;
if (vDist < queue.GetValue(v))
{
queue.Add(v, vDist);
prev[v] = u;
}
}
}
Tile cur = prev[end];
List <Tile> path = new List <Tile>();
while (cur != null)
{
path.Insert(0, cur);
cur = prev[cur];
}
return(path);
}
/************************************************************************************************************/
/// <summary>
/// The Dijkstra's algorithm.
/// </summary>
private void _dijkstra(TGraph graph, TVertex source)
{
var minPQ = new MinPriorityQueue <TVertex, decimal>((uint)_verticesCount);
var srcIndex = _nodesToIndices[source];
_srcIndex = srcIndex;
_distances[srcIndex] = 0;
minPQ.Enqueue(source, _distances[srcIndex]);
// Main loop
while (!minPQ.IsEmpty)
{
var current = minPQ.DequeueMin(); // get vertex with min weight
var currentIndex = _nodesToIndices[current]; // get its index
var edges = graph.OutgoingEdges(current) as IEnumerable <WeightedEdge <TVertex> >; // get its outgoing weighted edges
foreach (var edge in edges)
{
var adjacentIndex = _nodesToIndices[edge.Destination];
// calculate a new possible weighted path if the edge weight is less than infinity
var delta = Infinity;
if (edge.Weight < Infinity && _distances[currentIndex] + edge.Weight < Infinity) // Handles overflow
{
delta = _distances[currentIndex] + edge.Weight;
}
// Relax the edge
// if check is true, a shorter path is found from current to adjacent
if (delta < _distances[adjacentIndex])
{
_edgeTo[adjacentIndex] = edge;
_distances[adjacentIndex] = delta;
_predecessors[adjacentIndex] = currentIndex;
// decrease priority with a new distance if it exists; otherwise enqueque it
if (minPQ.Contains(edge.Destination))
{
minPQ.UpdatePriority(edge.Destination, delta);
}
else
{
minPQ.Enqueue(edge.Destination, delta);
}
}
} //end-foreach
} //end-while
}
/// <summary>
/// A* on the graph.
/// </summary>
/// <param name="pathStoreLoc">The Queue to store the path in.</param>
/// <param name="start">The starting node.</param>
/// <param name="end">The ending node.</param>
public double AStar(Queue<Node> pathStoreLoc, Node start, Node end, Node toIgnore = null)
{
MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>();
initializePathfinding(true);
if (toIgnore != null)
toIgnore.Visited = false;
System.Func<Node, Node, float> Heuristic;
if (allowedPaths == Paths.quadDir)
Heuristic = ManhattenHeuristic;
else if (allowedPaths == Paths.octDir)
Heuristic = DiagonalHeuristic;
else
Heuristic = DiagonalHeuristic;
start.CameFrom = null;
start.heuristicCost = Heuristic(start, end);
start.realCost = 0;
nodeList.Enqueue(start, start.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
StringBuilder encountered = new StringBuilder();
StringBuilder nodes = new StringBuilder();
nodes.Append("Start node ").Append(start.Number).AppendLine();
encountered.Append("Start node ").Append(start.Number).AppendLine();
nodes.Append("End node ").Append(end.Number).AppendLine();
encountered.Append("End node ").Append(end.Number).AppendLine();
#endif
while (nodeList.Count > 0)
{
//Pick the best looking node, by f-value.
Node best = nodeList.Dequeue();
double bestDist = best.realCost;
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine();
nodes.Append("Node ").Append(best.Number).AppendLine();
#endif
//If this is the end, stop, show the path, and return it.
if (best.Equals(end))
{
ReconstructPath(pathStoreLoc, end);
ShowPath(pathStoreLoc);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Finished!\n\nFinal dist: ")
.Append(best.realCost).AppendLine();
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return best.realCost;
}
best.Visited = true;
//string updateString = "updating: ";
foreach (Edge e in best.getEdges())
{
Node other = e.getNode();
//We already visited this node, move along,
if (other.Visited)
continue;
//Tentative distance.
double testDist = e.getWeight() + bestDist;
//If the other node isn't in the priority queue, add it.
if (!nodeList.Contains(other))
{
other.CameFrom = best;
other.realCost = testDist;
other.heuristicCost = Heuristic(other, end);
nodeList.Enqueue(other, other.realCost + other.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" added ").Append(other.Number)
.Append(", total estimated cost ")
.Append(other.realCost + other.heuristicCost)
.AppendLine();
#endif
continue;
}
//If the other node was a bad path, and this one's better, replace it.
else if (other.realCost > testDist)
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Update(other, other.realCost + other.heuristicCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" updated ").Append(other.Number)
.Append(", total new estimated cost ")
.Append(other.realCost + other.heuristicCost)
.AppendLine();
#endif
}
}
}
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Failed!\n");
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return double.PositiveInfinity;
}
/// <summary>
/// Returns a sorted list of nodes within a given endurance value.
/// Performs a Dijkstra-like algorithm.
/// </summary>
/// <param name="satifies">The predicate each node must follow.</param>
/// <param name="endurance">The maximum endurance to follow out.</param>
/// <returns>A sorted list of nodes within a given endurance value.</returns>
public List<Node> nodesThatSatisfyPred(Node startNode, System.Predicate<Node> satifies, float endurance = 16.0f, bool stopOnFirst = false, bool isPathfinding = true)
{
List<Node> foundNodes = new List<Node>();
MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>();
initializePathfinding(isPathfinding);
startNode.realCost = 0;
nodeList.Enqueue(startNode, startNode.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
StringBuilder encountered = new StringBuilder();
StringBuilder nodes = new StringBuilder();
nodes.Append("Start node ").Append(startNode.Number).AppendLine();
encountered.Append("Start node ").Append(startNode.Number).AppendLine();
encountered.Append("endurance = ").Append(endurance).AppendLine();
#endif
while (nodeList.Count > 0)
{
//Pick the best looking node, by f-value.
Node best = nodeList.Dequeue();
double bestDist = best.realCost;
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine();
nodes.Append("Node ").Append(best.Number).AppendLine();
#endif
best.Visited = true;
if (satifies(best))
{
foundNodes.Add(best);
if (stopOnFirst)
return foundNodes;
}
//string updateString = "updating: ";
foreach (Edge e in best.getEdges())
{
Node other = e.getNode();
//We already visited this node, move along,
if (other.Visited)
continue;
//Tentative distance.
double testDist = e.getWeight() + bestDist;
if (testDist > endurance)
continue;
//If the other node isn't in the priority queue, add it.
if (!nodeList.Contains(other))
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Enqueue(other, other.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" added ").Append(other.Number)
.Append(", total estimated cost ")
.Append(other.realCost).AppendLine();
#endif
continue;
}
//If the other node was a bad path, and this one's better, replace it.
else if (other.realCost > testDist)
{
other.CameFrom = best;
other.realCost = testDist;
nodeList.Update(other, other.realCost);
#if DEBUG_PATHFINDER_LOGDEBUG
encountered.Append(" updated ").Append(other.Number)
.Append(", total new estimated cost ")
.Append(other.realCost).AppendLine();
#endif
}
}
}
#if DEBUG_PATHFINDER_LOGDEBUG
Debug.Log(encountered);
Debug.Log(nodes);
#endif
return foundNodes;
}