/**
* 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;
}
/**
* initial the end node set, if the end node is intersect, add it directly or we find the end
* node's edge's entrance(s)
*
* @param nodeId
* @return
*/
public static HashSet<Long> initialEndSet(long nodeId) {
HashSet<Long> endSet = new HashSet<Long>();
NodeInfo end = OSMData.nodeHashMap.get(nodeId);
// initial start end set
if (end.isIntersect()) {
endSet.add(nodeId);
} else {
EdgeInfo edge = end.getOnEdgeList().getFirst();
if (!edge.isOneway()) {
endSet.add(edge.getEndNode());
}
endSet.add(edge.getStartNode());
}
return endSet;
}
public NodeInfoHelper getEndNodeHelper(long endNode) {
// TODO: not time dependent now, need to modify
NodeInfo end = OSMData.nodeHashMap.get(endNode);
NodeInfoHelper endNodeHelper = new NodeInfoHelper(endNode);
if (getNodeId() == end.getNodeId()) { // no need to do in edge routing
return this;
} else {
EdgeInfo edge = end.getOnEdgeList().getFirst();
int distance;
// from start to middle
if (getNodeId() == edge.getStartNode()) {
distance = edge.getStartDistance(endNode);
} else { // from end to middle
distance = edge.getEndDistance(endNode);
}
double speed = edge.getTravelSpeed();
int travelTime = 1; // second
travelTime = (int) Math.round(distance / speed);
endNodeHelper.setCost(getCost() + travelTime);
endNodeHelper.setParentId(getNodeId());
}
return endNodeHelper;
}
// TODO : if start or end is already in the highway, will occur the problem, need to fix
public static double routingHierarchy(
long startNode, long endNode, int startTime, int dayIndex, ArrayList<Long> pathNodeList) {
// System.out.println("start finding the path...");
int debug = 0;
try {
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;
}
NodeInfo start = OSMData.nodeHashMap.get(startNode);
NodeInfo end = OSMData.nodeHashMap.get(endNode);
double minDistance = Geometry.calculateDistance(start.getLocation(), end.getLocation());
if (minDistance < 5) { // use normal A* algorithm to calculate small distance
return routingAStar(
start.getNodeId(),
end.getNodeId(),
OSMRouteParam.START_TIME,
OSMRouteParam.DAY_INDEX,
pathNodeList);
}
SearchingSharing sharingData = new SearchingSharing();
HashMap<Long, NodeInfoHelper> nodeForwardCache = new HashMap<Long, NodeInfoHelper>();
HashMap<Long, NodeInfoHelper> nodeReverseCache = new HashMap<Long, NodeInfoHelper>();
ForwardSearching forwardSearching =
new ForwardSearching(
startNode, endNode, startTime, dayIndex, sharingData, nodeForwardCache);
ReverseSearching reverseSearching =
new ReverseSearching(endNode, startNode, sharingData, nodeReverseCache);
// two thread run simultaneously
Thread forwardThread = new Thread(forwardSearching);
Thread reverseThread = new Thread(reverseSearching);
// search forward
forwardThread.start();
// let forward searching for a while
// Thread.sleep(100);
// search reverse
reverseThread.start();
// waiting for thread finish
forwardThread.join();
reverseThread.join();
// get the searching intersects
ArrayList<Long> intersectList = sharingData.getIntersectList();
// pick the least cost one according to time-dependent
double minCost = Double.MAX_VALUE;
ArrayList<Long> minCostPath = new ArrayList<Long>();
for (long intersect : intersectList) {
NodeInfoHelper current = nodeForwardCache.get(intersect);
// cost from source to intersect
double cost = current.getCost();
current = nodeReverseCache.get(intersect);
// update the reverse cost as forward cost
current.setCost(cost);
ArrayList<Long> reversePath = new ArrayList<Long>();
double totalCost = Double.MAX_VALUE;
// recalculate from intersect to destination
while (true) {
long nodeId = current.getNodeId();
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;
long nextNodeId = current.getParentId();
double arriveTime = current.getCost();
// arrive end
if (nextNodeId == 0) {
totalCost = arriveTime;
break;
}
// add node
reversePath.add(nextNodeId);
// calculate cost according adjlist
LinkedList<ToNodeInfo> adjNodeList = OSMData.adjListHashMap.get(nodeId);
double costTime = 0;
for (ToNodeInfo toNode : adjNodeList) {
if (toNode.getNodeId() == nextNodeId) {
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);
costTime = arriveTime + (double) travelTime / OSMParam.MILLI_PER_SECOND;
break;
}
}
current = nodeReverseCache.get(nextNodeId);
if (costTime == 0) System.err.println("cost time cannot be zero!");
else current.setCost(costTime);
}
// process the left nodes to real destination
long lastNode = reversePath.get(reversePath.size() - 1);
if (lastNode != endNode) {
NodeInfo last = OSMData.nodeHashMap.get(lastNode);
NodeInfo dest = OSMData.nodeHashMap.get(endNode);
EdgeInfo onEdge = last.getEdgeFromNodes(dest);
current = nodeReverseCache.get(lastNode);
int totalDistance = onEdge.getDistance();
int distance;
long toNodeId;
if (onEdge.getStartNode() == lastNode) { // from start to middle
distance = onEdge.getStartDistance(endNode);
toNodeId = onEdge.getEndNode();
} else { // from end to middle
distance = onEdge.getEndDistance(endNode);
toNodeId = onEdge.getStartNode();
}
LinkedList<ToNodeInfo> adjNodeList = OSMData.adjListHashMap.get(lastNode);
double costTime = 0;
int timeIndex =
startTime
+ (int) (totalCost / 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;
for (ToNodeInfo toNode : adjNodeList) {
if (toNode.getNodeId() == toNodeId) {
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);
costTime = (double) travelTime / OSMParam.MILLI_PER_SECOND;
break;
}
}
if (costTime != 0) {
costTime *= (double) distance / totalDistance;
}
totalCost += costTime; // add cost
reversePath.add(endNode); // add dest
}
// if found less cost path, build forward path
if (totalCost < minCost) {
ArrayList<Long> forwardPath = new ArrayList<Long>();
minCost = totalCost;
current = nodeForwardCache.get(intersect);
long traceNodeId = current.getParentId();
while (traceNodeId != 0) {
forwardPath.add(traceNodeId); // add node
current = nodeForwardCache.get(traceNodeId);
traceNodeId = current.getParentId();
}
Collections.reverse(forwardPath); // reverse the path list
// record min-cost path, combine forward path and reverse path
minCostPath = new ArrayList<Long>();
minCostPath.addAll(forwardPath);
minCostPath.add(intersect);
minCostPath.addAll(reversePath);
// output kml
// OSMOutput.generatePathKML(nodeHashMap, pathNodeList, "path_" + intersect);
// ArrayList<Long> intersectNode = new ArrayList<Long>();
// intersectNode.add(intersect);
// OSMOutput.generatePathNodeKML(nodeHashMap, intersectNode, "intersect_" + intersect);
}
}
pathNodeList.addAll(minCostPath);
return minCost;
} catch (Exception e) {
e.printStackTrace();
System.err.println("routingHierarchy: debug code " + debug);
}
return 0;
}