public override SearchDetails GetPathTick()
{
// If there are still nodes on the queue and the destination hasn't been found, evaluate the next node.
if (_q.Count > 0 && !_destinationFound)
{
// Get the next node in the queue and check it hasn't already been visited
CurrentNode = _q.Dequeue();
if (AlreadyVisited(CurrentNode.Coord))
{
return(GetSearchDetails());
}
// Add node to those already visited
Closed.Add(CurrentNode);
Grid.SetCell(CurrentNode.Coord.X, CurrentNode.Coord.Y, Enums.CellType.Closed);
// Go through the neighbours of this node and add the new ones to the queue
var neighbours = GetNeighbours(CurrentNode);
foreach (var neighbour in neighbours)
{
if (AlreadyVisited(neighbour))
{
continue;
}
var neighbourNode = new Node(Id++, CurrentNode.Id, neighbour.X, neighbour.Y, 0, 0);
_q.Enqueue(neighbourNode);
Grid.SetCell(neighbour, Enums.CellType.Open);
if (!CoordsMatch(neighbour, Destination))
{
continue;
}
// Check if we've found the destination
Closed.Add(neighbourNode);
_destinationFound = true;
}
}
else
{
// The path has been found, reconstruct it be following back via the parentId's and reverse the order so it goes from A to B
Path = new List <Coord>();
var step = Closed.First(x => CoordsMatch(x.Coord, Destination));
while (!CoordsMatch(step.Coord, Origin))
{
Path.Add(step.Coord);
step = Closed.First(x => x.Id == step.ParentId);
}
Path.Add(Origin);
Path.Reverse();
}
return(GetSearchDetails());
}
public override SearchDetails GetPathTick()
{
if (NodoActual == null)
{
if (!_Listaabierta.Any())
{
return(GetSearchDetails());
}
// Take the current node off the open list to be examined
NodoActual = _Listaabierta.OrderBy(x => x.F).ThenBy(x => x.H).First();
// Move it to the closed list so it doesn't get examined again
_Listaabierta.Remove(NodoActual);
Cerrado.Add(NodoActual);
Cuadricula.SetCell(NodoActual.Coord, Enums.CellType.Closed);
_vecinos.AddRange(GetVECINO(NodoActual));
}
if (_vecinos.Any())
{
Cuadricula.SetCell(NodoActual.Coord, Enums.CellType.Current);
var thisNeighbour = _vecinos.First();
_vecinos.Remove(thisNeighbour);
// If the neighbour is the destination
if (CoordsMatch(thisNeighbour, Destination))
{
// Construct the path by tracing back through the closed list until there are no more parent id references
Path = new List <Coord> {
thisNeighbour
};
int?parentId = NodoActual.Id;
while (parentId.HasValue)
{
var nextNode = Closed.First(x => x.Id == parentId);
Path.Add(nextNode.Coord);
parentId = nextNode.ParentId;
}
// Reorder the path to be from origin to destination and return
Path.Reverse();
return(GetSearchDetails());
}
// Get the cost of the current node plus the extra step weight and heuristic
var hFromHere = GetH(thisNeighbour, Destination);
var cellWeight = Grid.GetCell(thisNeighbour.X, thisNeighbour.Y).Weight;
var neighbourCost = NodoActual.G + cellWeight + hFromHere;
// Check if the node is on the open list already and if it has a higher cost path
var openListItem = _Listaabierta.FirstOrDefault(x => x.Id == GetExistingNode(true, thisNeighbour));
if (openListItem != null && openListItem.F > neighbourCost)
{
// Repoint the openlist node to use this lower cost path
openListItem.F = neighbourCost;
openListItem.ParentId = NodoActual.Id;
}
// Check if the node is on the closed list already and if it has a higher cost path
var closedListItem = Closed.FirstOrDefault(x => x.Id == GetExistingNode(false, thisNeighbour));
if (closedListItem != null && closedListItem.F > neighbourCost)
{
// Repoint the closedlist node to use this lower cost path
closedListItem.F = neighbourCost;
closedListItem.ParentId = NodoActual.Id;
}
// If the neighbour node isn't on the open or closed list, add it
if (openListItem != null || closedListItem != null)
{
return(GetSearchDetails());
}
_Listaabierta.Add(new Nodo(Id++, NodoActual.Id, thisNeighbour, NodoActual.G + cellWeight, hFromHere));
Grid.SetCell(thisNeighbour.X, thisNeighbour.Y, Enums.CellType.Open);
}
else
{
Grid.SetCell(NodoActual.Coord, Enums.CellType.Closed);
NodoActual = null;
return(GetPathTick());
}
return(GetSearchDetails());
}