/** Set the distances from each vertex to the starting point as appropriate */ private void setDistancesFromStart(GeographicPoint start) { Collection<MapVertex> allNodes = Vertices.values(); for (MapVertex node : allNodes) { node.setDistFromStart(Double.POSITIVE_INFINITY); } MapVertex source = Vertices.get(start); source.setDistFromStart(0.0); // start to itself is of 0 distance }
/** * 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; }