private void expandForwardFrontier(Node current, ref Neighbor[] neighbors)
{
current.setClosedA();
var count = GetNeighbors(current, ref neighbors);
for (var i = 0; i < count; i++)
{
var neighbor = neighbors[i].node;
if (neighbor.isClosedA())
{
continue;
}
var tentativeScore = current.costA + neighbors[i].cost;
if (!neighbor.isOpenA())
{
neighbor.setOpenA();
neighbor.costA = tentativeScore;
neighbor.parentA = current;
openA.Add(neighbor, tentativeScore);
updateForwardFrontier(neighbor, tentativeScore);
}
else if (neighbor.costA > tentativeScore)
{
neighbor.costA = tentativeScore;
neighbor.parentA = current;
openA.Update(neighbor, tentativeScore);
updateForwardFrontier(neighbor, tentativeScore);
}
}
}
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);
}