Sunday, February 8, 2015

A* Shortest Path Finding Algorithm Implementation in Java

A nice Tutorial : https://www.youtube.com/watch?v=-L-WgKMFuhE

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
    add current to CLOSED

    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
            skip to the next neighbour

        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
                add neighbour to 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;
            if(!inOpen)open.add(t);
        }
    }
    
    public static void AStar(){ 
        
        //add the start location to open list.
        open.add(grid[startI][startJ]);
        
        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 :)

14 comments:

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

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

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

      that cause the error.

      Delete
    2. What JDK version are you using?

      Delete
    3. 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;
      }
      });

      Delete
  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){

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

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

    ReplyDelete
  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.

    ReplyDelete
  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;
    });"

    ReplyDelete
    Replies
    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.

      Delete
  6. Got it now.
    Thank you for your time!

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

    ReplyDelete