public static List Navigate(RoadLink start, RoadLink finish, DateTime time) { List closed = new List(); // build a priority queue of the paths we've found previously // sorted by the time it has currently spent traveling PriorityQueue> qq = new PriorityQueue>(); // add the starting point as the beginning qq.Push(LinkFitness(start, time), NavigationNode.MakePath(start,time)); while (qq.Count > 0) { // take the first path off the list, this has the lowest total // travel time so far float length = qq.PeekPriority(); List path = qq.Pop(); // check the last node in the path NavigationNode lastLink = path[path.Count-1]; // if we've visited this node previously, we're making a cycle if (closed.Contains(lastLink)) continue; // skip to ignore cycles // if we've reached the destination, add the destination link // and return if (lastLink.LeavingLink == finish) { path.Add(new NavigationNode(finish, null, null, time)); return path; } // mark visited closed.Add(lastLink); // create new paths based on where we can go to from here foreach (NavigationNode next in lastLink.Leave(time)) { // calculate the new length float newLength = length + TransitionFitness(next.LinkIntersection, next.EnteringLink, next.LeavingLink, time) + LinkFitness(next.LeavingLink, time); //TODO: calculate the new time to add to the end node // create the new path List newList = new List(path); newList.Add(next); // add it to the priority queue for consideration qq.Push(newLength, newList); } } // you can't get there from here... no really... you can't return null; } public static float LinkFitness(RoadLink link, DateTime time) { return (link.Distance / link.SpeedLimit); } public static float TransitionFitness(Intersection intersection, RoadLink inLink, RoadLink outLink, DateTime time) { // 20m is the guessed transition distance //HACK: lol we should quantify this return (0.0124274f) / ((inLink.SpeedLimit + outLink.SpeedLimit)/2.0f); }