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()); }