/**
* 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;
}
