예제 #1
0
  /**
   * Find the path from start to goal using A-Star search
   *
   * @param start The starting location
   * @param goal The goal location
   * @return The list of intersections that form the shortest path from start to goal (including
   *     both start and goal).
   */
  public List<GeographicPoint> aStarSearch(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeAccepter) {
    boolean debug = false;
    // Set up
    if (start == null || goal == null)
      throw new NullPointerException("Cannot find route from or to null node");
    MapNode startNode = pointNodeMap.get(start);
    MapNode endNode = pointNodeMap.get(goal);
    if (startNode == null) {
      System.err.println("Start node " + start + " does not exist");
      return null;
    }
    if (endNode == null) {
      System.err.println("End node " + goal + " does not exist");
      return null;
    }

    HashMap<MapNode, MapNode> parentMap = new HashMap<MapNode, MapNode>();
    PriorityQueue<MapNode> toExplore = new PriorityQueue<MapNode>();
    HashSet<MapNode> visited = new HashSet<MapNode>();
    // initialize distance for all nodes
    for (MapNode n : pointNodeMap.values()) {
      n.setDistance(Double.POSITIVE_INFINITY);
      n.setActualDistance(Double.POSITIVE_INFINITY);
    }
    startNode.setDistance(0);
    startNode.setActualDistance(0);

    toExplore.add(startNode);

    int count = 0;
    MapNode next = null;
    while (!toExplore.isEmpty()) {
      next = toExplore.remove();

      nodeAccepter.accept(next.getLocation());
      count++;
      if (debug) {
        System.out.println(
            "\nA* visiting"
                + next
                + "\nActual = "
                + next.getActualDistance()
                + ", Pred: "
                + next.getDistance());
      }
      if (next.equals(endNode)) break;
      if (!visited.contains(next)) {
        visited.add(next);
        Set<MapEdge> edges = next.getEdges();
        for (MapEdge edge : edges) {
          MapNode neighbor = edge.getEndNode();
          if (!visited.contains(neighbor)) {

            double currDist = edge.getLength() + next.getActualDistance();
            // core of A* is just to add to currDist the cost of getting to
            // the destination
            double predDist = currDist + (neighbor.getLocation()).distance(endNode.getLocation());
            if (predDist < neighbor.getDistance()) {
              // debug
              if (debug) {
                System.out.println("Adding to queue node at: " + neighbor.getLocation());
                System.out.println("Curr dist: " + currDist + " Pred Distance: " + predDist);
              }
              parentMap.put(neighbor, next);
              neighbor.setActualDistance(currDist);
              neighbor.setDistance(predDist);
              toExplore.add(neighbor);
            }
          }
        }
      }
    }
    if (!next.equals(endNode)) {
      System.out.println("No path found from " + start + " to " + goal);
      return null;
    }
    // Reconstruct the parent path
    List<GeographicPoint> path = reconstructPath(parentMap, startNode, endNode);
    System.out.println("Nodes visited in search: " + count);
    return path;
  }
예제 #2
0
  /**
   * Find the path from start to goal using Dijkstra's algorithm
   *
   * @param start The starting location
   * @param goal The goal location
   * @return The list of intersections that form the shortest path from start to goal (including
   *     both start and goal).
   */
  public List<GeographicPoint> dijkstra(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
    if (start == null || goal == null)
      throw new NullPointerException("Cannot find route from or to null node");
    MapNode startNode = pointNodeMap.get(start);
    MapNode endNode = pointNodeMap.get(goal);
    if (startNode == null) {
      System.err.println("Start node " + start + " does not exist");
      return null;
    }
    if (endNode == null) {
      System.err.println("End node " + goal + " does not exist");
      return null;
    }
    HashMap<MapNode, MapNode> parentMap = new HashMap<MapNode, MapNode>();
    PriorityQueue<MapNode> toExplore = new PriorityQueue<MapNode>();
    HashSet<MapNode> visited = new HashSet<MapNode>();
    // initialize distance for all nodes
    for (MapNode n : pointNodeMap.values()) {
      n.setDistance(Double.POSITIVE_INFINITY);
    }
    startNode.setDistance(0);

    toExplore.add(startNode);
    MapNode next = null;
    int count = 0; // count visited

    while (!toExplore.isEmpty()) {
      next = toExplore.remove();

      // hook for visualization
      nodeSearched.accept(next.getLocation());
      count++;

      System.out.println("DIJKSTRA visiting" + next);
      if (next.equals(endNode)) break;
      if (!visited.contains(next)) {
        visited.add(next);
        Set<MapEdge> edges = next.getEdges();
        for (MapEdge edge : edges) {
          MapNode neighbor = edge.getEndNode();
          if (!visited.contains(neighbor)) {

            double currDist = edge.getLength() + next.getDistance();
            if (currDist < neighbor.getDistance()) {
              // debug
              // System.out.println("Distance: " + currDist + "Node\n"+neighbor);
              parentMap.put(neighbor, next);
              neighbor.setDistance(currDist);
              toExplore.add(neighbor);
            }
          }
        }
      }
    }
    if (!next.equals(endNode)) {
      System.out.println("No path found from " + start + " to " + goal);
      return null;
    }
    // Reconstruct the parent path
    List<GeographicPoint> path = reconstructPath(parentMap, startNode, endNode);
    System.out.println("Nodes visited in search: " + count);

    return path;
  }