Пример #1
0
 /**
  * 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;
 }
Пример #2
0
 /**
  * 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
   }
 }
Пример #3
0
  /**
   * routing using A* algorithm http://en.wikipedia.org/wiki/A*_search_algorithm
   *
   * @param startNode
   * @param endNode
   * @param startTime
   * @param dayIndex
   * @param pathNodeList return path
   * @return
   */
  public static double routingAStar(
      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, NodeInfoHelper> nodeHelperCache = new HashMap<Long, NodeInfoHelper>();

      PriorityQueue<NodeInfoHelper> openSet =
          initialStartSet(startNode, endNode, startTime, dayIndex, nodeHelperCache);
      HashSet<Long> endSet = initialEndSet(endNode);

      NodeInfoHelper current = null;

      while (!openSet.isEmpty()) {
        // remove current from openset
        current = openSet.poll();

        // 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).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.setParentId(nodeId);
            openSet.offer(node); // add neighbor to openset again
          }
        }
      }
      if (totalCost != -1) {
        long traceNodeId = current.getNodeId();
        pathNodeList.add(traceNodeId); // add end node
        traceNodeId = current.getParentId();
        while (traceNodeId != 0) {
          pathNodeList.add(traceNodeId); // add node
          current = nodeHelperCache.get(traceNodeId);
          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;
  }
Пример #4
0
  @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");
  }
Пример #5
0
  @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");
  }