예제 #1
0
  /**
   * Initialize gScore and fScore
   *
   * @param gScore
   * @param fScore
   * @param network
   * @param startNode
   * @param endNode
   * @param heutistics
   */
  public static void initialize(
      Map<NodeInfo, Double> gScore,
      Map<NodeInfo, Double> fScore,
      LogicNetwork network,
      NodeInfo startNode,
      NodeInfo endNode,
      AStarShortestPathHeuristics heutistics) {
    for (NodeInfo node : network.getNodes()) {
      gScore.put(node, Double.POSITIVE_INFINITY);
      fScore.put(node, Double.POSITIVE_INFINITY);
    }

    gScore.put(startNode, 0.0);
    fScore.put(startNode, heutistics.heuristics(startNode.getNodeId(), endNode.getNodeId()));
  }
예제 #2
0
  /**
   * Reconstruct the path from start node to end node
   *
   * @param result
   * @param endNode
   * @param traceMap
   * @return the path or {@code null} if error occurred
   */
  public static PathInfo reconstructPath(
      NodeInfo endNode, Map<Integer, Integer> traceMap, LogicNetwork network) {
    PathInfo result = new PathInfo();
    int current = endNode.getNodeId();
    ArrayList<LinkInfo> tmpLinks = new ArrayList<>(); // the links in the reverse order

    while (traceMap.containsKey(current)) {
      int previous = traceMap.get(current);
      LinkInfo linkInfo = getLink(previous, current, network);
      tmpLinks.add(linkInfo);
      current = previous;
    }

    // put the links in the correct order
    for (int i = tmpLinks.size() - 1; 0 <= i; i--) {
      result.getLinks().add(tmpLinks.get(i));
    }
    // calculate values properties of the path
    double pathCost = 0.0;
    double attractiveness = 0.0;
    for (LinkInfo linkInfo : result.getLinks()) {
      pathCost += linkInfo.getCost();
      attractiveness += linkInfo.getAttractiveness();
      result.getNodes().add(network.getNodes().get(linkInfo.getStartNode()));
    }
    result.setPathCost(pathCost);
    result.setAttractiveness(attractiveness);
    result.setNumLinks(result.getLinks().size());
    result.setSimple(true);
    result.getNodes().add(endNode);

    return result;
  }
예제 #3
0
  public static PathInfo findShortestPathRoadNetwork(
      LogicNetwork network,
      NodeInfo startNode,
      NodeInfo endNode,
      AStarShortestPathHeuristics heutistics) {
    /*
     * Cost from start along best known path.
     */
    Map<NodeInfo, Double> gScore = new HashMap<>();
    /*
     * Estimated total cost from start to goal through a node.
     */
    Map<NodeInfo, Double> fScore = new HashMap<>();

    initialize(gScore, fScore, network, startNode, endNode, heutistics);
    //		System.out.println(gScore.toString());
    //		System.out.println(fScore.toString());

    /*
     * The set of nodes already evaluated.
     */
    Set<NodeInfo> closedSet = new HashSet<>();
    /*
     * The set of tentative nodes to be evaluated, initially containing the start node
     */
    Set<NodeInfo> openSet = new HashSet<>();
    openSet.add(startNode);
    //		System.out.println(openSet.toString());
    /*
     * Priority queue contains nodes in openSet sorted by length from the start node
     */
    PriorityQueue<NodeInfo> openPriorityQueue =
        new PriorityQueue<>(new AStarNodeScoreComparator(fScore));
    openPriorityQueue.add(startNode);
    //		System.out.println(openPriorityQueue.toString());
    /*
     * The map of navigated nodes.
     */
    Map<Integer, Integer> traceMap = new HashMap<>();

    /*
     * The A* algorithm
     */
    while (openPriorityQueue.isEmpty() == false) {
      NodeInfo current = openPriorityQueue.remove();
      //			System.out.println("Current: " + current.toString());
      if (current.getNodeId() == endNode.getNodeId())
        return reconstructPath(endNode, traceMap, network);

      openSet.remove(current);
      closedSet.add(current);

      // update neighbors
      for (LinkInfo neighborLink : network.getLinks().get(current.getNodeId())) {
        // Ignore the neighbor which is already evaluated.
        NodeInfo neighborNode = network.getNodes().get(neighborLink.getEndNode());
        //				System.out.println("neighborNode: " + neighborNode.toString());

        if (closedSet.contains(neighborNode.getNodeId())) continue;

        // length of this path.
        double tentativeGScore = gScore.get(current) + neighborLink.getCost();

        if (openSet.contains(neighborNode) == false) {
          // Discover a new node
          openSet.add(neighborNode);
        } else if (gScore.get(neighborNode.getNodeId()) <= tentativeGScore) {
          // This is not a better path.
          continue;
        }

        //				System.out.println(" Put neighborNode: " + neighborNode.toString());
        // This path is the best until now. Record it!
        traceMap.put(neighborNode.getNodeId(), current.getNodeId());
        gScore.put(neighborNode, tentativeGScore);
        // length of this path + estimated length to the end node
        double tentativeFScore =
            tentativeGScore + heutistics.heuristics(neighborNode.getNodeId(), endNode.getNodeId());
        fScore.put(neighborNode, tentativeFScore);

        openPriorityQueue.add(neighborNode);
      }
    }

    return null;
  }