/**
* 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;
}
/** 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
}
/**
* Get the number of road segments in the graph
*
* @return The number of edges in the graph.
*/
public int getNumEdges() {
// TODO: Implement this method in WEEK 2
int NumEdges = 0;
Collection<MapVertex> vertices = Vertices.values();
for (MapVertex vertex : vertices) { // accumulate each vertex's number of connected edges
NumEdges += vertex.getSize();
}
return NumEdges;
// return 0;
}
/** initialize the predicted distances from each node to the goal as appropriate */
private void setDistancesToEnd(GeographicPoint start, GeographicPoint goal) {
Collection<MapVertex> allNodes = Vertices.values();
for (MapVertex node : allNodes) {
// node.setPredictedDistToEnd(Double.POSITIVE_INFINITY);
node.setPredictedDistToEnd(calculateDistance(node, Vertices.get(goal)));
}
// MapVertex destination = Vertices.get(goal);
// destination.setPredictedDistToEnd(0.0);
// MapVertex source = Vertices.get(start); // set the predicted dist from start to goal as the
// straight line dist
// source.setPredictedDistToEnd(calculateDistance(source, destination));
}
/**
* Adds a directed edge to the graph from pt1 to pt2. Precondition: Both GeographicPoints have
* already been added to the graph
*
* @param from The starting point of the edge
* @param to The ending point of the edge
* @param roadName The name of the road
* @param roadType The type of the road
* @param length The length of the road, in km
* @throws IllegalArgumentException If the points have not already been added as nodes to the
* graph, if any of the arguments is null, or if the length is less than 0.
*/
public void addEdge(
GeographicPoint from, GeographicPoint to, String roadName, String roadType, double length)
throws IllegalArgumentException {
// TODO: Implement this method in WEEK 2
if (!Vertices.containsKey(from)
|| !Vertices.containsKey(to)
|| from == null
|| to == null
|| roadName == null
|| roadType == null
|| length < 0) {
throw new IllegalArgumentException("arguments are not valid");
}
MapEdge edge = new MapEdge(Vertices.get(from), Vertices.get(to), roadName, roadType, length);
MapVertex vertex = Vertices.get(from); // find the associated vertex
vertex.ConnectTo(edge); // add the newly created edge to vertex
}
/** @return the distance b/w two points given their latitude and longitude */
private double calculateDistance(MapVertex source, MapVertex destination) {
double EarthRadius = 6371000.0;
GeographicPoint sourceLocation = source.getLocation();
GeographicPoint destinationLocation = destination.getLocation();
double sourceLatitude = Math.toRadians(sourceLocation.x);
double sourceLongitude = Math.toRadians(sourceLocation.y);
double destinationLatitude = Math.toRadians(destinationLocation.x);
double destinationLongitude = Math.toRadians(destinationLocation.y);
double latDiff = sourceLatitude - destinationLatitude;
double lonDiff = sourceLongitude - destinationLongitude;
double a =
Math.sin(latDiff / 2) * Math.sin(latDiff / 2)
+ Math.cos(sourceLatitude)
* Math.cos(destinationLatitude)
* Math.sin(lonDiff / 2)
* Math.sin(lonDiff / 2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
return EarthRadius * c;
}
/**
* 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;
}
