Shortest path navigation across a grid using Best First Search

Question

The code below is an implementation of the Best First Search algorithm for navigation of the shortest path across a 2D NxN matrix. As a heuristic for the search I use the standard distance formula. The matrix is generated from a text file passed in as a command line argument,where the first line is the size N of and each subsequent line represents a row. For example,

5
.+..g
.....
.....
.+...
i+...

The walls are represented as + and both i and g represent the start and goal positions respectively. Navigation can only be done in four directions, up, down, left, and right. I first parse the file and create a matrix of Node objects

import java.util.*;

public class Node {

    double cost;
    int type;
    int x;
    int y;
    ArrayList<Node> neighbors = new ArrayList<>();
    Node parent = null;
    boolean inPath = false;


    public Node(int x, int y, int type) {
        this.x = x;
        this.y = y;
        this.type = type;
    }

    public int getX() { return this.x; }

    public void setX(int n) { this.x = n; }

    public int getY() { return this.y; }

    public void setY(int n) { this.y = n; }

    public double getCost() { return this.cost; }

    public void setCost(double n) { this.cost= n; }

    public void setType(int n) { this.type = n; }

    public int getType() { return this.type; }

    public void addNeighbor(Node n) {
        if(n.getType() != 3) {//check for not adding walls as neighbor=
            neighbors.add(n);
        }
    }
    public boolean isEqual(Node n2)
    {
        if(this.type == n2.getType()) { return true; }

        else return false;
    }
    public ArrayList<Node> getNeighbors() { return neighbors; }

    public Node getParent(){ return parent; }

    public void setParent(Node n) { parent = n; }

    public boolean getPath(){ return inPath; }
    public void setPath(){ inPath = true; }

}

to represent the + walls, i the initial position, g the goal position, and . the open spots.

import java.io.*;

public class fileParser {

    File inFile = null;
    int gridSize;
    int lineCount;
    Node[][] grid;
    Node initial;
    Node goal;

    public fileParser(String[] input) {
        if (0 < input.length) {
            inFile = new File(input[0]);
        }
        else {
            System.exit(1);
        }
    }

    public void parse() throws IOException {

        BufferedReader br = new BufferedReader(new FileReader(inFile));
        String line;
        gridSize = Integer.parseInt(br.readLine());
        grid = new Node[gridSize][gridSize];

        /*Create integer matrix for the given input. Nodes are given integer values corresponding to types*/
        while ((line = br.readLine()) != null) {
            for(int i = 0; i < line.length(); i++) {
                if(line.charAt(i) == '.') {
                    grid[lineCount][i] = new Node(lineCount, i, 0);//open
                }
                else if(line.charAt(i) == '+') {
                    grid[lineCount][i] = new Node(lineCount, i, 3);//wall
                }
                else if(line.charAt(i) == 'i') {
                    Node temp = new Node(lineCount, i, 1);//initial
                    grid[lineCount][i] = temp;
                    initial = temp;
                }
                else if(line.charAt(i) == 'g') {
                    Node temp = new Node(lineCount, i, 2);//goal
                    grid[lineCount][i] = temp;
                    goal = temp;
                }
            }
            lineCount++;
        }
        br.close();

        for(int i = 0; i < gridSize; i++) {
            for (int j = 0; j < gridSize; j++) {
                buildNeighbors(grid[i][j], i, j);
            }
        }
    }

    /*For each node that is not a wall represented as a 3, the corresponding up, down, left, and right neighbors will be
    added to a list*/
    public void buildNeighbors(Node n, int row, int col) {
        if(n.getType() != 3) {
            if(row == 0) {//Check for edge cases where neighbor amount will vary
                if(col == 0) {
                    n.addNeighbor(grid[row + 1][col]);
                    n.addNeighbor(grid[row][col + 1]);
                }
                else if(col == gridSize - 1){
                    n.addNeighbor(grid[row + 1][col]);
                    n.addNeighbor(grid[row][col - 1]);
                }
                else{
                    n.addNeighbor(grid[row + 1][col]);
                    n.addNeighbor(grid[row][col + 1]);
                    n.addNeighbor(grid[row][col - 1]);
                }
            }
            else if(row == gridSize - 1) {
                if(col == gridSize - 1){
                    n.addNeighbor(grid[row - 1][col]);
                    n.addNeighbor(grid[row][col - 1]);
                }
                else if(col == 0){
                    n.addNeighbor(grid[row - 1][col]);
                    n.addNeighbor(grid[row][col + 1]);
                }
                else{
                    n.addNeighbor(grid[row - 1][col]);
                    n.addNeighbor(grid[row][col - 1]);
                    n.addNeighbor(grid[row][col + 1]);
                }
            }
            else if(col == 0) {
                n.addNeighbor(grid[row + 1][col]);
                n.addNeighbor(grid[row - 1][col]);
                n.addNeighbor(grid[row][col + 1]);
            }
            else if(col == gridSize - 1) {
                n.addNeighbor(grid[row + 1][col]);
                n.addNeighbor(grid[row - 1][col]);
                n.addNeighbor(grid[row][col - 1]);
            }
            else{
                n.addNeighbor(grid[row + 1][col]);
                n.addNeighbor(grid[row - 1][col]);
                n.addNeighbor(grid[row][col - 1]);
                n.addNeighbor(grid[row][col + 1]);
            }
        }
    }

    public Node getInitial() { return initial; }

    public Node getGoal(){ return goal; }

    public Node[][] getGrid(){ return grid; }
}

Once I have the matrix properly parsed, I pass in the Node initial, Node goal, and Node[][] grid to the actual Strategy class to implement the search.

import java.util.ArrayList;
import java.util.PriorityQueue;

public class Strategy {

    Node initial;
    Node goal;
    Node[][] grid;
    ArrayList<Node> closed = new ArrayList<>();
    nodeComparator nc = new nodeComparator();
    PriorityQueue<Node> open = new PriorityQueue<>(nc);
    boolean pathFound = false;

    public Strategy(Node initial, Node goal, Node[][] grid) {
        this.initial = initial;
        this.goal = goal;
        this.grid = grid;
        open.add(initial);
    }

    public void evaluate(Node current){
        current.setCost(Math.sqrt(Math.pow((current.getX() - goal.getX()),2) + Math.pow((current.getY() - goal.getY()),2)));
    }

    public void getSuccessors(Node n) {
        for (Node neighbor : n.getNeighbors()) {//evaluate cost of all neighbors, set their parent, and add them to the open list
            if(!open.contains(neighbor) && !closed.contains(neighbor)) {
                evaluate(neighbor);
                open.add(neighbor);
                neighbor.setParent(n);
            }
        }
    }

    public void getPath(Node n) {
        Node current = n;
        while(current.getType() != 1) {//backtrack through parents and use boolean marker to indicate path before reaching the initial node
            current.setPath();
            current = current.getParent();
        }
    }

    public void printGrid() {
        if(!pathFound) {
            System.out.println("No path found");
        }
        else {
            for (int i = 0; i < grid.length; i++) {
                for (int j = 0; j < grid[0].length; j++) {
                    if (grid[i][j].getType() == 0) {
                        if (grid[i][j].getPath()) {//boolean tracker of what nodes are in the path
                            System.out.print("o ");
                        } else {
                            System.out.print(". ");
                        }
                    } else if (grid[i][j].getType() == 1) {//initial
                        System.out.print("i ");
                    } else if (grid[i][j].getType() == 2) {//goal
                        System.out.print("g ");
                    } else {
                        System.out.print("+ ");
                    }
                }
                System.out.println();
            }
        }
        System.out.println();
    }

    public void search() {
        while(!open.isEmpty()) {
            Node current = open.poll();
            closed.add(current);
            if(goal.isEqual(current)) {
                pathFound = true;
                getPath(current);
            }
            else {
                getSuccessors(current);
            }
        }
    }
}

The search evaluates the cost of each node by a calculating the Euclidean distance of the current Node to the goal. Therefore an override was created for the PriorityQueue to properly sort the Node objects added.

import java.util.Comparator;

    public class nodeComparator implements Comparator<Node> {
        @Override
        public int compare(Node n1, Node n2)
        {
            return Double.compare(n1.getCost(), n2.getCost());
        }

    }

In order to run the search I pass in the command line argument into the fileParser object and subsequently call Strategy search and print the finalized grid with the path represented by the character o. For example, the output to the initial example listed is

. + . o g 
. + o o . 
o o o . . 
o + . . . 
i + . . .

The calls are made in the main as such,

import java.io.IOException;

public class Main {

    public static void main(String[] args) {

        fileParser fp = new fileParser(args);

        try {
            fp.parse();

            System.out.println("The shortest path to the goal is: ");
            System.out.println("");
            Strategy strat = new Strategy(fp.getInitial(), fp.getGoal(), fp.getGrid());
            strat.search();
            strat.printGrid();
        }
        catch(IOException ex) {
            System.err.println(ex);
        }
    }
}

I would appreciate feedback regarding the performance and efficiency of the algorithm and areas where I have not provided adequate or proper implementation of it.

Answer 1

High-level reusable or purpose-built?

You wrote Strategy like a high-level, all purpose Dijkstra Pathfinder (works on nodes, which can be arbitrarily far, but your grid only allows 4-connected neighbours), which even uses an external Comparator for increased versatility.

You wrote Node like generic graph representation (it has a List of neighbours etc. but your grid only allows 4).

However Strategy uses intimate knowledge of Node: it knows it's in a grid. But Strategy does not need the grid to solve the problem (it uses getNeighbours()), it only needs it for printing the map... Which a Strategy shouldn't know how to do!

In the other hand you allow arbitrary distance in evaluate() function, but it only ever return 1...

Right now it's all entangled.

You need to decide what you want:

• Make a more integrated and efficient solution which leverages the knowlegdge of the grid structure
• Make a more high-level solver which offloads intimate logic to its Node function. With the right hooks, this can stil be made quite efficient (leaving the door open for high-level optimization like switching to A-start without even altering the underlying application at all), but above all, reusable

---

Since this is Code Review, I can weigh in and say: you will want the latter, and split Strategy further away from Node class. I think you need a Node abstract class for the Strategy, and GridNode class for your grid application.

Tip: Use Java's package. Put Strategy in its own package like algo.pathfinding. Add an abstract Node class to that package, make it completely unaware of its grid structure. Then gather the grid-related stuff in its own separate package like game.grid. Using this, it'll be much easier to pull off a well-designed API.

---

Remove the printing from the Strategy

It has nothing to do there. Let Strategy return a List<Node> as path, then pass that path with the grid to a PathPrinter class.

---

Performance

• Drop the sqrt. If you override Node's distance function using GridNode, you can (and should) use manahttan. If you know the ceels are adjasent, return always 1.
• Use switch when checking cell contents: this is faster, and less error-prone.

---

Ease of use

• Store the grid as a 2d array of enums. USing enums is more powerful than String, char or ints, and the switch is more powerful (IDE auto-checks missing cases etc.)