## Sunday, February 8, 2015

### A* Shortest Path Finding Algorithm Implementation in Java

Pseudo code:

OPEN //the set of nodes to be evaluated
CLOSED //the set of nodes already evaluated
add the start node to OPEN

loop
current = node in OPEN with the lowest f_cost
remove current from OPEN

if current is the target node //path has been found
return

foreach neighbour of the current node
if neighbour is not traversable or neighbour is in CLOSED

if new path to neighbour is shorter OR neighbour is not in OPEN
set f_cost of neighbour
set parent of neighbour to current
if neighbour is not in OPEN

Source:

```import java.util.*;

public class AStar {
public static final int DIAGONAL_COST = 14;
public static final int V_H_COST = 10;

static class Cell{
int heuristicCost = 0; //Heuristic cost
int finalCost = 0; //G+H
int i, j;
Cell parent;

Cell(int i, int j){
this.i = i;
this.j = j;
}

@Override
public String toString(){
return "["+this.i+", "+this.j+"]";
}
}

//Blocked cells are just null Cell values in grid
static Cell [][] grid = new Cell[5][5];

static PriorityQueue<Cell> open;

static boolean closed[][];
static int startI, startJ;
static int endI, endJ;

public static void setBlocked(int i, int j){
grid[i][j] = null;
}

public static void setStartCell(int i, int j){
startI = i;
startJ = j;
}

public static void setEndCell(int i, int j){
endI = i;
endJ = j;
}

static void checkAndUpdateCost(Cell current, Cell t, int cost){
if(t == null || closed[t.i][t.j])return;
int t_final_cost = t.heuristicCost+cost;

boolean inOpen = open.contains(t);
if(!inOpen || t_final_cost<t.finalCost){
t.finalCost = t_final_cost;
t.parent = current;
}
}

public static void AStar(){

//add the start location to open list.

Cell current;

while(true){
current = open.poll();
if(current==null)break;
closed[current.i][current.j]=true;

if(current.equals(grid[endI][endJ])){
return;
}

Cell t;
if(current.i-1>=0){
t = grid[current.i-1][current.j];
checkAndUpdateCost(current, t, current.finalCost+V_H_COST);

if(current.j-1>=0){
t = grid[current.i-1][current.j-1];
checkAndUpdateCost(current, t, current.finalCost+DIAGONAL_COST);
}

if(current.j+1<grid[0].length){
t = grid[current.i-1][current.j+1];
checkAndUpdateCost(current, t, current.finalCost+DIAGONAL_COST);
}
}

if(current.j-1>=0){
t = grid[current.i][current.j-1];
checkAndUpdateCost(current, t, current.finalCost+V_H_COST);
}

if(current.j+1<grid[0].length){
t = grid[current.i][current.j+1];
checkAndUpdateCost(current, t, current.finalCost+V_H_COST);
}

if(current.i+1<grid.length){
t = grid[current.i+1][current.j];
checkAndUpdateCost(current, t, current.finalCost+V_H_COST);

if(current.j-1>=0){
t = grid[current.i+1][current.j-1];
checkAndUpdateCost(current, t, current.finalCost+DIAGONAL_COST);
}

if(current.j+1<grid[0].length){
t = grid[current.i+1][current.j+1];
checkAndUpdateCost(current, t, current.finalCost+DIAGONAL_COST);
}
}
}
}

/*
Params :
tCase = test case No.
x, y = Board's dimensions
si, sj = start location's x and y coordinates
ei, ej = end location's x and y coordinates
int[][] blocked = array containing inaccessible cell coordinates
*/
public static void test(int tCase, int x, int y, int si, int sj, int ei, int ej, int[][] blocked){
System.out.println("\n\nTest Case #"+tCase);
//Reset
grid = new Cell[x][y];
closed = new boolean[x][y];
open = new PriorityQueue<>((Object o1, Object o2) -> {
Cell c1 = (Cell)o1;
Cell c2 = (Cell)o2;

return c1.finalCost<c2.finalCost?-1:
c1.finalCost>c2.finalCost?1:0;
});
//Set start position
setStartCell(si, sj);  //Setting to 0,0 by default. Will be useful for the UI part

//Set End Location
setEndCell(ei, ej);

for(int i=0;i<x;++i){
for(int j=0;j<y;++j){
grid[i][j] = new Cell(i, j);
grid[i][j].heuristicCost = Math.abs(i-endI)+Math.abs(j-endJ);
//                  System.out.print(grid[i][j].heuristicCost+" ");
}
//              System.out.println();
}
grid[si][sj].finalCost = 0;

/*
Set blocked cells. Simply set the cell values to null
for blocked cells.
*/
for(int i=0;i<blocked.length;++i){
setBlocked(blocked[i][0], blocked[i][1]);
}

//Display initial map
System.out.println("Grid: ");
for(int i=0;i<x;++i){
for(int j=0;j<y;++j){
if(i==si&&j==sj)System.out.print("SO  "); //Source
else if(i==ei && j==ej)System.out.print("DE  ");  //Destination
else if(grid[i][j]!=null)System.out.printf("%-3d ", 0);
else System.out.print("BL  ");
}
System.out.println();
}
System.out.println();

AStar();
System.out.println("\nScores for cells: ");
for(int i=0;i<x;++i){
for(int j=0;j<x;++j){
if(grid[i][j]!=null)System.out.printf("%-3d ", grid[i][j].finalCost);
else System.out.print("BL  ");
}
System.out.println();
}
System.out.println();

if(closed[endI][endJ]){
//Trace back the path
System.out.println("Path: ");
Cell current = grid[endI][endJ];
System.out.print(current);
while(current.parent!=null){
System.out.print(" -> "+current.parent);
current = current.parent;
}
System.out.println();
}else System.out.println("No possible path");
}

public static void main(String[] args) throws Exception{
test(1, 5, 5, 0, 0, 3, 2, new int[][]{{0,4},{2,2},{3,1},{3,3}});
test(2, 5, 5, 0, 0, 4, 4, new int[][]{{0,4},{2,2},{3,1},{3,3}});
test(3, 7, 7, 2, 1, 5, 4, new int[][]{{4,1},{4,3},{5,3},{2,3}});

test(1, 5, 5, 0, 0, 4, 4, new int[][]{{3,4},{3,3},{4,3}});
}
}```

Output:

Test Case #1
Grid:
SO  0   0   0   BL
0   0   0   0   0
0   0   BL  0   0
0   BL  DE  BL  0
0   0   0   0   0

Scores for cells:
0   14  27  41  BL
14  17  29  42  56
27  29  BL  45  59
39  BL  43  BL  0
52  55  0   0   0

Path:
[3, 2] -> [2, 1] -> [1, 1] -> [0, 0]

Test Case #2
Grid:
SO  0   0   0   BL
0   0   0   0   0
0   0   BL  0   0
0   BL  0   BL  0
0   0   0   0   DE

Scores for cells:
0   17  33  48  BL
17  20  35  49  62
33  35  BL  52  64
48  BL  52  BL  67
62  65  64  67  77

Path:
[4, 4] -> [3, 4] -> [2, 3] -> [1, 2] -> [1, 1] -> [0, 0]

Test Case #3
Grid:
0   0   0   0   0   0   0
0   0   0   0   0   0   0
0   SO  0   BL  0   0   0
0   0   0   0   0   0   0
0   BL  0   BL  0   0   0
0   0   0   BL  DE  0   0
0   0   0   0   0   0   0

Scores for cells:
40  35  37  40  53  68  0
22  17  20  34  48  63  0
17  0   15  BL  48  61  75
20  15  18  31  43  56  70
34  BL  31  BL  46  58  73
48  48  43  BL  56  61  0
63  61  56  59  0   0   0

Path:
[5, 4] -> [4, 4] -> [3, 3] -> [3, 2] -> [2, 1]

Test Case #1
Grid:
SO  0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   BL  BL
0   0   0   BL  DE

Scores for cells:
0   17  33  48  62
17  20  35  49  62
33  35  38  51  63
48  49  51  BL  BL
62  62  63  BL  0

No possible path

Add a comment if you find any error :)

1. Im getting 64 errors....I saved it as AStar.java.....Any help would be highly appreciated

1. What type of errors?

2. open = new PriorityQueue<>((o1, o2) -> {
Cell c1 = (Cell)o1;
Cell c2 = (Cell)o2;

return c1.finalCostc2.finalCost?1:0;
});

that cause the error.

3. What JDK version are you using?

4. This code use lambdas (introduced in Java 8), you can't use this on Java 7 or earlier, try:

open = new PriorityQueue(16, new Comparator() {
@Override
public int compare(Cell c1, Cell c2) {
return c1.finalCost < c2.finalCost ? -1:
c1.finalCost > c2.finalCost ? 1 : 0;
}
});

2. Thank you for this code, it works very well. I just found a minor bug when printing scores:

for(int i=0;i<x;++i){
for(int j=0;j<x;++j){

3. Hey,
Would you kindly explain what the following code exactly does?
Thank you.

"return c1.finalCostc2.finalCost?1:0;"

4. c1.finalCost<c2.finalCost?-1:c1.finalCost>c2.finalCost?1:0;

Check if c1's final Cost is less than c2's. If so, return -1.
else check if it is greater than c2's. If so, return 1.
else (both are equal) return 0.

5. Yeah,i meant to ask what its use is for your code.
I am new to the whole PriorityQueue thing and i am trying to understand what the following actually does.
"((Object o1, Object o2) -> {
Cell c1 = (Cell)o1;
Cell c2 = (Cell)o2;

return c1.finalCostc2.finalCost?1:0;
});"

1. public PriorityQueue(Comparator comparator)

Creates a PriorityQueue with the default initial capacity and whose elements are ordered according to the specified comparator.

Parameters:
comparator - the comparator that will be used to order this priority queue. If null, the natural ordering of the elements will be used.

Since:
1.8

You can write this code without using the lambda expression :

PriorityQueue pq = new PriorityQueue<>(new Comparator() {

@Override
public int compare(Object o1, Object o2) {
//Do stuff
return 0;
}
});

It tells the priority queue to use your own specified comparator to help order the elements according to their priority.

6. Got it now.

7. thank youuu..

8. what exactly is the use of diagonal cost and V_H cost? why are the value set as 14 and 10 respectively?

9. open = new PriorityQueue<>(Object o1, Object o2) -> {
Cell c1 = (Cell)o1;
Cell c2 = (Cell)o2;

return c1.finalCostc2.finalCost?1:0;
};
we are getting five errors in this part of the code. please help soon to rectify them. it is urgent.

1. Update JDK version if you're using an older one.