@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"); }
@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"); }
// 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; }