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; }