# Java A* Implementation

There are a thousand implementations of A* in Java. This one is mine.

EDIT: If you’re drawing your maps left to right, top to bottom, you’re best off storing your maps as [y][x] to avoid cache misses and to improve data locality. The code below assumes [x][y] because I wrote it for someone else.

``` import java.awt.Point; import java.util.ArrayList; public class AStar { public static Point[] findPath(double[][] map, Point start, Point finish) { // These are for running A*. Point current = null; ArrayList candidates = new ArrayList(); Point[][] parent = new Point[map.length][map[0].length]; double[][] cost = new double[map.length][map[0].length]; // All costs start at infinity, but Java inits them to zero. double[][] costToGoal = new double[map.length][map[0].length]; // Set initial costs to infinity. for(int i=0; i < map.length; i++) { for(int j=0; j < map[i].length; j++) { cost[i][j] = Float.POSITIVE_INFINITY; costToGoal[i][j] = Float.POSITIVE_INFINITY; } } // We make start its own parent so that we know it's visited. // This may have problems when reversing, but we can safely do so because we have the start point as an argument. parent[start.x][start.y] = start; cost[start.x][start.y] = 0; costToGoal[start.x][start.y] = heuristic(map, start.x, start.y, finish.x, finish.y); candidates.add(start); while(!candidates.isEmpty()) { // Do an O(n) search for the smallest candidate. In a densely connected graph, a minheap may drive the cost to O(n^2 log n). // Since this isn't really densely connected, we could switch for a performance boost. Point minCostCandidate = null; double minCost = Float.POSITIVE_INFINITY; for(Point p : candidates) { if(cost[p.x][p.y] + costToGoal[p.x][p.y] < minCost) { minCostCandidate = p; minCost = cost[p.x][p.y] + costToGoal[p.x][p.y]; } } candidates.remove(minCostCandidate); current = minCostCandidate; if(current.equals(finish)) { break; } // We've got the shortest path so far. Now let's figure out the cost of the eight items around this candidate. // If a neighbor has a _higher_ cost, make this node the parent and decrease the cost. for(int dy=-1; dy<2; dy++) { for(int dx=-1; dx<2; dx++) { // Short-hand for our neighbor's location. int nbrx = current.x+dx; int nbry = current.y+dy; // Make sure it's a valid spot on the map. if(dx==0 && dy==0) { continue; } // Skip the current point. Can't be your own parent. if(!isInsideMap(map, nbrx, nbry)) { continue; } // Don't plot outside the map. // Distance from the start to here plus the distance from here to the neighbor. double distToNeighbor = cost[current.x][current.y] + distanceSquared(map, current.x, current.y, nbrx, nbry); // If we have a faster way to get here, update it! if(distToNeighbor < cost[nbrx][nbry]) { cost[nbrx][nbry] = distToNeighbor; costToGoal[nbrx][nbry] = heuristic(map, nbrx, nbry, finish.x, finish.y); parent[nbrx][nbry] = current; candidates.add(new Point(nbrx, nbry)); } } } } if(!current.equals(finish)) { return null; // No path to goal. } ArrayList prepath = new ArrayList(); while(!current.equals(start)) { prepath.add(current); current = parent[current.x][current.y]; } Point[] path = new Point[prepath.size()]; // Reverse the path before returning it. for(int i=0; i < path.length; i++) { path[i] = prepath.get(prepath.size()-1-i); } return path; } public static boolean isInsideMap(double[][] map, int x, int y) { return x >= 0 && x < map.length && y >= 0 && y < map[0].length; } public static double heuristic(double[][] map, int ax, int ay, int goalx, int goaly) { double dx = ax - goalx; double dy = ay - goaly; double dz = map[ax][ay] - map[goalx][goaly]; return dx*dx + dy*dy + dz*dz; } public static double distanceSquared(double[][] map, int ax, int ay, int bx, int by) { final double UPHILL_COST_SCALAR = 1.0; // Going up hill costs 3x more than going down hill. final double DOWNHILL_COST_SCALAR = 1.0; // Going down hill costs just as much as going flat. double deltaDistance = (ax-bx)*(ax-bx) + (ay-by)*(ay-by); double deltaHeight = map[ax][ay] - map[bx][by]; if(deltaHeight < 0) { deltaHeight = Math.max(0, deltaHeight*DOWNHILL_COST_SCALAR); // We are assuming that moving downhill has a non-negative cost. } else if(deltaHeight > 0) { deltaHeight *= UPHILL_COST_SCALAR; } else { // No change. } return deltaDistance + deltaHeight*deltaHeight; } } ```

And some helpful main to produce pretty outputs:

``` public static void main(String[] args) { final int XSIZE = 40; final int YSIZE = 40; // Run this to test. double[][] map = new double[XSIZE][YSIZE]; Random rand = new Random(); for(int i=0; i < XSIZE; i++) { for(int j=0; j < YSIZE; j++) { map[i][j] = rand.nextInt(2); } } Point start = new Point(rand.nextInt(XSIZE), rand.nextInt(YSIZE)); Point end = new Point(rand.nextInt(XSIZE), rand.nextInt(YSIZE)); Point[] path = findPath(map, start, end); // Draw the map char[][] output = new char[XSIZE][YSIZE]; for(int i=0; i < XSIZE; i++) { for(int j=0; j < YSIZE; j++) { if(map[i][j] != 0) { output[i][j] = '#'; } else { output[i][j] = ' '; } } } // Draw the path on the map. if(path != null) { for(Point p : path) { output[p.x][p.y] = '.'; } } // Draw the start and end. output[start.x][start.y] = 'S'; output[end.x][end.y] = 'E'; // Print everything. for(int j=0; j < YSIZE; j++) { for(int i=0; i < XSIZE; i++) { System.out.print(output[i][j]); } System.out.println(); } } ```

And some selected sample output.

```     # #  #### # ### # #     ##   #   #
### #E.### ##  ##  ##     #  #######
# # # # #.... ## #   # ##  ## #   # #
##### #  # ###.##    # ## ## ### # ##  #
## ## #### ## #.# # ## ## #  # ######  #
##   ### # ## ##.# # ## ###    #   #  #
#### #  ### #   #.# ###### ## # ###### #
#  ##    ##  ### .########## #   ## #
## # #  ##     . # ##### # # # #####
###   #  ###   ##  .   #     ##    ####
#      ##  #  #### # .##  ### ##  #####
##   ##    # #  # ####S # # #    # ### #
```

```#   #  #  #   # #  ## # ## # #######  ##
# ##### #  #   #   #  #. ##    ###..# #
## ##  ## ##........... .......... #E# #
######   #.  ##   # ## #  ##    ## # ##
#   ## ##.## ##   # # #  ###  ####   ##
## #    #. #      #  ## # #      ###  #
#### ##.###  #####    ### # #### # ###
#  # ##.   # ####  #   ###  # #   ##
#### S# #    ####     #  ### #  ##  ###
##  #  ##  # #   #  ## #  ##### ## ###
#  ## ## #  # #     # ##    ## ### # ##
```