/** * Find the path from start to goal using breadth first 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 (unweighted) path from start to goal * (including both start and goal). */ public List<GeographicPoint> bfs( GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) { // TODO: Implement this method in WEEK 2 // Hook for visualization. See writeup. // nodeSearched.accept(next.getLocation()); Queue<GeographicPoint> queue = new LinkedList<GeographicPoint>(); Set<GeographicPoint> visited = new HashSet<GeographicPoint>(); Map<GeographicPoint, GeographicPoint> parentMap = new HashMap<GeographicPoint, GeographicPoint>(); queue.add(start); visited.add(start); while (!queue.isEmpty()) { GeographicPoint curr = queue.poll(); nodeSearched.accept(curr); if (curr.equals(goal)) { // reaches the goal return RetrievePath(parentMap, start, goal); // retrieve the path } MapVertex node = Vertices.get(curr); // the MapVertex associated with the location curr Set<MapEdge> edges = node.getEdges(); // the edges connected to the node for (MapEdge edge : edges) { MapVertex next = edge.getEnd(); // neighbor of the location curr GeographicPoint nextLoc = next.getLocation(); if (!visited.contains(nextLoc)) { visited.add(nextLoc); parentMap.put(nextLoc, curr); // set curr as the parent of next in tha path map queue.add(nextLoc); } } } /////// if the goal is not achievable return null; }
// method to print edge points out to a file public void printEdgePointsToFile(String filename) { try { PrintWriter writer = new PrintWriter(filename, "UTF-8"); for (MapEdge e : edges) { writer.println(e.getStartPoint() + " " + e.getEndPoint()); } writer.flush(); writer.close(); } catch (Exception e) { System.out.println("Exception opening file " + e); } }
/** * 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; }
/** * Find the path from start to goal using A-Star search * * @param start The starting location * @param goal The goal location * @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> nodeAccepter) { boolean debug = false; // Set up if (start == null || goal == null) throw new NullPointerException("Cannot find route from or to null node"); MapNode startNode = pointNodeMap.get(start); MapNode endNode = pointNodeMap.get(goal); if (startNode == null) { System.err.println("Start node " + start + " does not exist"); return null; } if (endNode == null) { System.err.println("End node " + goal + " does not exist"); return null; } HashMap<MapNode, MapNode> parentMap = new HashMap<MapNode, MapNode>(); PriorityQueue<MapNode> toExplore = new PriorityQueue<MapNode>(); HashSet<MapNode> visited = new HashSet<MapNode>(); // initialize distance for all nodes for (MapNode n : pointNodeMap.values()) { n.setDistance(Double.POSITIVE_INFINITY); n.setActualDistance(Double.POSITIVE_INFINITY); } startNode.setDistance(0); startNode.setActualDistance(0); toExplore.add(startNode); int count = 0; MapNode next = null; while (!toExplore.isEmpty()) { next = toExplore.remove(); nodeAccepter.accept(next.getLocation()); count++; if (debug) { System.out.println( "\nA* visiting" + next + "\nActual = " + next.getActualDistance() + ", Pred: " + next.getDistance()); } if (next.equals(endNode)) break; if (!visited.contains(next)) { visited.add(next); Set<MapEdge> edges = next.getEdges(); for (MapEdge edge : edges) { MapNode neighbor = edge.getEndNode(); if (!visited.contains(neighbor)) { double currDist = edge.getLength() + next.getActualDistance(); // core of A* is just to add to currDist the cost of getting to // the destination double predDist = currDist + (neighbor.getLocation()).distance(endNode.getLocation()); if (predDist < neighbor.getDistance()) { // debug if (debug) { System.out.println("Adding to queue node at: " + neighbor.getLocation()); System.out.println("Curr dist: " + currDist + " Pred Distance: " + predDist); } parentMap.put(neighbor, next); neighbor.setActualDistance(currDist); neighbor.setDistance(predDist); toExplore.add(neighbor); } } } } } if (!next.equals(endNode)) { System.out.println("No path found from " + start + " to " + goal); return null; } // Reconstruct the parent path List<GeographicPoint> path = reconstructPath(parentMap, startNode, endNode); System.out.println("Nodes visited in search: " + count); return path; }
/** * Find the path from start to goal using Dijkstra's algorithm * * @param start The starting location * @param goal The goal location * @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) { if (start == null || goal == null) throw new NullPointerException("Cannot find route from or to null node"); MapNode startNode = pointNodeMap.get(start); MapNode endNode = pointNodeMap.get(goal); if (startNode == null) { System.err.println("Start node " + start + " does not exist"); return null; } if (endNode == null) { System.err.println("End node " + goal + " does not exist"); return null; } HashMap<MapNode, MapNode> parentMap = new HashMap<MapNode, MapNode>(); PriorityQueue<MapNode> toExplore = new PriorityQueue<MapNode>(); HashSet<MapNode> visited = new HashSet<MapNode>(); // initialize distance for all nodes for (MapNode n : pointNodeMap.values()) { n.setDistance(Double.POSITIVE_INFINITY); } startNode.setDistance(0); toExplore.add(startNode); MapNode next = null; int count = 0; // count visited while (!toExplore.isEmpty()) { next = toExplore.remove(); // hook for visualization nodeSearched.accept(next.getLocation()); count++; System.out.println("DIJKSTRA visiting" + next); if (next.equals(endNode)) break; if (!visited.contains(next)) { visited.add(next); Set<MapEdge> edges = next.getEdges(); for (MapEdge edge : edges) { MapNode neighbor = edge.getEndNode(); if (!visited.contains(neighbor)) { double currDist = edge.getLength() + next.getDistance(); if (currDist < neighbor.getDistance()) { // debug // System.out.println("Distance: " + currDist + "Node\n"+neighbor); parentMap.put(neighbor, next); neighbor.setDistance(currDist); toExplore.add(neighbor); } } } } } if (!next.equals(endNode)) { System.out.println("No path found from " + start + " to " + goal); return null; } // Reconstruct the parent path List<GeographicPoint> path = reconstructPath(parentMap, startNode, endNode); System.out.println("Nodes visited in search: " + count); return path; }
/** * 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; }