/**
* Find the path from start to goal using Dijkstra's algorithm
*
* @param start The starting location
* @param goal The goal location
* @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
* @return The list of intersections that form the shortest path from start to goal (including
* both start and goal).
*/
public List<GeographicPoint> dijkstra(
GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
PriorityQueue<MapNode> pq = new PriorityQueue<MapNode>(); // Initialize priority queue
HashSet<MapNode> visited = new HashSet<MapNode>();
HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap
// Initialize values to infinity
MapNode startNode = vertices.get(start);
initializeNodes(startNode, vertices);
// Enqueue start node
pq.add(startNode);
int dijkstraCount = 0;
// while queue is not empty
while (pq.isEmpty() == false) {
// dequeue current node from the front of the queue
MapNode dqNode = pq.remove();
dijkstraCount++;
// Hook for visualization.
nodeSearched.accept(dqNode.getCoordinates());
// if curr is not visited
if (visited.contains(dqNode) == false) {
// add curr to the visited set
visited.add(dqNode);
// if the current node is equal to the goal node...
if (vertices.get(dqNode.getCoordinates()) == vertices.get(goal)) {
System.out.println("Dijkstra count: " + dijkstraCount);
List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();
// Build a list using the path found from the start node to the goal node.
listCoordinates = buildPath(dqNode, parent);
// Print out the visited nodes
printPathOfCoordinates(listCoordinates);
return listCoordinates;
}
// for each of the current node's neighbors not in the visited set
for (MapEdge e : dqNode.getEdges()) {
MapNode n = vertices.get(e.end);
if (visited.contains(n) == false) {
// visited.add(vertices.get(e.end));
// 1. if path through curr to n is shorter
double newWeight = dqNode.getWeight() + e.getDistance();
if (newWeight < n.getWeight()) {
// 2. update n's distances
n.setWeight(newWeight);
// 3. update curr as n's parent in parent map
parent.put(n, dqNode);
// 4. enqueue (n, distance) into the PQ
pq.add(n);
}
}
}
} // if
} // while
return null;
}
