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.
