Efficient path planning

Download this little Java game. Unzip it. Build it. Run it. You will see a pixelated terrain map and a small robot. When you click the mouse, the robot moves in a straight line to the spot where you clicked.

Implement Uniform-cost search (UCS). UCS is similar to breadth-first search, except it uses a priority queue. Breadth-first search only finds the optimal (lowest-cost) path between any two states if all actions have uniform cost. Uniform cost search, by contrast, can find the optimal (lowest-cost) path between any two states, even if the actions do not have uniform cost. States go into the priority queue in any order. The state that can be reached with the lowest cost always comes out first. You can find pseudo-code for uniform cost search on-line. Or, you can use this pseudo-code:

class MyState {
  public double cost;
  MyState parent;
  State state;

  MyState(double cost, MyState par) {
  }
}

class MyPlanner {
  method uniform_cost_search(MyState startState, MyState goalState) {
    PriorityQueue frontier = new PriorityQueue();
    Set beenthere = new Set();
    startState.cost = 0.0;
    startState.parent = null;
    beenthere.add(startState);
    frontier.add(startState);
    while(frontier.size() > 0) {
      MyState s = frontier.pop_first(); // get lowest-cost state
      if(s.state.isEqual(goalState.state))
        return s;
      for each action, a {
        child = transition(s, a); // compute the next state
        acost = action_cost(s, a); // compute the cost of the action
        if(child is in beenthere) {
          oldchild = beenthere.find(child)
          if(s.cost + acost < oldchild.cost) {
            oldchild.cost = s.cost + acost;
            oldchild.parent = s;
          }
        }
        else {
          child.cost = s.cost + acost;
          child.parent = s;
          frontier.add(child);
          beenthere.add(child);
        }
      }
    }
    throw new RuntimeException("There is no path to the goal");
  }
}

Remember to always keep your general-purpose code separated from your problem-specific code.

Make the robot follow an efficient path to arrive at the place where the user clicks. In Agent.update, if the agent has not yet reached the place where the user clicked, use UCS to plan a path from the agent's current location to the place where the user clicked. Then, call Model.setDestination to make the agent move toward the destination of the first or second step (not the last step) in your plan.

(Analogy: In dog racing, a mechanical rabbit is driven along a track just a little bit ahead of the leading dog. This keeps the dogs racing forward, while staying on the track. If the rabbit gets too far ahead, the dogs might start cutting corners. Wouldn't it be more efficient to cache the plan? Yes, you can do that if you want, but it should not be necessary since this map is so small.)

To make UCS work with this problem, we will need to discretize the possible actions. Let's say you can step toward any of the 8 adjacent squares. Each square is 10-by-10 pixels in size, so your possible immediate destinations are:
```
(x+10, y-10)
(x+10, y)
(x+10, y+10)
(x, y+10)
(x, y-10)
(x-10, y-10)
(x-10, y)
(x-10, y+10)
```
The cost of each action is determined by the speed associated with the terrain square upon which you are standing and the distance you will travel at that speed. (Note that you have to travel farther to move diagonally.)

Put all your code in Agent.java. (You should not need to edit any of the other files, but you will need to call Model.getX, Model.getY, and Model.getTravelSpeed.)

Draw the planned path and the frontier. Use red lines to show the path the agent is following. Also, draw something (maybe little circles) at all of the points that are in the frontier when UCS finds the goal.

Implement A* search. A* search is a generalization of UCS. Instead of using a priority queue that sorts by cost, it uses one that sorts by cost+heuristic. The heuristic is an underestimate of the remaining cost to reach the goal. When the user left-clicks, use UCS. When the user right-clicks, use A*.

For the A* heuristic, use the shortest possible distance (a straight line) times the lowest possible cost (the cost of the fastest square anywhere on the map). (Also note that cost is the reciprocal of speed.) Note that the map does not change, so you should not have to scan the entire map to find the lowest cost square more than once, ever.

This heuristic is fast to compute, but also provides enough enformation to speed up the search. There are faster ways to underestimate the remaining cost. For example, 0 is always less than or equal to the remaining cost. However, a constant heuristic of zero would do nothing to make A* search any faster. (In fact, it would be equivalent to UCS.) A good heuristic should try to get as close as possible to the remaining cost. You could use UCS to exactly compute the remaining cost, but then you would have spent so much time computing the heuristic that there is no point to it. So, a good heuristic must strike a balance between speed and utility.

zip up your source code. Example:
```
zip -r -9 submission1.zip src
```
Make sure your archive contains a script named build.bash (or build.bat, if you use Windows) that builds your project. Include everything needed to make it work, but do not include any .class files. Click on the "Project submission" link on the main class page to submit your archive file. If you have any difficulties with the submission server, please e-mail your submission directly to the grader.

FAQ:

Q: What is the relationship between speed and cost?
A: If we want to minimize travel time, then travel time is the cost. If you double your speed, the travel time will be cut in half. So cost is the reciprocal of speed.

Q: How much farther is a diagonal move than a forward or lateral move?
A: sqrt(2). Think about the Pythagorean theorem.

Q: How do I keep my general purpose code separated from my problem specific code?
A: In this case, the problem-specific code contains your representation of state and the ability to iterate over actions. The general-purpose code is your implementation of UCS or A* search. If you could plug in a new problem without having to change any code in the region of your search algorithm, you have done well.

Q: There is no "HashSet.find" method in Java. What do I do?
A: This is a deficiency of the Java HashSet class. (There is no good reason why this method does not exist.) Use HashMap instead.

Q: There is no "TreeSet.find" method in Java. What do I do?
A: First call "TreeSet.contains" to see if the object you want is there. Then, call "TreeSet.floor" to get it.

Q: When I use a non-zero heuristic, A* behaves inconsistently. What's going on?
A: Make sure you are keeping the cost-so-far separate from the estimated-remaining-cost. If you mix them, then it is tricky to produce the child states without compounding the heuristic.

Q: Can the queue and set use the same comparator?
A: No. The queue sorts by cost. The set is supposed to determine whether you have been in a particular state before. Finding a state with the same cost is not the same as finding the same state.

Q: Searching seems to slow the agent down. Is this expected?
A: No. It should be very fast and responsive. There are only 120x60 unique states, so a modern computer could visit them all within a negligible amount of time. Usually, this problem is caused by doing exact comparisons in the comparator that you use with your set. (It is almost never a good idea to expect floating point values to be exactly equivalent. For the purposes of searching in this game, two states should be considered equivalent if they fall in the same 10x10 region.)

Q: When I draw the frontier, it looks blinky. How do I make it more persistent?
A: Make sure you are drawing the last completed queue, not one that is in the process of being used.

Q: My path wiggles while the robot follows it. Is that a problem?
A: No. If it bothers you, you can make it stable by discretizing the start and goal states. (Divide by 10, truncate by casting to an int, then multiply by 10.)

Q: I am not familiar with graphics commands in Java. What will I need?
A: Everything you will need is in the Graphics class. You will probably need to call "setColor", "drawLine", and "fillOval".

Q: What causes "Exception in thread "main" javax.imageio.IIOException: Can't read input file!"?
A: You are probably trying to run from a different folder. Many IDEs do this by default. One solution is to execute from the command line. (This is how the grader will do it, so it is a good idea to test in that mode.) Another solution is to change the working directory in your IDE to be the one where you unzipped this source code.