I am trying to solve my A* Path finding algorithm. I have three classes: Menu, Grid, and Node.
If you run my program, you will see that it prints an unusual spiral, skippy, bunny hopping path. I believe the unexpected behaviour has something to do with the following function:
printAStarPath(int startx, int starty, int endx, int endy)
In my opinion, I think the problem has something to do with setting parent nodes improperly. I am pretty sure Node and Menu work properly.
The menu function basically manages the user input. The user can add walls, start location, and end location, and the size of the grid. I also included some tests in the menu (so you don't always have type everything again every time you test). The printAStarPath(...) function takes in a start x,y location and an end x,y location.
I want this to print a grid like so:
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
Unfortunately, I get this craziness:
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][*]
[ ][S][ ][X][ ][E][*]
[ ][ ][*][X][ ][ ][*]
[ ][ ][ ][*][*][*][ ]
Another example of what happens with different input:
[E][ ][ ][ ][ ]
[ ][*][ ][ ][*]
[ ][ ][X][ ][*]
[ ][ ][ ][ ][*]
[ ][*][ ][*][S]
Some of my grids look like arrows pointing down-right-ward. Some look like they go down-right then spiral up and to the left.
I am using the A* path finding method and I am calculating the Heuristic (or H-cost) by the Manhattan Method. I use recursion to get my exact G-cost as well as tracing back from the end location to the start location.
Here's the article that I followed.
Menu:
package pathFinding;
import java.util.*;
public class Menu {
private Grid board;
public Menu(){
board = new Grid(0, 0);
}//end constructor
static public void main (String[] args){
Menu pft = new Menu();
pft.boardMenu();
System.out.print("process terminated.");
}//end main
public void boardMenu(){
// very user error prone !
boolean kg = true;
Scanner in = new Scanner(System.in);
int input;
while(kg){
input = -1;
System.out.print("\n\n\n\n");
System.out.print(">>> Hi there,"
+ "\n(0) quit"
+ "\n(1) test 1"
+ "\n(2) test 2"
+ "\n(3) new board"
+ "\n>>> ");
input = in.nextInt();
if (input == 3){
this.initUserData();
} else if (input == 0){
kg = false;
} else if (input == 1){
board.setSize(7, 5);
board.setCollidable(3, 1);
board.setCollidable(3, 2);
board.setCollidable(3, 3);
board.printAStarPath(1, 2, 5, 2);
} else if (input == 2){
board.setSize(25, 25);
board.setCollidable(5, 4);
board.setCollidable(4, 5);
board.setCollidable(3, 3);
board.printAStarPath(15, 15, 4, 4);
}
} // end while
} // end boardMenu
public void initUserData(){
boolean kgTmp = true;
int xTmp, yTmp, iTmp, jTmp = 0;
// initiate input device
Scanner in = new Scanner(System.in);
// 0: determine board size
System.out.print("\nBoard width: ");
xTmp = in.nextInt();
System.out.print("\nBoard height: ");
yTmp = in.nextInt();
board.setSize(xTmp, yTmp);
// 1: determine obstruction locations
kgTmp = true;
while(kgTmp){
System.out.print("\nObstruction x Loc: ");
xTmp = in.nextInt();
System.out.print("\nObstruction y Loc: ");
yTmp = in.nextInt();
board.setCollidable(xTmp, yTmp);
System.out.print("\nMore Obstructions?(0=no;1=yes): ");
if(in.nextInt() == 0)
kgTmp = false;
} // end while
// 2: determine start location
System.out.print("\nStart x Loc: ");
xTmp = in.nextInt();
System.out.print("\nStart y Loc: ");
yTmp = in.nextInt();
// 3: determine end location
System.out.print("\nEnd x Loc: ");
iTmp = in.nextInt();
System.out.print("\nEnd y Loc: ");
jTmp = in.nextInt();
System.out.println("\nredy for astar");
// 4: determine and print A* path
board.printAStarPath(xTmp, yTmp, iTmp, jTmp);
System.out.println("\nastar shud be done?");
} // end initBoardData
} // end class def
Grid:
package pathFinding;
import java.util.*;
public class Grid {
private List<List<Node>> grid;
List<List<Integer>> path;
private int width;
private int height;
//------------------------------------------------------------------------
// Name: Constructor
// Desc: Takes in a width & height. Initializes stuff.
//------------------------------------------------------------------------
public Grid(int width, int height){
grid = new ArrayList<List<Node>>();
path = new ArrayList<List<Integer>>();
this.width = width;
this.height = height;
initGrid(width, height);
} // end constructor
//------------------------------------------------------------------------
// Name: initGrid
// Desc: initializes the grid with data
//------------------------------------------------------------------------
public void initGrid(int w, int h){
// add columns
for (int i=0;i<w;i++)
grid.add(new ArrayList<Node>());
// fill grid with nodes
for (int i=0;i<w;i++)
for (int j=0;j<h;j++)
grid.get(i).add(new Node(i, j));
} // end initGrid
//------------------------------------------------------------------------
// Name: setSize
// Desc: Sets the size of the grid
//------------------------------------------------------------------------
public void setSize(int w, int h){
this.width = w;
this.height = h;
// update the nodes
clearAll();
initGrid(width, height);
} // end setSize
//------------------------------------------------------------------------
// Name: clearAll
// Desc: Clears any data in grid and path
//------------------------------------------------------------------------
public void clearAll(){
// removes all rows/columns/nodes
grid.clear();
path.clear();
} // end clearAll
//------------------------------------------------------------------------
// Name: printGrid
// Desc: Prints the whole grid
//------------------------------------------------------------------------
public void printGrid(){
// prints every node's value
// loop thru columns
for (int j=0;j<height;j++){
// thru row
for (int i=0;i<width;i++)
grid.get(i).get(j).printText();
System.out.println();
} // end j loop
} // end printGrid
//------------------------------------------------------------------------
// Name: setCollidable
// Desc: Sets a node at x,y to collidable
//------------------------------------------------------------------------
public void setCollidable(int x, int y){
// makes a node at x,y collidable
grid.get(x).get(y).setCollidable(true);
grid.get(x).get(y).setText("[X]");
} // end setCollidable
//------------------------------------------------------------------------
// Name: printAStarPath
// Desc: Finds and prints the path from start to end
// Errr: This function only almost works :( ... oh well i tried
//------------------------------------------------------------------------
public void printAStarPath(int startx, int starty, int endx, int endy){
//========================================================
// PSEUDO CODE BRO.
//========================================================
// 1: Declarations
// PART ONE
// 2: Initialize:
// 1: Drop current node from openList
// Add current node to closedList
// 2: Set current node as parent for each neighbor
// Add neighbors to openList
//
// PART TWO
// 3: Loop: (thru openList)
//
// (openList should contain neighbors of closedList nodes here)
//
// EXAMPLE:
//
// n n n n n
// n n n n n>
// n S * * n-> E (the closest star is the current node)
// n n n n n>
// n n n n n
//
// 1: Set neighbor w/ lowest F-cost from the openList as current node
// 2: Add this new node to the closedList
// Remove from openList
// 3: Loop (for each neighbor):
// 1: Add openlist'less neighbors to openList
// Set current node as parent for neighbor node
// 2: If neighbor is already on the openList:
// 1: Get G-cost of neighbor IF: neighbor's parent is current node's parent (default)
// IF: neighbor's parent is current node
// 2: If the 2nd G-cost is less:
// 1: set neighbor's parent to current node
// 2: recalculate F and G costs (possibly you don't need this)
// 4: Stop: IF: end node is in closedList or,
// IF: end node is not in closedList and openList is empty
// 4: Save/Return Path
// 5: Print Results: (if you wanna print)
// 1: Fill grid with proper symbols
// 2: Print grid
//===========//
// 1 //
//===========//
List<List<Integer>> closedList = new ArrayList<List<Integer>>();
List<List<Integer>> openList = new ArrayList<List<Integer>>();
int x = startx;
int y = starty;
int gOrig = 0;
int gThru = 0;
boolean condition = false;
//===========//
// 2 //
//===========//
if (closedList.contains(Arrays.asList(x, y)) == false)
closedList.add(Arrays.asList(x, y));
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3 //
//===========//
while(condition == false){
//===========//
// 3.1 //
//===========//
// selecting lowest F-cost node
x = getLowestFCostNodePos(openList, endx, endy)[0];
y = getLowestFCostNodePos(openList, endx, endy)[1];
//===========//
// 3.2 //
//===========//
closedList.add(Arrays.asList(x, y));
openList.remove(Arrays.asList(x, y));
//===========//
// 3.3 //
//===========//
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
if (openList.contains(Arrays.asList(i, j)) == false){
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
}//end if (in openList?)
else{
// getting G-costs
gOrig = grid.get(i).get(j).getG();
grid.get(i).get(j).setParent(grid.get(x).get(y));
gThru = grid.get(i).get(j).getG();
// comparing G-costs
if (gOrig < gThru){
// revert parent back the way it was
grid.get(i).get(j).setParent(grid.get(x).get(y).getParent());
}//end if (G-costs)
// adding to openList
openList.add(Arrays.asList(i, j));
}//end else (in openList?)
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3.5 //
//===========//
if (openList.size() == 0){
condition = true;
System.out.print("\nNo Path.\n");
} else if (closedList.contains(Arrays.asList(endx, endy)) == true){
condition = true;
System.out.print("\nPath Found.\n");
}
}//end while loop (condition)
//===========//
// 4 //
//===========//
if (openList.size() > 0)
getNodePath(grid.get(endx).get(endy));
//===========//
// 5.1 //
//===========//
if (openList.size() > 0)
for (int i=0; i<path.size(); i++){
// setting symbols
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
}
// setting start/end
grid.get(startx).get(starty).setText("[S]");
grid.get(endx).get(endy).setText("[E]");
//===========//
// 5.2 //
//===========//
printGrid();
} // end printAStarPath
//------------------------------------------------------------------
// Name: getNodePath
// Desc: returns coordinates of path (in order) from start to end
//------------------------------------------------------------------
public void getNodePath(Node node){
// redo this function with the parent of node
if (node.getParent() != null){
// add a coordinate to path list
this.path.add(0, Arrays.asList(node.getX(), node.getY()));
// recur
getNodePath(node.getParent());
}//end if (recursive)
} // end getNodePath
//------------------------------------------------------------------
// Name: getLowestFCostNodePos
// Desc: returns coordinates of node with lowest F-cost in openList
//------------------------------------------------------------------
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int xTmp = 0;
int yTmp = 0;
int fMin = 1000000;
int[] cords = new int[2];
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
xTmp = openList.get(i).get(0);
yTmp = openList.get(i).get(1);
// compare F-values
if (fMin > grid.get(xTmp).get(yTmp).getF(endx, endy)){
// set temporary F-cost
fMin = grid.get(xTmp).get(yTmp).getF(endx, endy);
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = xTmp;
cords[1] = yTmp;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
} // end class def
Node:
package pathFinding;
public class Node {
private String text;
private int x;
private int y;
private boolean collidable;
private Node parent;
public Node(int x, int y){
text = "[ ]";
this.x = x;
this.y = y;
collidable = false;
parent = null;
} // end constructor
public void setText(String text){
this.text = text;
} // end setText
public int getX(){
return this.x;
} // end getX
public int getY(){
return this.y;
} // end getY
public void setCollidable(boolean arg0){
collidable = arg0;
} // end setCollidable
public boolean getCollidable(){
return collidable;
} // end getCollidable
public void setParent(Node n){
parent = n;
} // end setParent
public Node getParent(){
// for parent location: return new int[] {parent.getX(), parent.getY()};
return parent;
} // end getParent
public void printText(){
System.out.print(this.text);
} // end printText
public int getF(int endx, int endy){
return this.getG() + this.getH(endx, endy);
} // end getF
public int getG(){
// calculate exact distance from start
if (parent != null){
if (parent.getX()-this.x == 0 || parent.getY()-this.y == 0)
return parent.getG() + 10;
else
return parent.getG() + 14;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end (Manhattan distance)
return (Math.abs(this.x-endx) + Math.abs(this.y-endy))*10;
} // end getH
} // end class def
I came back to this code after a while and I just tested a new graph which, sadly, gives me this:
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][X][ ][ ][ ][ ]
[ ][ ][X][X][X][X][ ][*][ ][ ]
[ ][ ][*][*][E][X][ ][ ][*][ ]
[ ][*][X][X][X][X][ ][ ][ ][S]
[ ][ ][*][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][*][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][*][ ][*][ ][ ][ ]
[ ][ ][ ][ ][ ][*][ ][ ][ ][ ]
Can anyone figure out why this is happening?
Ok, so I've gone through your code and with the help of other's comments here I've managed to get it to work. I'm only posting the changed methods:
Grid.getLowestFCostNodePos doesn't keep track of the X and Y values of the node with the lowest F:
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int fMin = 1000000;
int[] cords = new int[2];
int minX = -1;
int minY = -1;
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
int xTmp = openList.get(i).get(0);
int yTmp = openList.get(i).get(1);
int fCandidate = grid.get(xTmp).get(yTmp).getF(endx, endy);
// compare F-values
if (fMin > fCandidate) {
// set temporary F-cost
fMin = fCandidate;
minX = xTmp;
minY = yTmp;
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = minX;
cords[1] = minY;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
Node.getG() Node.getH()` doesn't use the same units (H considers a step worth of 1 and G a step worth of 10 for N/S/E/W steps 14 for diagonal steps) and H doesn't consider diagonal steps. I normalised this to make one step always cost 1:
public int getG(){
// calculate exact distance from start
if (parent != null){
return parent.getG() + 1;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end
// Since we can walk diagonally we can cover the smallest of
// dx and dy while covering the longest. The distance is therefore
// the largest of the two
return Math.max(Math.abs(this.x - endx), Math.abs(this.y - endy));
} // end getH
Test board 1:
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
Test board 2:
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][E][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][X][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][S][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
Custom board:
[S][X][ ][ ][ ]
[*][X][ ][*][ ]
[*][X][*][X][*]
[*][X][*][X][*]
[ ][*][ ][X][E]
That said, you should consider implementing a Point class instead of using an ArrayList everywhere and use local variables and helper methods because your code is extremely verbose and cumbersome to read.
Lines like these give me a headache:
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
Changing ArrayList<Integer> for a custom Point class and using two local variables greatly improves readability:
Point point = path.get(i);
List<Node> row = grid.get(point.getX());
row.get(point.getY()).setText("[*]");
If you were to add an utility method to Grid for getting a specific Node from a Point you could reduce it to:
getNode(path.get(i)).setText("[*]");
And that method could be used in a lot of places to improve readability.