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