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