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