public static List <Node> GetPath(Node startNode, Node targetNode) { if (startNode == targetNode) { return(new List <Node>()); } if (startNode.Region != targetNode.Region) { return(null); } List <Subregion> subregionsToErase = AStarSubregionSearch.GetPath(startNode.subregion, targetNode.subregion); Stack <Subregion> subregions = new Stack <Subregion>(subregionsToErase); Heap <Node> openSet = new Heap <Node>(Pathfinder.MapWidth * Pathfinder.MapHeight); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode; do { //debug if (openSet.Count == 0) { foreach (var subregion in subregionsToErase) { subregion.child = null; } return(null); } currentNode = openSet.RemoveFirst(); if (subregions.Count == 0 || currentNode.subregion == subregions.Peek()) { break; } } while (true); closedSet.Add(currentNode); HandleAddToClosedSet?.Invoke(currentNode); if (currentNode == targetNode) { var path = RetracePath(startNode, targetNode); foreach (var subregion in subregionsToErase) { subregion.child = null; } return(path); } foreach (var neighbour in GetNeighbours(currentNode)) { if (closedSet.Contains(neighbour)) { continue; } if (subregions.Count != 0 && subregions.Peek().child == neighbour.subregion) { subregions.Pop(); } if (closedSet.Contains(neighbour) || (subregions.Count != 0 && !subregions.Peek().nodes.Contains(neighbour))) { continue; } int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); bool isInOpenSet = openSet.Contains(neighbour); if (newCostToNeighbour < neighbour.gCost || !isInOpenSet) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = Heuristic(neighbour, targetNode); if (neighbour.subregion.child != null) { neighbour.rCost = GetDistance(neighbour, PathGrid.NodeAt(neighbour.subregion.child.avergX, neighbour.subregion.child.avergY)); } else { neighbour.rCost = 0; } neighbour.parent = currentNode; if (!isInOpenSet) { openSet.Add(neighbour); } } } } foreach (var subregion in subregionsToErase) { subregion.child = null; } return(null); }
public static List <Node> GetPath(Node startNode, Node targetNode) { if (startNode == targetNode) { return(new List <Node>()); } if (startNode.Region != targetNode.Region) { return(null); } List <Subregion> subregions = AStarSubregionSearch.GetPath(startNode.subregion, targetNode.subregion); Node[] possibleNodes = new Node[Pathfinder.MapWidth * Pathfinder.MapHeight]; foreach (Subregion s in subregions) { foreach (Node n in s.nodes) { possibleNodes[n.X + n.Y * Pathfinder.MapHeight] = n; } } Heap <Node> openSet = new Heap <Node>(Pathfinder.MapWidth * Pathfinder.MapHeight); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); HandleAddToClosedSet?.Invoke(currentNode); if (currentNode == targetNode) { var path = RetracePath(startNode, targetNode); return(path); } foreach (var neighbour in GetNeighbours(currentNode)) { if (closedSet.Contains(neighbour) || possibleNodes[neighbour.X + neighbour.Y * Pathfinder.MapHeight] == null) { continue; } int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (openSet.Contains(neighbour)) { if (newCostToNeighbour < neighbour.gCost) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = Heuristic(neighbour, targetNode); neighbour.rCost = 0; neighbour.parent = currentNode; openSet.UpdateItem(neighbour); } } else { neighbour.gCost = newCostToNeighbour; neighbour.hCost = Heuristic(neighbour, targetNode); neighbour.rCost = 0; neighbour.parent = currentNode; openSet.Add(neighbour); } } } return(null); }