/// <summary>
/// Searches in a graph
/// </summary>
/// <returns>The state of the search</returns>
public override SearchState Search()
{
int closestNode;
List <K> edges;
double hCost, gCost;
while (queue.Count != 0)
{
//Dequeue the first node (the one with the lower cost) and add it to the shortest path tree
closestNode = queue.Dequeue();
shortestPathTree[closestNode] = frontier[closestNode];
//If it´s the target, return
if (closestNode == target)
{
found = SearchState.CompletedAndFound;
return(found);
}
//Get all edges that start from the current node
edges = graph.GetEdgesFromNode(closestNode);
for (int i = 0; i < edges.Count; i++)
{
//Calculate the estimated cost to reach the target from the next node
hCost = heuristic(graph, target, edges[i].End);
//Calculate the cost to reach the next node with the current one and the edge
gCost = gCosts[closestNode] + edges[i].Cost;
//If the node wasn´t in the frontier, add it
if (frontier[edges[i].End] == null)
{
fCosts[edges[i].End] = gCost + hCost;
gCosts[edges[i].End] = gCost;
queue.Enqueue(edges[i].End);
frontier[edges[i].End] = edges[i];
}
else //If it was, see if the new way of reaching the node is shorter or not. If it´s shorter, update the cost to reach it
{
if ((gCost < gCosts[edges[i].End]) && (shortestPathTree[edges[i].End] == null))
{
fCosts[edges[i].End] = gCost + hCost;
gCosts[edges[i].End] = gCost;
queue.ChangePriority(edges[i].End);
frontier[edges[i].End] = edges[i];
}
}
}
}
found = SearchState.CompletedAndNotFound;
return(found);
}