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 static List <Subregion> GetPath(Subregion startSubregion, Subregion targetSubregion)
{
if (startSubregion.Region != targetSubregion.Region)
{
return(null);
}
if (startSubregion == targetSubregion)
{
List <Subregion> result = new List <Subregion>();
result.Add(startSubregion);
return(result);
}
MinHeap <Subregion> openSet = new MinHeap <Subregion>(startSubregion.Region.Subregions.Count);
HashSet <Subregion> closedSet = new HashSet <Subregion>();
openSet.Add(startSubregion);
while (openSet.Count > 0)
{
Subregion currentSubregion = openSet.RemoveFirst();
closedSet.Add(currentSubregion);
if (currentSubregion == targetSubregion)
{
return(RetracePath(startSubregion, targetSubregion));
}
foreach (Subregion neighbour in currentSubregion.NeighbouringSubregions)
{
if (closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentSubregion.gCost + GetDistance(currentSubregion, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetSubregion);
neighbour.ParentSubregion = currentSubregion;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(null);
}
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);
}
}
}
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);
}
public static List <Node> GetPath(Node startNode, Node targetNode)
{
if (startNode == targetNode)
{
return(new List <Node>());
}
if (startNode.Region != targetNode.Region)
{
return(null);
}
//Swap source with target for optimization of path retracing.
Node tmp = startNode;
startNode = targetNode;
targetNode = tmp;
List <Subregion> subregionPath = ASubregionSearch.GetPath(startNode.Subregion, targetNode.Subregion);
Stack <Subregion> corridor = new Stack <Subregion>(subregionPath);
MinHeap <Node> openSet = new MinHeap <Node>(Pathfinder.MapWidth * Pathfinder.MapHeight);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode;
do
{
currentNode = openSet.RemoveFirst();
if (corridor.Count == 0 || currentNode.Subregion == corridor.Peek())
{
break;
}
} while (true);
closedSet.Add(currentNode);
HandleAddToClosedSet?.Invoke(currentNode); //visualization
if (currentNode == targetNode)
{
List <Node> path = RetracePath(startNode, targetNode);
foreach (Subregion subregion in subregionPath)
{
subregion.Child = null;
}
return(path);
}
foreach (Node neighbour in currentNode.GetAllNeighbours())
{
if (!neighbour.IsTraversable || closedSet.Contains(neighbour))
{
continue;
}
if (corridor.Count != 0 && corridor.Peek().Child == neighbour.Subregion)
{
corridor.Pop();
}
if (corridor.Count != 0 && !corridor.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 = GetDistance(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 (Subregion subregion in subregionPath)
{
subregion.Child = null;
}
return(null);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSucces = false;
Node startNode = _grid.WorldPositionToNode(request.start);
Node targetNode = _grid.WorldPositionToNode(request.end);
if (startNode.IsWalkable && targetNode.IsWalkable)
{
MinHeap <Node> openSet = new MinHeap <Node>(_grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node current = openSet.RemoveFirst();
closedSet.Add(current);
if (current == targetNode)
{
sw.Stop();
UnityEngine.Debug.Log("Path found " + sw.ElapsedMilliseconds + " ms");
pathSucces = true;
break;
}
foreach (Node neighbour in current.Neighbours)
{
if (neighbour == null || !neighbour.IsWalkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCost = current.GCost + GetDistance(current, neighbour);
if (newMovementCost < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newMovementCost;
neighbour.HCost = GetDistance(neighbour, targetNode);
neighbour.Parent = current;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathSucces)
{
waypoints = RetracePath(startNode, targetNode);
}
callback(new PathResult(waypoints, pathSucces, request.callback));
}