dijkstra.java

import java.awt.Point;
import java.util.ArrayList;
import java.util.List;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

/// A sample AI that takes a very suboptimal path.
/**
 * This is a sample AI that moves as far horizontally as necessary to reach the target,
 * then as far vertically as necessary to reach the target.  It is intended primarily as
 * a demonstration of the various pieces of the program.
 * 
 */
public class Dijkstra implements AIModule{
    /// Creates the path to the goal.
    public List<Point> createPath(final TerrainMap map){
        // Holds the resulting path
        final ArrayList<Point> path = new ArrayList<Point>();
        final Point start = map.getStartPoint();
        final Point goal = map.getEndPoint();

        // Keep track of where we are and add the start point.
        final Point CurrentPoint = map.getStartPoint();
        path.add(new Point(CurrentPoint));

        final int size = map.getWidth()*map.getHeight(); // used to size data structures appropriately
        final Set<Point> closedSet = new HashSet<Point>(size); // The set of nodes already evaluated.
        final List<Point> openSet = new ArrayList<Point>(size); // The set of tentative nodes to be evaluated, initially containing the start node
        openSet.add(start);
        final Map<Point, Point> cameFrom = new HashMap<Point, Point>(size); // The map of navigated nodes.

        final Map<Point,Double> gScore = new HashMap<Point, Double>(); // Cost from start along best known path.
        gScore.put(start, 0.0);
        // System.out.println(gScore);
        // System.out.println(start);
        // System.out.println(map.getEndPoint());

        final Map<Point,Double> fScore = new HashMap<Point, Double>();
        fScore.put(start, getHeuristic(start,goal));

        final Comparator<Point> comparator = new Comparator<Point>() {
          /**
           * {@inheritDoc}
           */
          @Override
          public int compare(Point o1, Point o2) {
              if (fScore.get(o1) < fScore.get(o2))
                  return -1;
              if (fScore.get(o2) < fScore.get(o1))
                  return 1;
              return 0;
          }
        };

        System.out.println(map.getTile(goal));

        while (!openSet.isEmpty()) {
            // System.out.println("here");
            final Point current = openSet.get(0);
            if (current.equals(goal))
                return reconstructPath(cameFrom, goal);

            openSet.remove(0);
            closedSet.add(current);
            for (Point neighbor : map.getNeighbors(current)) {
                if (closedSet.contains(neighbor))
                    continue; // Ignore the neighbor which is already evaluated.

                final double tenativeGScore = gScore.get(current) + map.getCost(current, neighbor); // length of this path.
                if (!openSet.contains(neighbor))
                    openSet.add(neighbor); // Discover a new node
                else if (tenativeGScore >= gScore.get(neighbor))
                    continue;

                // This path is the best until now. Record it!
                cameFrom.put(neighbor, current);
                gScore.put(neighbor, tenativeGScore);
                final double estimatedFScore = gScore.get(neighbor) + getHeuristic(neighbor, goal);
                fScore.put(neighbor, estimatedFScore);

                // fScore has changed, re-sort the list
                Collections.sort(openSet,comparator);
            }
        }

        // We're done!  Hand it back.
        return null;
    }

    private double euclDist(double x1, double y1, double x2, double y2) {       
        return Math.sqrt((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1));
    }

    protected double getHeuristic(Point current, Point goal) {
      // return euclDist(current.x, current.y, goal.x, goal.y);
      // return Math.max( Math.abs(current.x-goal.x), Math.abs(current.y-goal.y) );
      return 0;
    }

    private List<Point> reconstructPath(Map<Point, Point> cameFrom, Point current) {
      final List<Point> totalPath = new ArrayList<Point>();
      totalPath.add(current);
      while (current != null) {
          final Point previous = current;
          current = cameFrom.get(current);
          if (current != null) {
              totalPath.add(current);
          }
      }
      Collections.reverse(totalPath);
      // System.out.println(totalPath);
      return totalPath;
    }
}