/// <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;
}