private bool CalculateShortestPath() { Node currentNode; Position[] neighbors = new Position[8]; heap.Add(startNode, 0); while (heap.Count > 0) { currentNode = heap.Remove(); if (currentNode == targetNode) { return(true); } currentNode.setClosed(); var count = GetNeighbors(currentNode, ref neighbors); for (var i = 0; i < count; i++) { var neighbor = neighbors[i]; var neighborNode = GetNeighborNode(currentNode, neighbor); if (neighborNode == null || neighborNode.isClosed()) { continue; } int newGCost = NewGCost(currentNode, neighborNode); if (newGCost < neighborNode.gCost || !neighborNode.isOpen()) { #if DEBUG_PATHFINDING if (showDebug) { DebugDrawer.Draw(new Vector2Int(currentNode.x, currentNode.y), new Vector2Int(neighborNode.x, neighborNode.y), Color.white); DebugDrawer.DrawCube(new Vector2Int(neighborNode.x, neighborNode.y), Vector2Int.one, Color.white); } #endif neighborNode.gCost = newGCost; neighborNode.hCost = Heuristic(neighborNode, targetNode); neighborNode.parent = currentNode; if (!neighborNode.isOpen()) { heap.Add(neighborNode, neighborNode.gCost + neighborNode.hCost); neighborNode.setOpen(); } else { heap.Update(neighborNode, neighborNode.gCost + neighborNode.hCost); } } } } return(false); }
public virtual List <Node> GetPath(Vector2Int start, Vector2Int target, float upperBound = float.PositiveInfinity) { nodes = new Dictionary <long, Node>(); var startNode = GetStartNode(start); var endNode = GetEndNode(target); var neighbors = new Neighbor[8]; middleNode = null; bestPathLength = float.PositiveInfinity; openA = new MinHeap <Node, float>(); openB = new MinHeap <Node, float>(); openA.Add(startNode, 0); openB.Add(endNode, 0); var ticks = 0; while (openA.Count > 0 && openB.Count > 0) { ticks++; var mtmp = openA.Peek().costA + openB.Peek().costB; if (mtmp >= bestPathLength) { Debug.Log("Found path in " + ticks + " ticks " + expandedNodes + " expanded nodes"); return(tracebackPath(middleNode)); } if (mtmp >= upperBound) { return(null); } expandForwardFrontier(openA.Remove(), ref neighbors); ticks++; mtmp = openA.Peek().costA + openB.Peek().costB; if (mtmp >= bestPathLength) { Debug.Log("Found path in " + ticks + " ticks " + expandedNodes + " expanded nodes"); return(tracebackPath(middleNode)); } if (mtmp >= upperBound) { return(null); } expandBackwardFrontier(openB.Remove(), ref neighbors); } return(null); }