Esempio n. 1
0
  /**
   * Find the path from start to goal using breadth first search
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @return The list of intersections that form the shortest (unweighted) path from start to goal
   *     (including both start and goal).
   */
  public List<GeographicPoint> bfs(
      GeographicPoint start, GeographicPoint goal, Consumer<GeographicPoint> nodeSearched) {
    // TODO: Implement this method in WEEK 2

    // Hook for visualization.  See writeup.
    // nodeSearched.accept(next.getLocation());

    Queue<GeographicPoint> queue = new LinkedList<GeographicPoint>();
    Set<GeographicPoint> visited = new HashSet<GeographicPoint>();
    Map<GeographicPoint, GeographicPoint> parentMap =
        new HashMap<GeographicPoint, GeographicPoint>();
    queue.add(start);
    visited.add(start);
    while (!queue.isEmpty()) {
      GeographicPoint curr = queue.poll();
      nodeSearched.accept(curr);
      if (curr.equals(goal)) { // reaches the goal
        return RetrievePath(parentMap, start, goal); // retrieve the path
      }
      MapVertex node = Vertices.get(curr); // the MapVertex associated with the location curr
      Set<MapEdge> edges = node.getEdges(); // the edges connected to the node
      for (MapEdge edge : edges) {
        MapVertex next = edge.getEnd(); // neighbor of the location curr
        GeographicPoint nextLoc = next.getLocation();
        if (!visited.contains(nextLoc)) {
          visited.add(nextLoc);
          parentMap.put(nextLoc, curr); // set curr as the parent of next in tha path map
          queue.add(nextLoc);
        }
      }
    }
    /////// if the goal is not achievable
    return null;
  }
Esempio n. 2
0
  // method to print edge points out to a file
  public void printEdgePointsToFile(String filename) {

    try {
      PrintWriter writer = new PrintWriter(filename, "UTF-8");
      for (MapEdge e : edges) {
        writer.println(e.getStartPoint() + " " + e.getEndPoint());
      }
      writer.flush();
      writer.close();
    } catch (Exception e) {
      System.out.println("Exception opening file " + e);
    }
  }
Esempio n. 3
0
  /**
   * Find the path from start to goal using A-Star search
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @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> nodeSearched) {
    // TODO: Implement this method in WEEK 3

    // Hook for visualization.  See writeup.
    // nodeSearched.accept(next.getLocation());

    PriorityQueue<MapVertex> pq = new PriorityQueue<MapVertex>(MapVertex.AStarComparator);
    Set<MapVertex> visited = new HashSet<MapVertex>();
    Map<GeographicPoint, GeographicPoint> parentMap =
        new HashMap<GeographicPoint, GeographicPoint>();
    setDistancesFromStart(start);
    setDistancesToEnd(start, goal);
    pq.add(Vertices.get(start)); // put the start node into the PQ
    while (!pq.isEmpty()) {
      MapVertex curr = pq.poll();
      nodeSearched.accept(curr.getLocation()); // for visualization
      if (!visited.contains(curr)) {
        visited.add(curr);
        if (curr.getLocation().equals(goal)) return RetrievePath(parentMap, start, goal);
        ; // retrieve the shortest path
        Set<MapEdge> edges = curr.getEdges();
        for (MapEdge edge : edges) {
          MapVertex next = edge.getEnd();
          if (!visited.contains(next)) {
            double currLenFromStart = curr.getDistFromStart() + edge.getRoadLength();
            if (currLenFromStart < next.getDistFromStart()) {
              next.setDistFromStart(currLenFromStart); // update shortest distance to the start
              parentMap.put(
                  next.getLocation(), curr.getLocation()); // update the parent-child relation
              pq.add(next);
            }
          }
        }
      }
    }

    return null;
  }
Esempio n. 4
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;
  }
Esempio n. 5
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;
  }
Esempio n. 6
0
  /**
   * Find the path from start to goal using Dijkstra's algorithm
   *
   * @param start The starting location
   * @param goal The goal location
   * @param nodeSearched A hook for visualization. See assignment instructions for how to use it.
   * @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) {

    PriorityQueue<MapNode> pq = new PriorityQueue<MapNode>(); // Initialize priority queue
    HashSet<MapNode> visited = new HashSet<MapNode>();
    HashMap<MapNode, MapNode> parent = new HashMap<MapNode, MapNode>(); // Initialize parent HashMap

    // Initialize values to infinity
    MapNode startNode = vertices.get(start);
    initializeNodes(startNode, vertices);

    // Enqueue start node
    pq.add(startNode);

    int dijkstraCount = 0;
    // while queue is not empty
    while (pq.isEmpty() == false) {

      // dequeue current node from the front of the queue
      MapNode dqNode = pq.remove();
      dijkstraCount++;

      // Hook for visualization.
      nodeSearched.accept(dqNode.getCoordinates());

      // if curr is not visited
      if (visited.contains(dqNode) == false) {

        // add curr to the visited set
        visited.add(dqNode);

        // if the current node is equal to the goal node...
        if (vertices.get(dqNode.getCoordinates()) == vertices.get(goal)) {
          System.out.println("Dijkstra count: " + dijkstraCount);
          List<GeographicPoint> listCoordinates = new ArrayList<GeographicPoint>();

          // Build a list using the path found from the start node to the goal node.
          listCoordinates = buildPath(dqNode, parent);

          // Print out the visited nodes
          printPathOfCoordinates(listCoordinates);
          return listCoordinates;
        }

        // for each of the current node's neighbors not in the visited set
        for (MapEdge e : dqNode.getEdges()) {
          MapNode n = vertices.get(e.end);

          if (visited.contains(n) == false) {
            // visited.add(vertices.get(e.end));
            // 1. if path through curr to n is shorter
            double newWeight = dqNode.getWeight() + e.getDistance();
            if (newWeight < n.getWeight()) {
              // 2. update n's distances
              n.setWeight(newWeight);
              // 3. update curr as n's parent in parent map
              parent.put(n, dqNode);
              // 4. enqueue (n, distance) into the PQ
              pq.add(n);
            }
          }
        }
      } // if
    } // while

    return null;
  }