/**
* 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;
}
// 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;
}
/**
* add intersect to indicate one path has been found
*
* @param node
* @return true when done (found four intersects and paths)
*/
public void addIntersect(long node) {
intersectList.add(node);
}
public boolean isSearchingFinish() {
return intersectList.size() == OSMRouteParam.FOUND_PATH_COUNT;
}
// TODO : need to fix turn by turn ramp
public static String turnByTurn(ArrayList<Long> pathNodeList) {
String turnByTurnText = "";
int prevHierarchy = -1;
int prevDirIndex = -1;
String prevName = "";
long distance = 0;
boolean firstRoute = true;
NodeInfo prevNodeInfo = null;
if (pathNodeList == null || pathNodeList.size() == 0) {
return getErrorText() + OSMParam.LINEEND;
}
for (long nodeId : pathNodeList) {
if (prevNodeInfo == null) {
prevNodeInfo = OSMData.nodeHashMap.get(nodeId);
continue;
}
NodeInfo nodeInfo = OSMData.nodeHashMap.get(nodeId);
EdgeInfo edge = prevNodeInfo.getEdgeFromNodes(nodeInfo);
String highway = edge.getHighway();
String name = edge.getName();
int hierarchy = 6;
if (OSMData.hierarchyHashMap.containsKey(highway)) {
hierarchy = OSMData.hierarchyHashMap.get(highway);
}
int dirIndex = Geometry.getDirectionIndex(prevNodeInfo.getLocation(), nodeInfo.getLocation());
// initial prev
if (prevHierarchy == -1) {
prevHierarchy = hierarchy;
prevDirIndex = dirIndex;
prevName = name;
}
if (prevHierarchy > 1 && hierarchy == 1) { // from arterial to highway
if (firstRoute) {
// first route message
turnByTurnText = getFirstRouteText(prevDirIndex, prevName, name) + OSMParam.LINEEND;
firstRoute = false;
}
// distance message
turnByTurnText += getDistanceText(distance) + OSMParam.LINEEND;
turnByTurnText += getRampText(name) + OSMParam.LINEEND;
distance = 0;
} else if (prevHierarchy == 1 && hierarchy > 1) { // from highway to arterial
if (firstRoute) {
// first route message
turnByTurnText = getFirstRouteText(prevDirIndex, prevName, name) + OSMParam.LINEEND;
firstRoute = false;
}
// distance message
turnByTurnText += getDistanceText(distance) + OSMParam.LINEEND;
turnByTurnText += getExitText(name) + OSMParam.LINEEND;
distance = 0;
} else if (prevHierarchy == 1 && hierarchy == 1) { // on the highway
if (!prevName.equals(name)) { // change highway
if (firstRoute) {
// first route message
turnByTurnText = getFirstRouteText(prevDirIndex, prevName, name) + OSMParam.LINEEND;
firstRoute = false;
}
// distance message
turnByTurnText += getDistanceText(distance) + OSMParam.LINEEND;
turnByTurnText += getExitText(name) + OSMParam.LINEEND;
distance = 0;
}
} else { // on the arterial
// change direction or road happen
if (!Geometry.isSameDirection(dirIndex, prevDirIndex) || !prevName.equals(name)) {
if (firstRoute) {
// first route message
turnByTurnText = getFirstRouteText(prevDirIndex, prevName, name) + OSMParam.LINEEND;
firstRoute = false;
}
// distance message
turnByTurnText += getDistanceText(distance) + OSMParam.LINEEND;
if (!Geometry.isSameDirection(dirIndex, prevDirIndex)) { // change direction
turnByTurnText +=
getTurnText(prevDirIndex, dirIndex, prevName, name) + OSMParam.LINEEND;
} else { // change road
turnByTurnText += getMergeText(name) + OSMParam.LINEEND;
}
distance = 0;
}
}
prevNodeInfo = nodeInfo;
prevHierarchy = hierarchy;
prevDirIndex = dirIndex;
prevName = name;
distance += edge.getDistance();
}
// distance message
turnByTurnText += getDistanceText(distance) + OSMParam.LINEEND;
turnByTurnText += getArriveText() + OSMParam.LINEEND;
return turnByTurnText;
}