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.