/**
* 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) {
// Initialize priority queue
PriorityQueue<MapNode> pq = new PriorityQueue<MapNode>(); // Initialize priority queue
// Initialize HashSet
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);
// Initialize estimated distance
double heuristicDistance = start.distance(goal);
// Enqueue start node
pq.add(startNode);
// Count
int aStarCount = 0;
// while queue is not empty
while (pq.isEmpty() == false) {
// dequeue current node from the front of the queue
// MapNode dqNode = pq.remove();
MapNode dqNode = pq.poll();
aStarCount++;
// 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);
// 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) {
// calculate the distance from the start node to the current node,
// to each neighboring node
// double g = dqNode.getWeight() + e.getDistance();
double g = dqNode.getWeight() + dqNode.getCoordinates().distance(n.getCoordinates());
// calculate the estimated distance from the neighboring node to
// the goal
double h = n.getCoordinates().distance(goal);
// set the weight of the neighboring node
n.setWeight(g + h);
// set the current node as it's neighboring nodes' parent
parent.put(n, dqNode);
// If neighbor is the goal, return!
if (vertices.get(n.getCoordinates()) == vertices.get(goal)) {
List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();
// Build a list using the path found from the start node to the goal node.
listCoordinates = buildPath(n, parent);
// Print out the visited nodes
printPathOfCoordinates(listCoordinates);
System.out.println("a star count: " + aStarCount);
return listCoordinates;
}
// if the original heuristic distance is less than or equal
// to the new heuristic distance, add the node to the
// priority queue.
if (heuristicDistance <= n.getWeight()) {
pq.offer(n);
}
}
}
} // if
} // while
return null;
}
/**
* 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;
}
