/**
* initial for routing, if the start node is an intersect, then it should already in the adjlist,
* than we just add it. If the start node is not intersect, then the node will not in adjlist, we
* need find the node's two edge entrance if the edge is bidirection or one edge entrance if the
* edge is oneway.
*
* @param startNode
* @param endNode
* @param startTime
* @param dayIndex
* @param nodeHelperCache
*/
public static PriorityQueue<NodeInfoHelper> initialStartSet(
long startNode,
long endNode,
int startTime,
int dayIndex,
HashMap<Long, NodeInfoHelper> nodeHelperCache) {
PriorityQueue<NodeInfoHelper> openSet =
new PriorityQueue<NodeInfoHelper>(
10000,
new Comparator<NodeInfoHelper>() {
public int compare(NodeInfoHelper n1, NodeInfoHelper n2) {
return (int) (n1.getTotalCost() - n2.getTotalCost());
}
});
NodeInfo start = OSMData.nodeHashMap.get(startNode);
NodeInfoHelper current;
// initial start end set
if (start.isIntersect()) {
// initial
current = new NodeInfoHelper(startNode);
current.setCost(0);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(startNode, endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
} else {
EdgeInfo edge = start.getOnEdgeList().getFirst();
double travelTime = 1; // second
int distance; // feet
int totalDistance = edge.getDistance(); // feet
if (!edge.isOneway()) {
// distance from start to middle
distance = edge.getStartDistance(startNode);
travelTime = edge.getTravelTime(startTime, dayIndex, false);
travelTime *= (double) distance / totalDistance;
travelTime /= OSMParam.MILLI_PER_SECOND;
current = new NodeInfoHelper(edge.getStartNode());
current.setCost(travelTime);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(edge.getStartNode(), endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
}
// distance from middle to end
distance = edge.getEndDistance(startNode);
travelTime = edge.getTravelTime(startTime, dayIndex, true);
travelTime *= (double) distance / totalDistance;
current = new NodeInfoHelper(edge.getEndNode());
current.setCost(travelTime);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(edge.getEndNode(), endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
}
return openSet;
}
/**
* using low bound for travel time, for reverse searching
*
* @param endNode
* @param startNode
* @param openSet
* @param nodeHelperCache
*/
public void initialEndSet(
long startNode,
long endNode,
PriorityQueue<NodeInfoHelper> openSet,
HashMap<Long, NodeInfoHelper> nodeHelperCache) {
NodeInfo start = OSMData.nodeHashMap.get(startNode);
NodeInfoHelper current;
// initial start end set
if (start.isIntersect()) {
// initial
current = new NodeInfoHelper(startNode);
current.setCost(0);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(startNode, endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
} else {
EdgeInfo edge = start.getOnEdgeList().getFirst();
double travelTime = 1; // second
int distance; // feet
int totalDistance = edge.getDistance(); // feet
if (!edge.isOneway()) {
// distance from start to middle
distance = edge.getStartDistance(startNode);
// get low bound of travel time
travelTime = edge.getTravelTimeMin(false);
travelTime *= (double) distance / totalDistance;
current = new NodeInfoHelper(edge.getStartNode());
current.setCost(travelTime);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(edge.getStartNode(), endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
}
// distance from middle to end
distance = edge.getEndDistance(startNode);
travelTime = edge.getTravelTimeMin(true);
travelTime *= (double) distance / totalDistance;
current = new NodeInfoHelper(edge.getEndNode());
current.setCost(travelTime);
current.setCurrentLevel(10);
current.setHeuristic(OSMRouting.estimateHeuristic(edge.getEndNode(), endNode));
openSet.offer(current); // push the start node
nodeHelperCache.put(current.getNodeId(), current); // add cache
}
}
public static FibonacciHeap<NodeInfoHelper> initialStartSet(
long startNode,
long endNode,
HashMap<Long, FibonacciHeapNode<NodeInfoHelper>> nodeHelperCache) {
FibonacciHeap<NodeInfoHelper> openSet = new FibonacciHeap<NodeInfoHelper>();
NodeInfo start = OSMData.nodeHashMap.get(startNode);
NodeInfoHelper initial;
FibonacciHeapNode<NodeInfoHelper> fInitial;
// initial start end set
if (start.isIntersect()) {
// initial
initial = new NodeInfoHelper(startNode);
initial.setCost(0);
initial.setHeuristic(estimateHeuristic(startNode, endNode));
fInitial = new FibonacciHeapNode<NodeInfoHelper>(initial);
openSet.insert(fInitial, initial.getTotalCost()); // push the start node
nodeHelperCache.put(initial.getNodeId(), fInitial); // add cache
} else {
EdgeInfo edge = start.getOnEdgeList().getFirst();
double speed = edge.getTravelSpeed();
int travelTime = 1; // second
int distance;
if (!edge.isOneway()) {
// distance from start to middle
distance = edge.getStartDistance(startNode);
travelTime = (int) Math.round(distance / speed * OSMParam.MILLI_PER_SECOND);
initial = new NodeInfoHelper(edge.getStartNode());
initial.setCost(travelTime);
initial.setHeuristic(estimateHeuristic(edge.getStartNode(), endNode));
fInitial = new FibonacciHeapNode<NodeInfoHelper>(initial);
openSet.insert(fInitial, initial.getTotalCost()); // push the start node
nodeHelperCache.put(initial.getNodeId(), fInitial); // add cache
}
distance = edge.getEndDistance(startNode);
travelTime = (int) Math.round(distance / speed * OSMParam.MILLI_PER_SECOND);
initial = new NodeInfoHelper(edge.getEndNode());
initial.setCost(travelTime);
initial.setHeuristic(estimateHeuristic(edge.getEndNode(), endNode));
fInitial = new FibonacciHeapNode<NodeInfoHelper>(initial);
openSet.insert(fInitial, initial.getTotalCost()); // push the start node
nodeHelperCache.put(initial.getNodeId(), fInitial); // add cache
}
return openSet;
}
/**
* routing using A* algorithm with fibonacci heap basically same as routingAStar function
*
* @param startNode
* @param endNode
* @param startTime
* @param pathNodeList return path
* @return
*/
public static double routingAStarFibonacci(
long startNode, long endNode, int startTime, int dayIndex, ArrayList<Long> pathNodeList) {
System.out.println("start finding the path...");
int debug = 0;
double totalCost = -1;
try {
// test store transversal nodes
// HashSet<Long> transversalSet = new HashSet<Long>();
if (!OSMData.nodeHashMap.containsKey(startNode)
|| !OSMData.nodeHashMap.containsKey(endNode)) {
System.err.println("cannot find start or end node!");
return -1;
}
if (startNode == endNode) {
System.out.println("start node is the same as end node.");
return 0;
}
HashSet<Long> closedSet = new HashSet<Long>();
HashMap<Long, FibonacciHeapNode<NodeInfoHelper>> nodeHelperCache =
new HashMap<Long, FibonacciHeapNode<NodeInfoHelper>>();
FibonacciHeap<NodeInfoHelper> openSet = initialStartSet(startNode, endNode, nodeHelperCache);
HashSet<Long> endSet = initialEndSet(endNode);
NodeInfoHelper current = null;
FibonacciHeapNode<NodeInfoHelper> fCurrent = null;
while (!openSet.isEmpty()) {
// remove current from openset
fCurrent = openSet.min();
openSet.removeMin();
current = fCurrent.getData();
// if(!transversalSet.contains(current.getNodeId()))
// transversalSet.add(current.getNodeId());
long nodeId = current.getNodeId();
// add current to closedset
closedSet.add(nodeId);
if (endSet.contains(nodeId)) { // find the destination
current = current.getEndNodeHelper(endNode);
totalCost = current.getCost();
break;
}
// for time dependent routing
int timeIndex =
startTime
+ (int)
(current.getCost() / OSMParam.SECOND_PER_MINUTE / OSMRouteParam.TIME_INTERVAL);
if (timeIndex
> OSMRouteParam.TIME_RANGE
- 1) // time [6am - 9 pm], we regard times after 9pm as constant edge weights
timeIndex = OSMRouteParam.TIME_RANGE - 1;
LinkedList<ToNodeInfo> adjNodeList = OSMData.adjListHashMap.get(nodeId);
if (adjNodeList == null) continue; // this node cannot go anywhere
double arriveTime = current.getCost();
// for each neighbor in neighbor_nodes(current)
for (ToNodeInfo toNode : adjNodeList) {
debug++;
long toNodeId = toNode.getNodeId();
int travelTime;
if (toNode.isFix()) // fix time
travelTime = toNode.getTravelTime();
else // fetch from time array
travelTime = toNode.getSpecificTravelTime(dayIndex, timeIndex);
// tentative_g_score := g_score[current] + dist_between(current,neighbor)
double costTime = arriveTime + (double) travelTime / OSMParam.MILLI_PER_SECOND;
// tentative_f_score := tentative_g_score + heuristic_cost_estimate(neighbor, goal)
double heuristicTime = estimateHeuristic(toNodeId, endNode);
double totalCostTime = costTime + heuristicTime;
// if neighbor in closedset and tentative_f_score >= f_score[neighbor]
if (closedSet.contains(toNodeId)
&& nodeHelperCache.get(toNodeId).getData().getTotalCost() <= totalCostTime) {
continue;
}
NodeInfoHelper node = null;
FibonacciHeapNode<NodeInfoHelper> fNode = null;
// if neighbor not in openset or tentative_f_score < f_score[neighbor]
if (!nodeHelperCache.containsKey(toNodeId)) { // neighbor not in openset
// create new one
node = new NodeInfoHelper(toNodeId);
node.setCost(costTime);
node.setHeuristic(heuristicTime);
node.setParentId(nodeId);
fNode = new FibonacciHeapNode<NodeInfoHelper>(node);
openSet.insert(fNode, node.getTotalCost());
nodeHelperCache.put(node.getNodeId(), fNode);
} else if (nodeHelperCache.get(toNodeId).getData().getTotalCost()
> totalCostTime) { // neighbor in openset
fNode = nodeHelperCache.get(toNodeId);
node = fNode.getData();
// update information
node.setCost(costTime);
node.setHeuristic(heuristicTime);
node.setParentId(nodeId);
if (closedSet.contains(toNodeId)) { // neighbor in closeset
closedSet.remove(toNodeId); // remove neighbor form colseset
openSet.insert(fNode, node.getTotalCost());
} else { // neighbor in openset, decreaseKey
openSet.decreaseKey(fNode, node.getTotalCost());
}
}
}
}
if (totalCost != -1) {
long traceNodeId = current.getNodeId();
pathNodeList.add(traceNodeId); // add end node
traceNodeId = current.getParentId();
while (traceNodeId != 0) {
pathNodeList.add(traceNodeId); // add node
fCurrent = nodeHelperCache.get(traceNodeId);
current = fCurrent.getData();
traceNodeId = current.getParentId();
}
Collections.reverse(pathNodeList); // reverse the path list
System.out.println("find the path successful!");
} else {
System.out.println("can not find the path!");
}
// OSMOutput.generateTransversalNodeKML(transversalSet, nodeHashMap);
} catch (Exception e) {
e.printStackTrace();
System.err.println(
"tdsp: debug code " + debug + ", start node " + startNode + ", end node " + endNode);
}
return totalCost;
}
@Override
public void run() {
// TODO Auto-generated method stub
PriorityQueue<NodeInfoHelper> openSet =
new PriorityQueue<NodeInfoHelper>(
10000,
new Comparator<NodeInfoHelper>() {
public int compare(NodeInfoHelper n1, NodeInfoHelper n2) {
return (int) (n1.getTotalCost() - n2.getTotalCost());
}
});
HashSet<Long> closedSet = new HashSet<Long>();
initialEndSet(startNode, endNode, openSet, nodeHelperCache);
NodeInfoHelper current = null;
// HashSet<Long> transversalSet = new HashSet<Long>();
while (!openSet.isEmpty()) {
// remove current from openset
current = openSet.poll();
long nodeId = current.getNodeId();
// transversalSet.add(nodeId);
// add current to closedset
closedSet.add(nodeId);
// check if forward searching finish
if (sharingData.isSearchingFinish()) break;
// add to reverse cover nodes, only add the nodes on the highway
if (current.getCurrentLevel() == 1) sharingData.addReverseNode(nodeId);
NodeInfo fromNodeInfo = OSMData.nodeHashMap.get(nodeId);
LinkedList<ToNodeInfo> adjNodeList = adjListHashMap.get(nodeId);
if (adjNodeList == null) continue; // this node cannot go anywhere
double arriveTime = current.getCost();
// for each neighbor in neighbor_nodes(current)
for (ToNodeInfo toNode : adjNodeList) {
long toNodeId = toNode.getNodeId();
NodeInfo toNodeInfo = OSMData.nodeHashMap.get(toNodeId);
EdgeInfo edgeInfo = fromNodeInfo.getEdgeFromNodes(toNodeInfo);
// check if "highway" not found in param, add it
// String highway = edgeInfo.getHighway();
int level = 10;
if (OSMData.hierarchyHashMap.containsKey(edgeInfo.getHighway()))
level = OSMData.hierarchyHashMap.get(edgeInfo.getHighway());
// 1) always level up, e.g. currently in primary, never go secondary
// if(level > current.getCurrentLevel()) { // do not level down
// continue;
// }
// 2) keep on highway
if (current.getCurrentLevel() == 1 && level > 1) {
continue;
}
int travelTime = toNode.getMinTravelTime();
// tentative_g_score := g_score[current] + dist_between(current,neighbor)
double costTime = arriveTime + (double) travelTime / OSMParam.MILLI_PER_SECOND;
// tentative_f_score := tentative_g_score + heuristic_cost_estimate(neighbor, goal)
double heuristicTime = OSMRouting.estimateHeuristic(toNodeId, endNode);
double totalCostTime = costTime + heuristicTime;
// if neighbor in closedset and tentative_f_score >= f_score[neighbor]
if (closedSet.contains(toNodeId)
&& nodeHelperCache.get(toNodeId).getTotalCost() <= totalCostTime) {
continue;
}
NodeInfoHelper node = null;
// if neighbor not in openset or tentative_f_score < f_score[neighbor]
if (!nodeHelperCache.containsKey(toNodeId)) { // neighbor not in openset
node = new NodeInfoHelper(toNodeId);
nodeHelperCache.put(node.getNodeId(), node);
} else if (nodeHelperCache.get(toNodeId).getTotalCost()
> totalCostTime) { // neighbor in openset
node = nodeHelperCache.get(toNodeId);
if (closedSet.contains(toNodeId)) { // neighbor in closeset
closedSet.remove(toNodeId); // remove neighbor form colseset
} else {
openSet.remove(node);
}
}
// neighbor need update
if (node != null) {
node.setCost(costTime);
node.setHeuristic(heuristicTime);
node.setCurrentLevel(level);
node.setParentId(nodeId);
openSet.offer(node); // add neighbor to openset again
}
}
}
// OSMOutput.generateTransversalNodeKML(transversalSet, nodeHashMap, "reverse_transversal");
}
@Override
public void run() {
HashSet<Long> closedSet = new HashSet<Long>();
PriorityQueue<NodeInfoHelper> openSet =
OSMRouting.initialStartSet(startNode, endNode, startTime, dayIndex, nodeHelperCache);
NodeInfoHelper current = null;
// test
// HashSet<Long> transversalSet = new HashSet<Long>();
while (!openSet.isEmpty()) {
// remove current from openset
current = openSet.poll();
long nodeId = current.getNodeId();
// transversalSet.add(nodeId);
// add current to closedset
closedSet.add(nodeId);
// check reverse searching covering nodes
if (sharingData.isNodeInReverseSet(nodeId)) {
// we found a path, stop here
sharingData.addIntersect(nodeId);
if (sharingData.isSearchingFinish()) break;
else continue;
}
NodeInfo fromNodeInfo = OSMData.nodeHashMap.get(nodeId);
// for time dependent routing in forwarding search
int timeIndex =
startTime
+ (int)
(current.getCost() / OSMParam.SECOND_PER_MINUTE / OSMRouteParam.TIME_INTERVAL);
if (timeIndex
> OSMRouteParam.TIME_RANGE
- 1) // time [6am - 9 pm], we regard times after 9pm as constant edge weights
timeIndex = OSMRouteParam.TIME_RANGE - 1;
LinkedList<ToNodeInfo> adjNodeList = adjListHashMap.get(nodeId);
if (adjNodeList == null) continue; // this node cannot go anywhere
double arriveTime = current.getCost();
// for each neighbor in neighbor_nodes(current)
for (ToNodeInfo toNode : adjNodeList) {
long toNodeId = toNode.getNodeId();
NodeInfo toNodeInfo = OSMData.nodeHashMap.get(toNodeId);
EdgeInfo edgeInfo = fromNodeInfo.getEdgeFromNodes(toNodeInfo);
// check if "highway" not found in param, add it
// String highway = edgeInfo.getHighway();
int level = 10;
if (OSMData.hierarchyHashMap.containsKey(edgeInfo.getHighway()))
level = OSMData.hierarchyHashMap.get(edgeInfo.getHighway());
// 1) always level up, e.g. currently in primary, never go secondary
// if(level > current.getCurrentLevel()) { // do not level down
// continue;
// }
// 2) keep on highway
if (current.getCurrentLevel() == 1 && level > 1) {
continue;
}
int travelTime;
// forward searching is time dependent
if (toNode.isFix()) // fix time
travelTime = toNode.getTravelTime();
else // fetch from time array
travelTime = toNode.getSpecificTravelTime(dayIndex, timeIndex);
// tentative_g_score := g_score[current] + dist_between(current,neighbor)
double costTime = arriveTime + (double) travelTime / OSMParam.MILLI_PER_SECOND;
// tentative_f_score := tentative_g_score + heuristic_cost_estimate(neighbor, goal)
double heuristicTime = OSMRouting.estimateHeuristic(toNodeId, endNode);
double totalCostTime = costTime + heuristicTime;
// if neighbor in closedset and tentative_f_score >= f_score[neighbor]
if (closedSet.contains(toNodeId)
&& nodeHelperCache.get(toNodeId).getTotalCost() <= totalCostTime) {
continue;
}
NodeInfoHelper node = null;
// if neighbor not in openset or tentative_f_score < f_score[neighbor]
if (!nodeHelperCache.containsKey(toNodeId)) { // neighbor not in openset
node = new NodeInfoHelper(toNodeId);
nodeHelperCache.put(node.getNodeId(), node);
} else if (nodeHelperCache.get(toNodeId).getTotalCost()
> totalCostTime) { // neighbor in openset
node = nodeHelperCache.get(toNodeId);
if (closedSet.contains(toNodeId)) { // neighbor in closeset
closedSet.remove(toNodeId); // remove neighbor form colseset
} else {
openSet.remove(node);
}
}
// neighbor need update
if (node != null) {
node.setCost(costTime);
node.setHeuristic(heuristicTime);
node.setCurrentLevel(level);
node.setParentId(nodeId);
openSet.offer(node); // add neighbor to openset again
}
}
}
// test
// OSMOutput.generateTransversalNodeKML(transversalSet, "forward_transversal");
}