IEnumerator Pathfind()
{
frontier.Enqueue(new InternalPathNode(startPos, ActionType.WALK), 0);
movementCosts.Set(startPos, 0);
parents.Set(startPos, new InternalPathNode(startPos, ActionType.WALK));
closestDistance = int.MaxValue;
InternalPathNode current;
var cycles = 0;
while (frontier.Count > 0)
{
++cycles;
current = frontier.Dequeue();
var adj = GetAdjacent(current.coord);
foreach (var pos in adj)
{
if (!IsClosed(pos.coord))
{
if (!frontier.Contains(pos))
{
movementCosts.Set(pos.coord, Mathf.Infinity); // so (new < orig) comparison works
}
UpdateNode(current, pos);
if (pos.coord == endPos)
{
ReconstructPath(pos);
yield break;
}
if (HexUtil.DistanceFlat(pos.coord, endPos) < closestDistance)
{
closestDistance = HexUtil.DistanceFlat(pos.coord, endPos);
closestPoint = pos;
}
}
}
if (cycles >= cyclesPerFrame)
{
cycles = 0;
yield return(null);
}
}
// endpos was not on connected graph; return closest node
ReconstructPath(closestPoint);
}
public bool IsValidQueue()
{
lock (_queue)
{
// Check all items in cache are in the queue
foreach (IList <SimpleNode> nodes in _itemToNodesCache.Values)
{
foreach (SimpleNode node in nodes)
{
if (!_queue.Contains(node))
{
return(false);
}
}
}
// Check all items in queue are in cache
foreach (SimpleNode node in _queue)
{
if (GetExistingNode(node.Data) == null)
{
return(false);
}
}
// Check queue structure itself
return(_queue.IsValidQueue());
}
}
public static List <IPathNode> FindPath(IPathNode start, IPathNode finish)
{
if (start == finish)
{
return(new List <IPathNode>());
}
var frontier = new GenericPriorityQueue <IPathNode, int>(1000);
frontier.Enqueue(start, 0);
var cameFrom = new Dictionary <IPathNode, IPathNode>();
var costSoFar = new Dictionary <IPathNode, int>();
IPathNode current;
IPathNode next;
List <IPathNode> neighbours;
int newCost;
int priority;
cameFrom[start] = null;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (current == finish)
{
break;
}
neighbours = current.GetNeighbours();
for (int i = 0; i < neighbours.Count; i++)
{
next = neighbours[i];
newCost = costSoFar[current] + (next == finish || next.IsWalkable ? 1 : 10000);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
if (costSoFar.ContainsKey(next))
{
costSoFar[next] = newCost;
}
else
{
costSoFar.Add(next, newCost);
}
priority = newCost + finish.GetHeuristic(next);
if (frontier.Contains(next))
{
frontier.UpdatePriority(next, priority);
}
else
{
frontier.Enqueue(next, priority);
}
cameFrom[next] = current;
}
}
}
var path = new List <IPathNode>();
if (cameFrom.ContainsKey(finish))
{
IPathNode last = finish;
path.Add(finish);
while (cameFrom[last] != start)
{
path.Add(cameFrom[last]);
last = cameFrom[last];
}
path.Reverse();
}
return(path);
}
