// On Scene Start, Load the graph
public List <PathfindNode> FindPath(PathfindNode origin, PathfindNode dest)
{
List <PathfindNode> path = new List <PathfindNode>();
PriorityQueue <PathfindPriorityQueueItem> priority =
new PriorityQueue <PathfindPriorityQueueItem>();
List <PathfindPriorityQueueItem> unseen = new List <PathfindPriorityQueueItem>();
List <PathfindPriorityQueueItem> seen = new List <PathfindPriorityQueueItem>();
InitializeUnseenList(unseen, origin, dest, priority);
PathfindPriorityQueueItem item;
while (priority.Count() > 0 && (item = priority.Dequeue()).node != null)
{
seen.Add(item);
unseen.Remove(item);
foreach (PathfindNode adj in item.node.nodes)
{
PathfindPriorityQueueItem enqueuedItem = FindInList(adj, unseen);
float segmentDistance = item.node.edges[adj].distance;
if (enqueuedItem != null)
{
if (segmentDistance >= 0)
{
if (priority.Contains(enqueuedItem))
{
float pathWeight = segmentDistance + item.pathDistanceFromOrigin;
if (pathWeight < enqueuedItem.pathDistanceFromOrigin)
{
enqueuedItem.pathDistanceFromOrigin = pathWeight;
enqueuedItem.prev = item.node;
priority.Sort();
}
}
else
{
enqueuedItem.prev = item.node;
enqueuedItem.pathDistanceFromOrigin = segmentDistance + item.pathDistanceFromOrigin;
priority.Enqueue(enqueuedItem);
}
}
}
}
if (item.node == dest)
{
PathfindPriorityQueueItem current = item;
while (current != null)
{
path.Add(current.node);
current = FindInList(current.prev, seen);
}
break;
}
}
path.Reverse();
return(path);
}
public int CompareTo(object obj)
{
if (obj == null)
{
return(1);
}
PathfindPriorityQueueItem other = obj as PathfindPriorityQueueItem;
if (other != null)
{
float thisHeuristic = distanceToDest + pathDistanceFromOrigin;
float otherHeuristic = other.distanceToDest + other.pathDistanceFromOrigin;
return(thisHeuristic.CompareTo(otherHeuristic));
}
else
{
throw new ArgumentException("Object is not a PathfindPriorityQueueItem");
}
}
private void InitializeUnseenList(List <PathfindPriorityQueueItem> unseen,
PathfindNode origin,
PathfindNode dest,
PriorityQueue <PathfindPriorityQueueItem> priority)
{
foreach (PathfindNode node in graph)
{
PathfindPriorityQueueItem item = new PathfindPriorityQueueItem();
item.node = node;
item.prev = null;
float dist = Vector2.Distance(node.transform.position, dest.transform.position);
item.distanceToDest = dist;
item.pathDistanceFromOrigin = 99999999; // basically infinity
if (node == origin)
{
item.pathDistanceFromOrigin = 0;
priority.Enqueue(item);
}
unseen.Add(item);
}
}
