/**
* <summary>
* Search the nodes from start to the goal
* and get the paths
* </summary>
*
* <param name="start">Node start</param>
* <param name="goal">Node goal</param>
*
* <returns>
* List<Node> paths
* </returns>
*/
public List <Node> Search(Node start, Node goal)
{
// Paths
List <Node> paths = new List <Node>();
// Priority queue
BinaryMinHeap queue = new BinaryMinHeap();
queue.Insert(start);
//start.SetCameFrom(null);
start.SetGScore(0);
start.SetFScore(this.Hueristic(start, goal));
// Filter out empty nodes
List <Node> filtered = queue.GetNodes().FindAll((node) => { return(node != null); });
while (filtered.Count > 0)
{
Node current = queue.Extract();
// If the goal is found, get and return reconstructed paths
if (current.GetID() == goal.GetID())
{
return(paths = this.ReconstructPath(current));
}
for (int i = 0; i < current.GetNeighbours().Count; i++)
{
Node neighbour = current.GetNeighbours()[i];
// The cost of moving to the next neighbour
float tentativeGScore = current.GetGScore() + this.MovementCost(current, neighbour);
// if the new gScore is less than the neighbour gScore
if (tentativeGScore < neighbour.GetGScore())
{
// Set neighbour cameFrom to the current
neighbour.SetCameFrom(current);
// Set the neighbour gScore to the lower gScore
neighbour.SetGScore(tentativeGScore);
// Set the new FScore
neighbour.SetFScore(neighbour.GetGScore() + this.Hueristic(neighbour, goal));
bool exist = false;
// Is the neighbour in the open set or priority queue
for (int n = 0; n < queue.GetNodes().Count; n++)
{
Node node = queue.GetNodes()[n];
if (node != null && node.GetID() == neighbour.GetID())
{
exist = true;
break;
}
}
if (!exist)
{
queue.Insert(neighbour);
}
}
}
}
return(paths);
}
