Exemple #1
0
 public double expected_yield(int count) {
   if (count > 10) return 0.0;
   if (nodeNumber.intValue() == 0) return 1.0;
   Integer parent = MainClass.objectMaintainer.getParent(nodeNumber);
   if (parent == null) return 0.0;
   NodeInfo parentni = (NodeInfo) MainClass.sensorAnalyzer.proprietaryNodeInfo.get(parent);
   if (parentni == null) return 0.0;
   return link_quality * parentni.expected_yield(count + 1);
 }
Exemple #2
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;
 }
Exemple #3
0
 /**
  * initial the end node set, if the end node is intersect, add it directly or we find the end
  * node's edge's entrance(s)
  *
  * @param nodeId
  * @return
  */
 public static HashSet<Long> initialEndSet(long nodeId) {
   HashSet<Long> endSet = new HashSet<Long>();
   NodeInfo end = OSMData.nodeHashMap.get(nodeId);
   // initial start end set
   if (end.isIntersect()) {
     endSet.add(nodeId);
   } else {
     EdgeInfo edge = end.getOnEdgeList().getFirst();
     if (!edge.isOneway()) {
       endSet.add(edge.getEndNode());
     }
     endSet.add(edge.getStartNode());
   }
   return endSet;
 }
Exemple #4
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
   }
 }
Exemple #5
0
  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;
  }
Exemple #6
0
  public static void tdsp() {
    // node list used for store path
    ArrayList<Long> pathNodeList = new ArrayList<Long>();

    // test start end node
    // OSMOutput.generateStartEndlNodeKML(START_NODE, END_NODE, nodeHashMap);
    // test count time
    long begintime = System.currentTimeMillis();

    LocationInfo startLoc = new LocationInfo(OSMRouteParam.START_LAT, OSMRouteParam.START_LON);
    LocationInfo endLoc = new LocationInfo(OSMRouteParam.END_LAT, OSMRouteParam.END_LON);
    NodeInfo start = startLoc.searchNode();
    NodeInfo end = endLoc.searchNode();

    double cost;
    // routing using A* algorithm with priority queue
    // cost = routingAStar(start.getNodeId(), end.getNodeId(), OSMRouteParam.START_TIME,
    // OSMRouteParam.DAY_INDEX, pathNodeList);

    // routing using A* algorithm with fibonacci heap
    // cost = routingAStarFibonacci(start.getNodeId(), end.getNodeId(), OSMRouteParam.START_TIME,
    // OSMRouteParam.DAY_INDEX, pathNodeList);

    // routing using bidirectional hierarchy search
    cost =
        routingHierarchy(
            start.getNodeId(),
            end.getNodeId(),
            OSMRouteParam.START_TIME,
            OSMRouteParam.DAY_INDEX,
            pathNodeList);

    long endtime = System.currentTimeMillis();
    long response = (endtime - begintime);
    System.out.println("routing cost: " + cost + " s, response time: " + response + " ms");

    // output kml
    OSMOutput.generatePathKML(pathNodeList);
    // OSMOutput.generatePathNodeKML(pathNodeList);

    String turnByTurn = turnByTurn(pathNodeList);
    System.out.println(turnByTurn);
  }
Exemple #7
0
 public NodeInfoHelper getEndNodeHelper(long endNode) {
   // TODO: not time dependent now, need to modify
   NodeInfo end = OSMData.nodeHashMap.get(endNode);
   NodeInfoHelper endNodeHelper = new NodeInfoHelper(endNode);
   if (getNodeId() == end.getNodeId()) { // no need to do in edge routing
     return this;
   } else {
     EdgeInfo edge = end.getOnEdgeList().getFirst();
     int distance;
     // from start to middle
     if (getNodeId() == edge.getStartNode()) {
       distance = edge.getStartDistance(endNode);
     } else { // from end to middle
       distance = edge.getEndDistance(endNode);
     }
     double speed = edge.getTravelSpeed();
     int travelTime = 1; // second
     travelTime = (int) Math.round(distance / speed);
     endNodeHelper.setCost(getCost() + travelTime);
     endNodeHelper.setParentId(getNodeId());
   }
   return endNodeHelper;
 }
Exemple #8
0
 /**
  * estimate heuristic, using distance/speed
  *
  * @param curNode
  * @param endNode
  * @return
  */
 public static double estimateHeuristic(long curNode, long endNode) {
   NodeInfo cur = OSMData.nodeHashMap.get(curNode);
   NodeInfo end = OSMData.nodeHashMap.get(endNode);
   double distance = Geometry.calculateDistance(cur.getLocation(), end.getLocation());
   return distance / OSMRouteParam.HEURISTIC_SPEED * OSMParam.SECOND_PER_HOUR;
 }
Exemple #9
0
  // TODO : if start or end is already in the highway, will occur the problem, need to fix
  public static double routingHierarchy(
      long startNode, long endNode, int startTime, int dayIndex, ArrayList<Long> pathNodeList) {
    // System.out.println("start finding the path...");
    int debug = 0;
    try {
      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;
      }

      NodeInfo start = OSMData.nodeHashMap.get(startNode);
      NodeInfo end = OSMData.nodeHashMap.get(endNode);
      double minDistance = Geometry.calculateDistance(start.getLocation(), end.getLocation());
      if (minDistance < 5) { // use normal A* algorithm to calculate small distance
        return routingAStar(
            start.getNodeId(),
            end.getNodeId(),
            OSMRouteParam.START_TIME,
            OSMRouteParam.DAY_INDEX,
            pathNodeList);
      }

      SearchingSharing sharingData = new SearchingSharing();
      HashMap<Long, NodeInfoHelper> nodeForwardCache = new HashMap<Long, NodeInfoHelper>();
      HashMap<Long, NodeInfoHelper> nodeReverseCache = new HashMap<Long, NodeInfoHelper>();
      ForwardSearching forwardSearching =
          new ForwardSearching(
              startNode, endNode, startTime, dayIndex, sharingData, nodeForwardCache);
      ReverseSearching reverseSearching =
          new ReverseSearching(endNode, startNode, sharingData, nodeReverseCache);
      // two thread run simultaneously
      Thread forwardThread = new Thread(forwardSearching);
      Thread reverseThread = new Thread(reverseSearching);
      // search forward
      forwardThread.start();
      // let forward searching for a while
      // Thread.sleep(100);
      // search reverse
      reverseThread.start();
      // waiting for thread finish
      forwardThread.join();
      reverseThread.join();
      // get the searching intersects
      ArrayList<Long> intersectList = sharingData.getIntersectList();
      // pick the least cost one according to time-dependent
      double minCost = Double.MAX_VALUE;
      ArrayList<Long> minCostPath = new ArrayList<Long>();
      for (long intersect : intersectList) {
        NodeInfoHelper current = nodeForwardCache.get(intersect);
        // cost from source to intersect
        double cost = current.getCost();
        current = nodeReverseCache.get(intersect);
        // update the reverse cost as forward cost
        current.setCost(cost);
        ArrayList<Long> reversePath = new ArrayList<Long>();
        double totalCost = Double.MAX_VALUE;
        // recalculate from intersect to destination
        while (true) {
          long nodeId = current.getNodeId();
          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;
          long nextNodeId = current.getParentId();
          double arriveTime = current.getCost();
          // arrive end
          if (nextNodeId == 0) {
            totalCost = arriveTime;
            break;
          }
          // add node
          reversePath.add(nextNodeId);
          // calculate cost according adjlist
          LinkedList<ToNodeInfo> adjNodeList = OSMData.adjListHashMap.get(nodeId);
          double costTime = 0;
          for (ToNodeInfo toNode : adjNodeList) {
            if (toNode.getNodeId() == nextNodeId) {
              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);
              costTime = arriveTime + (double) travelTime / OSMParam.MILLI_PER_SECOND;
              break;
            }
          }
          current = nodeReverseCache.get(nextNodeId);
          if (costTime == 0) System.err.println("cost time cannot be zero!");
          else current.setCost(costTime);
        }

        // process the left nodes to real destination
        long lastNode = reversePath.get(reversePath.size() - 1);
        if (lastNode != endNode) {
          NodeInfo last = OSMData.nodeHashMap.get(lastNode);
          NodeInfo dest = OSMData.nodeHashMap.get(endNode);
          EdgeInfo onEdge = last.getEdgeFromNodes(dest);
          current = nodeReverseCache.get(lastNode);
          int totalDistance = onEdge.getDistance();
          int distance;
          long toNodeId;
          if (onEdge.getStartNode() == lastNode) { // from start to middle
            distance = onEdge.getStartDistance(endNode);
            toNodeId = onEdge.getEndNode();
          } else { // from end to middle
            distance = onEdge.getEndDistance(endNode);
            toNodeId = onEdge.getStartNode();
          }
          LinkedList<ToNodeInfo> adjNodeList = OSMData.adjListHashMap.get(lastNode);
          double costTime = 0;
          int timeIndex =
              startTime
                  + (int) (totalCost / 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;
          for (ToNodeInfo toNode : adjNodeList) {
            if (toNode.getNodeId() == toNodeId) {
              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);
              costTime = (double) travelTime / OSMParam.MILLI_PER_SECOND;
              break;
            }
          }
          if (costTime != 0) {
            costTime *= (double) distance / totalDistance;
          }
          totalCost += costTime; // add cost
          reversePath.add(endNode); // add dest
        }

        // if found less cost path, build forward path
        if (totalCost < minCost) {
          ArrayList<Long> forwardPath = new ArrayList<Long>();
          minCost = totalCost;
          current = nodeForwardCache.get(intersect);
          long traceNodeId = current.getParentId();
          while (traceNodeId != 0) {
            forwardPath.add(traceNodeId); // add node
            current = nodeForwardCache.get(traceNodeId);
            traceNodeId = current.getParentId();
          }
          Collections.reverse(forwardPath); // reverse the path list
          // record min-cost path, combine forward path and reverse path
          minCostPath = new ArrayList<Long>();
          minCostPath.addAll(forwardPath);
          minCostPath.add(intersect);
          minCostPath.addAll(reversePath);
          // output kml
          // OSMOutput.generatePathKML(nodeHashMap, pathNodeList, "path_" + intersect);
          // ArrayList<Long> intersectNode = new ArrayList<Long>();
          // intersectNode.add(intersect);
          // OSMOutput.generatePathNodeKML(nodeHashMap, intersectNode, "intersect_" + intersect);
        }
      }
      pathNodeList.addAll(minCostPath);
      return minCost;
    } catch (Exception e) {
      e.printStackTrace();
      System.err.println("routingHierarchy: debug code " + debug);
    }
    return 0;
  }
Exemple #10
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");
  }
Exemple #11
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");
  }
Exemple #12
0
  // 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;
  }