public void relaxEdges(FibonacciHeap<Vertex> unvisited) { Vertex[] vertices = Graph.this.vertices; for (Edge edge : edges) { Vertex to = vertices[edge.to]; int newDistance = distance + edge.cost; if (newDistance < to.distance) { to.distance = newDistance; unvisited.decreaseKey(to.heapEntry, newDistance); } } }
private void generateShortestPathsFrom(GraphNode source) throws NegativeEdgeException { Iterator<GraphNode> l = g.getNodeIterator(); ListIterator<GraphEdge> m; FibonacciHeap<GraphNode> h = new FibonacciHeap<GraphNode>(); GraphNode t, v; GraphEdge e; h.insert(source, 0); data(source).setPrev(null); data(source).setIncidentEdge(null); this.source = source; while (l.hasNext()) { t = l.next(); if (t == source) continue; data(t).setPrev(null); data(t).setIncidentEdge(null); h.insert(t, Integer.MAX_VALUE); } while (!h.isEmpty()) { t = h.extractMin(); m = t.getEdgeIterator(); while (m.hasNext()) { e = m.next(); if (g.getNodeById(e.getTarget().getId()) == null) { m.remove(); continue; } if (e.getWeight() < 0) { throw new NegativeEdgeException( "Dijkstra's algorithm won't work on graphs with negative edge weights!"); } v = e.getTarget(); if (h.getKey(v) > h.getKey(t) + e.getWeight()) { try { h.decreaseKey(v, h.getKey(t) + e.getWeight()); } catch (KeyNotLessException x) { // can't happen! } data(v).setPrev(t); data(v).setIncidentEdge(e); } } } }
/** * 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; }