コード例 #1
0
ファイル: OSMRouting.java プロジェクト: usunyu/ClearPath
  @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");
  }
コード例 #2
0
ファイル: OSMRouting.java プロジェクト: usunyu/ClearPath
  @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");
  }
コード例 #3
0
ファイル: OSMRouting.java プロジェクト: usunyu/ClearPath
  // 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;
  }