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