/**
* Adds a directed edge to the graph from one point to another. 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 {
MapNode a = vertices.get(from);
a.implementAddEdge(from, to, roadName, roadType, length);
numEdges++;
}
/**
* Reconstruct a path from start to goal using the parentMap
*
* @param parentMap the HashNode map of children and their parents
* @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).
*/
private List<GeographicPoint> reconstructPath(
HashMap<MapNode, MapNode> parentMap, MapNode start, MapNode goal) {
LinkedList<GeographicPoint> path = new LinkedList<GeographicPoint>();
MapNode current = goal;
while (!current.equals(start)) {
path.addFirst(current.getLocation());
current = parentMap.get(current);
}
// add start
path.addFirst(start.getLocation());
return path;
}
/**
* Find the path from start to goal using Breadth First 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> bfs(
GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
// Setup - check validity of inputs
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;
}
// setup to begin BFS
HashMap<MapNode, MapNode> parentMap = new HashMap<MapNode, MapNode>();
Queue<MapNode> toExplore = new LinkedList<MapNode>();
HashSet<MapNode> visited = new HashSet<MapNode>();
toExplore.add(startNode);
MapNode next = null;
while (!toExplore.isEmpty()) {
next = toExplore.remove();
// hook for visualization
nodeSearched.accept(next.getLocation());
if (next.equals(endNode)) break;
Set<MapNode> neighbors = getNeighbors(next);
for (MapNode neighbor : neighbors) {
if (!visited.contains(neighbor)) {
visited.add(neighbor);
parentMap.put(neighbor, next);
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);
return path;
}
public void test() throws Exception {
new Plant();
try {
Path dbPath = Paths.get("cow.db");
int maxRootBlockSize = 1000;
Db db = new Db(new BaseRegistry(), dbPath, maxRootBlockSize);
DbFactoryRegistry registry = db.dbFactoryRegistry;
MapNode m1 = registry.nilMap;
ImmutableFactory factory1 = registry.getImmutableFactory(m1);
assertTrue(factory1 instanceof NilMapNodeFactory);
assertEquals(2, factory1.getDurableLength(m1));
ByteBuffer byteBuffer1 = ByteBuffer.allocate(factory1.getDurableLength(m1));
factory1.writeDurable(m1, byteBuffer1);
assertEquals(2, byteBuffer1.position());
byteBuffer1.flip();
ImmutableFactory factory2 = registry.readId(byteBuffer1);
assertTrue(factory2 instanceof NilMapNodeFactory);
Object object2 = factory2.deserialize(byteBuffer1);
assertEquals(2, byteBuffer1.position());
assertTrue(object2.equals(registry.nilMap));
MapNode m2 = m1;
m2 = m2.add("a", "1");
m2 = m2.add("a", "2");
m2 = m2.add("a", "3");
ImmutableFactory factory3 = registry.getImmutableFactory(m2);
assertTrue(factory3 instanceof MapNodeFactory);
assertEquals(96, factory3.getDurableLength(m2));
ByteBuffer byteBuffer2 = ByteBuffer.allocate(factory3.getDurableLength(m2));
factory3.writeDurable(m2, byteBuffer2);
assertEquals(96, byteBuffer2.position());
byteBuffer2.flip();
ImmutableFactory factory4 = registry.readId(byteBuffer2);
assertTrue(factory4 instanceof MapNodeFactory);
Object object4 = factory4.deserialize(byteBuffer2);
assertEquals(96, byteBuffer2.position());
assertEquals("123", String.join("", ((MapNode) object4).getList("a").flatList()));
} finally {
Plant.close();
}
}
/**
* Build a list using the path found from the start node to the goal node.
*
* @param dqNode The goal node
* @param parent The hashmap of every node's parent
* @return The list of intersections that form the shortest (unweighted) path from start to goal
* (including both start and goal).
*/
private List<GeographicPoint> buildPath(MapNode dqNode, HashMap<MapNode, MapNode> parent) {
List<MapNode> mapPath = new ArrayList<MapNode>();
List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();
boolean loop = true;
while (loop) {
mapPath.add(dqNode);
dqNode = parent.get(dqNode);
if (parent.get(dqNode) == null) {
mapPath.add(dqNode);
loop = false;
}
}
Collections.reverse(mapPath);
for (MapNode n : mapPath) {
listCoordinates.add(n.getCoordinates());
}
return listCoordinates;
}
@Override
public void nodesSelected(Node[] nodes) {
if (selectedNodes != nodes) {
if (selectedMapNodes != null) {
for (MapNode node : selectedMapNodes) {
node.isSelected = false;
}
}
selectedNodes = nodes;
if (nodes == null || nodes.length == 0) {
selectedNode = null;
selectedMapNodes = null;
} else {
selectedMapNodes = new MapNode[nodes.length];
for (int i = 0, n = nodes.length; i < n; i++) {
selectedMapNodes[i] = addMapNode(nodes[i]);
selectedMapNodes[i].isSelected = true;
}
selectedNode = selectedMapNodes[0];
}
repaint();
}
}
/**
* 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) {
Queue<MapNode> q = new LinkedList<MapNode>(); // Initialize queue
HashSet<MapNode> visited = new HashSet<MapNode>(); // Initialize visited HashSet
HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap
MapNode startNode = vertices.get(start); // get start node
q.add(startNode); // enqueue start node
visited.add(startNode); // add start node to visited
// Hook for visualization. See writeup.
nodeSearched.accept(startNode.getCoordinates());
while (q.isEmpty() == false) { // while queue is not empty
MapNode dqNode = q.remove(); // dequeue current node from the front of the queue
nodeSearched.accept(dqNode.getCoordinates());
if (vertices.get(goal)
== vertices.get(
dqNode.getCoordinates())) { // if current node = goal, then return the parent map
List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();
listCoordinates =
buildPath(
dqNode,
parent); // Build a list using the path found from the start node to the goal node.
return listCoordinates;
}
for (MapEdge e :
dqNode.getEdges()) { // for each of the current node's neighbors not in the visited set
MapNode n = vertices.get(e.end);
if (visited.contains(n) == false) {
visited.add(n);
parent.put(n, dqNode);
q.add(n);
}
}
} // while
return null;
}
// get a set of neighbor nodes from a mapnode
private Set<MapNode> getNeighbors(MapNode node) {
return node.getNeighbors();
}
// Add an edge when you already know the nodes involved in the edge
private void addEdge(MapNode n1, MapNode n2, String roadName, String roadType, double length) {
MapEdge edge = new MapEdge(roadName, roadType, n1, n2, length);
edges.add(edge);
n1.addEdge(edge);
}
public void paint(Graphics g) {
Graphics2D g2d = (Graphics2D) g;
int lx = 10;
super.paint(g);
long time = System.currentTimeMillis();
if (mapImage != null) {
mapImage.paintIcon(this, g, 0, 0);
}
MapNode[] nodes = nodeList;
if (nodes != null) {
Line2D line = new Line2D.Double();
for (int i = 0, m = nodes.length; i < m; i++) {
MapNode n = nodes[i];
int x, y;
if (n.node.hasLocation()) {
x = n.node.getX();
y = n.node.getY();
} else {
x = lx;
y = 10;
lx += 30;
}
if (!hideNetwork) {
FontMetrics fm = g.getFontMetrics();
int fnHeight = fm.getHeight();
int fnDescent = fm.getDescent();
for (int j = 0, mu = n.node.getLinkCount(); j < mu; j++) {
Link link = n.node.getLink(j);
if (link.node.hasLocation()) {
long age = (time - link.getLastActive()) / 100;
int x2 = link.node.getX();
int y2 = link.node.getY();
if (n.isSelected) {
if (age > LINK_COLOR.length) {
age = 100;
} else {
age -= 50;
}
}
line.setLine(x, y, x2, y2);
if (age < LINK_COLOR.length) {
g.setColor(age < 0 ? LINK_COLOR[0] : LINK_COLOR[(int) age]);
} else {
g.setColor(LINK_COLOR[LINK_COLOR.length - 1]);
}
g2d.draw(line);
// g.setColor(Color.lightGray);
int xn1, xn2, yn1, yn2;
if (x <= x2) {
xn1 = x;
xn2 = x2;
yn1 = y;
yn2 = y2;
} else {
xn1 = x2;
xn2 = x;
yn1 = y2;
yn2 = y;
}
int dx = xn1 + (xn2 - xn1) / 2 + 4;
int dy = yn1 + (yn2 - yn1) / 2 - fnDescent;
if (yn2 < yn1) {
dy += fnHeight - fnDescent;
}
g.drawString("ETX:" + (((int) (link.getETX() * 100 + 0.5)) / 100.0), dx, dy);
}
}
}
n.paint(g, x, y);
g.setColor(Color.black);
if (n.isSelected) {
BasicGraphicsUtils.drawDashedRect(g, x - delta, y - delta, 2 * delta, 2 * delta);
}
if (selectedNode != null && selectedNode.message != null) {
g.drawString(selectedNode.message, 10, 10);
}
}
}
}
@Override
public int hashCode() {
return startLocation.hashCode() + endLocation.hashCode();
}
/**
* 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;
}
/**
* Initialize the vertices before running Dijkstra;s algorithm. The start node's weight is
* initialized to 0, and every other vertex's weight is initialized to infinity.
*
* @param startNode The starting node
* @param vertices Every vertex that is not the start node
*/
public void initializeNodes(MapNode startNode, HashMap<GeographicPoint, MapNode> vertices) {
for (MapNode n : vertices.values()) {
n.setWeight(Double.POSITIVE_INFINITY);
}
startNode.setWeight(0);
}
/**
* 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;
}
/**
* 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 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;
}
@Override
public void serialize(Object durable, ByteBuffer byteBuffer) {
((MapNode) durable).serialize(byteBuffer);
}
