/**
* Find the path from start to goal using A-Star search
*
* @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> aStarSearch(
GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
// TODO: Implement this method in WEEK 3
// Hook for visualization. See writeup.
// nodeSearched.accept(next.getLocation());
PriorityQueue<MapVertex> pq = new PriorityQueue<MapVertex>(MapVertex.AStarComparator);
Set<MapVertex> visited = new HashSet<MapVertex>();
Map<GeographicPoint, GeographicPoint> parentMap =
new HashMap<GeographicPoint, GeographicPoint>();
setDistancesFromStart(start);
setDistancesToEnd(start, goal);
pq.add(Vertices.get(start)); // put the start node into the PQ
while (!pq.isEmpty()) {
MapVertex curr = pq.poll();
nodeSearched.accept(curr.getLocation()); // for visualization
if (!visited.contains(curr)) {
visited.add(curr);
if (curr.getLocation().equals(goal)) return RetrievePath(parentMap, start, goal);
; // retrieve the shortest path
Set<MapEdge> edges = curr.getEdges();
for (MapEdge edge : edges) {
MapVertex next = edge.getEnd();
if (!visited.contains(next)) {
double currLenFromStart = curr.getDistFromStart() + edge.getRoadLength();
if (currLenFromStart < next.getDistFromStart()) {
next.setDistFromStart(currLenFromStart); // update shortest distance to the start
parentMap.put(
next.getLocation(), curr.getLocation()); // update the parent-child relation
pq.add(next);
}
}
}
}
}
return null;
}
