Hello,
I am working on a A* Search Program using a MinHeap in order to get the lowest F(x) valued nodes, however, I am not sure why my MinHeap starts going crazy. I tried the MinHeap Code alone, and it worked fine, but when I run it together with the program, it messes up after a while.
If you could help me make it work I would really appreciate it.
My program consists of:
Board.java
Node.java
MinHeap.java
AStarSearch.java
Main.java
PS. There are several print out coments that I used for debugging.
When running the program you will see a sequence of numbers followed by "size: number", that is the values in the MinHeap, you will notice that after a while the min value is not really the minimum, that is my headache.
Thanks for your help in advance.
// Board.java
//
public class Board {
int dim = 4;
Node map[][] = new Node[dim][dim];
int adjx[]={-1,0,1,0};
int adjy[]={0,-1,0,1};
int diagx[]={-1,1,1,-1};
int diagy[]={-1,-1,1,1};
static Node adj[]= new Node [4];
static Node diag[]= new Node [4];
public Board()
{ //initiating Board with Nodes
for(int i=0;i<dim;i++)
{
for(int j=0;j<dim;j++)
{
map[i][j]=new Node(i,j);
}
}
//Assigning Values Nodes
for(int i=0;i<dim;i++)
{
for(int j=0;j<dim;j++)
{
int x=map[i][j].getX(); //current x
int y=map[i][j].getY(); //current y
//Finding Adjacent Nodes
for(int t=0; t<4; t++)
{
int adjX=x+adjx[t];
int adjY=y+adjy[t];
if(adjX>=0 && adjX<dim && adjY >=0 && adjY<dim )//limits for board
{
adj[t]=map[adjX][adjY]; //adjacent array
}
}
//Assigning Adjacent Nodes
for(int t=0;t<4;t++)
{
if(t==0)
map[i][j].setAdj1(adj[t]);
if(t==1)
map[i][j].setAdj2(adj[t]);
if(t==2)
map[i][j].setAdj3(adj[t]);
if(t==3)
map[i][j].setAdj4(adj[t]);
}
//Finding Diagonal Nodes
for(int t=0; t<4; t++)
{
int diagX=x+diagx[t];
int diagY=y+diagy[t];
if(diagX>=0 && diagX<dim && diagY >=0 && diagY<dim )//limits for board
{
diag[t]=map[diagX][diagY]; //diagonal array
}
}
//Assigning Diagonal Nodes
for(int t=0;t<4;t++)
{
if(t==0)
map[i][j].setDiag1(diag[t]);
if(t==1)
map[i][j].setDiag2(diag[t]);
if(t==2)
map[i][j].setDiag3(diag[t]);
if(t==3)
map[i][j].setDiag4(diag[t]);
}
/* System.out.println("Adjacent Squares for ("+i+","+j+")");
for(int t=0;t<4;t++)
{
if(adj[t]!=null)
{
System.out.println("T="+t+" ("+adj[t].getX()+","+adj[t].getY()+")");
System.out.println("G cost: " + map[i][j].getG());
}
}
System.out.println("Diagonal Squares for ("+i+","+j+")");
for(int t=0;t<4;t++)
{
if(diag[t]!=null)
System.out.println("T="+t+" ("+diag[t].getX()+","+diag[t].getY()+")");
}
*/
for(int t=0;t<4;t++)
{ //Reseting arrays to null
adj[t]=null;
diag[t]=null;
}
}
}
}
//Get a node from the map
public Node getNode(int x, int y)
{
return map[x][y];
}
//Set Start Node
public void setStart(Node x)
{
map[x.getX()][x.getY()].setValue("A");
}
public void setWall(Node x)
{
map[x.getX()][x.getY()].setValue("W");
}
//Set Goal Node
public void setGoal(Node x)
{
map[x.getX()][x.getY()].setValue("B");
}
//Print board value
public void printBoard()
{
for(int i=0;i<dim;i++)
{
for(int j=0;j<dim;j++)
{
System.out.print(map[i][j].getValue()+" ");
}
System.out.println();
}
}
}
// Node.java
//
public class Node {
private String value;
private int x;
private int y;
private Node adj1;
private Node adj2;
private Node adj3;
private Node adj4;
private Node diag1;
private Node diag2;
private Node diag3;
private Node diag4;
private int f;
private int g;
private int h;
public Node(int X, int Y){
value="*";
x=X;
y=Y;
f=0;
g=0;
h=0;
adj1=null;
adj2=null;
adj3=null;
adj4=null;
diag1=null;
diag2=null;
diag3=null;
diag4=null;
}
public String getValue() {
return value;
}
public void setValue(String value) {
this.value = value;
}
public int getX() {
return x;
}
public void setX(int x) {
this.x = x;
}
public int getY() {
return y;
}
public void setY(int y) {
this.y = y;
}
public Node getAdj1() {
return adj1;
}
public void setAdj1(Node adj1) {
this.adj1 = adj1;
}
public Node getAdj2() {
return adj2;
}
public void setAdj2(Node adj2) {
this.adj2 = adj2;
}
public Node getAdj3() {
return adj3;
}
public void setAdj3(Node adj3) {
this.adj3 = adj3;
}
public Node getAdj4() {
return adj4;
}
public void setAdj4(Node adj4) {
this.adj4 = adj4;
}
public Node getDiag1() {
return diag1;
}
public void setDiag1(Node diag1) {
this.diag1 = diag1;
}
public Node getDiag2() {
return diag2;
}
public void setDiag2(Node diag2) {
this.diag2 = diag2;
}
public Node getDiag3() {
return diag3;
}
public void setDiag3(Node diag3) {
this.diag3 = diag3;
}
public Node getDiag4() {
return diag4;
}
public void setDiag4(Node diag4) {
this.diag4 = diag4;
}
public int getF() {
return f;
}
public void setF(int f) {
this.f = f;
}
public int getG() {
return g;
}
public void setG(int g) {
this.g = g;
}
public int getH() {
return h;
}
public void setH(int h) {
this.h = h;
}
}
// MinHeap.java
//
public class MinHeap {
private Node[] Heap;
private int maxsize;
private int size;
public MinHeap(int max) {
maxsize = max;
Heap = new Node[maxsize];
size = 0 ;
Heap[0] = new Node(4,4); //new node init //Heap[0].setF(1);
}
private int leftchild(int pos) {
return 2*pos;
}
private int rightchild(int pos) {
return 2*pos + 1;
}
private int parent(int pos) {
return pos / 2;
}
private boolean isleaf(int pos) {
return ((pos > size/2) && (pos <= size));
}
private void swap(int pos1, int pos2) {
Node tmp;
tmp = Heap[pos1];
Heap[pos1] = Heap[pos2];
Heap[pos2] = tmp;
}
public void insert(Node elem) {
size++;
Heap[size] = elem;
int current = size;
while (Heap[current].getF() < Heap[parent(current)].getF())
{
System.out.println(Heap[current].getF()+" HORROR");
swap(current, parent(current));
current = parent(current);
}
}
public void print() {
int i;
for (i=1; i<=size;i++)
{
System.out.print(Heap[i].getF() + " ");
}
System.out.println("size: "+size);
// System.out.println();
}
public Node removeMin() {
swap(1,size);
size--;
if (size != 0)
pushdown(1);
return Heap[size+1];
}
public Node getMin(){
return Heap[1];
}
public boolean isEmpty()
{
if(size==0)
{
return true;
}
else
{
return false;
}
}
private void pushdown(int position) {
int smallestchild;
while (!isleaf(position))
{
smallestchild = leftchild(position);
if ((smallestchild < size) && (Heap[smallestchild].getF() > Heap[smallestchild+1].getF()))
smallestchild = smallestchild + 1;
if (Heap[position].getF() <= Heap[smallestchild].getF())
return;
swap(position,smallestchild);
position = smallestchild;
}
}
}
// AStarSearch.java
//
public class AStarSearch {
private int dim=4; //dimension of map
private int adjG;
private int diagG;
private Node Start;
private Node Goal;
private Board Map;
private MinHeap OpenSet; //Set of Tentative Nodes
private Node[] ClosedSet; //Set of Nodes Evaluated
private int ClosedSetCnt;
private Node nodex; //node having lowest f
private int G;
public AStarSearch(Node A, Node B, Board M)
{
Start=A;
Goal=B;
Map=M;
G=0;
diagG=14;
adjG=10;
OpenSet=new MinHeap(8*dim*dim);
OpenSet.insert(Map.getNode(Start.getX(),Start.getY()));
ClosedSet=new Node[8*dim*dim];
ClosedSetCnt=0;
}
public String search()
{
while(!OpenSet.isEmpty())
{
nodex=OpenSet.getMin(); //node having lowest f
System.out.println("Current Min: "+OpenSet.getMin().getF());
G=G+nodex.getG();
System.out.println("Main Node: ("+nodex.getX()+","+nodex.getY()+") "+G);
if(nodex.getValue()==Goal.getValue())
{
ClosedSet[ClosedSetCnt]=nodex;
return "Found";
}
System.out.println("Size of CloseSet: "+ClosedSetCnt);
OpenSet.removeMin(); //remove nodex from OpenSet
OpenSet.print();
ClosedSet[ClosedSetCnt]=nodex; //add nodex to ClosedSet
if(nodex.getAdj1()!=null)
{
if(nodex.getAdj1().getValue()!="W")
{
System.out.println("Adj1 ("+nodex.getAdj1().getX()+","+nodex.getAdj1().getY()+")");
nodex.getAdj1().setG(G+adjG);
nodex.getAdj1().setH(heuristic(nodex.getAdj1(),Goal));
nodex.getAdj1().setF(nodex.getAdj1().getG()+nodex.getAdj1().getH());
OpenSet.insert(nodex.getAdj1());
System.out.println("---G of Adj1:" +nodex.getAdj1().getG());
System.out.println("---H of Adj1:" +nodex.getAdj1().getH());
System.out.println("---F of Adj1:" +nodex.getAdj1().getF());
OpenSet.print();
/* if(nodex.getAdj1().getF()==OpenSet.getMin().getF() && ClosedSetCnt==1)
{
//System.out.println(OpenSet.)
return "Problem";
}*/
}
}
if(nodex.getAdj2()!=null)
{
if(nodex.getAdj2().getValue()!="W")
{
System.out.println("Adj2 ("+nodex.getAdj2().getX()+","+nodex.getAdj2().getY()+")");
nodex.getAdj2().setG(G+adjG);
nodex.getAdj2().setH(heuristic(nodex.getAdj2(),Goal));
nodex.getAdj2().setF(nodex.getAdj2().getG()+nodex.getAdj2().getH());
OpenSet.insert(nodex.getAdj2());
System.out.println("---G of Adj2:" +nodex.getAdj2().getG());
System.out.println("---H of Adj2:" +nodex.getAdj2().getH());
System.out.println("---F of Adj2:" +nodex.getAdj2().getF());
OpenSet.print();
}
}
if(nodex.getAdj3()!=null)
{
if(nodex.getAdj3().getValue()!="W")
{
System.out.println("Adj3 ("+nodex.getAdj3().getX()+","+nodex.getAdj3().getY()+")");
nodex.getAdj3().setG(G+adjG);
nodex.getAdj3().setH(heuristic(nodex.getAdj3(),Goal));
nodex.getAdj3().setF(nodex.getAdj3().getG()+nodex.getAdj3().getH());
OpenSet.insert(nodex.getAdj3());
System.out.println("---G of Adj3:" +nodex.getAdj3().getG());
System.out.println("---H of Adj3:" +nodex.getAdj3().getH());
System.out.println("---F of Adj3:" +nodex.getAdj3().getF());
OpenSet.print();
}
}
if(nodex.getAdj4()!=null)
{
if(nodex.getAdj4().getValue()!="W")
{
System.out.println("Adj4 ("+nodex.getAdj4().getX()+","+nodex.getAdj4().getY()+")");
nodex.getAdj4().setG(G+adjG);
nodex.getAdj4().setH(heuristic(nodex.getAdj4(),Goal));
nodex.getAdj4().setF(nodex.getAdj4().getG()+nodex.getAdj4().getH());
OpenSet.insert(nodex.getAdj4());
System.out.println("---G of Adj4:" +nodex.getAdj4().getG());
System.out.println("---H of Adj4:" +nodex.getAdj4().getH());
System.out.println("---F of Adj4:" +nodex.getAdj4().getF());
System.out.println("Value of Adj4:" +nodex.getAdj4().getValue());
OpenSet.print();
}
}
if(nodex.getDiag1()!=null)
{
if(nodex.getDiag1().getValue()!="W")
{
System.out.println("Diag1 ("+nodex.getDiag1().getX()+","+nodex.getDiag1().getY()+")");
nodex.getDiag1().setG(G+diagG);
nodex.getDiag1().setH(heuristic(nodex.getDiag1(),Goal));
nodex.getDiag1().setF(nodex.getDiag1().getG()+nodex.getDiag1().getH());
OpenSet.insert(nodex.getDiag1());
System.out.println("---G of Diag1:" +nodex.getDiag1().getG());
System.out.println("---H of Diag1:" +nodex.getDiag1().getH());
System.out.println("---F of Diag1:" +nodex.getDiag1().getF());
OpenSet.print();
}
}
if(nodex.getDiag2()!=null)
{
if(nodex.getDiag2().getValue()!="W")
{
System.out.println("Diag2 ("+nodex.getDiag2().getX()+","+nodex.getDiag2().getY()+")");
nodex.getDiag2().setG(G+diagG);
nodex.getDiag2().setH(heuristic(nodex.getDiag2(),Goal));
nodex.getDiag2().setF(nodex.getDiag2().getG()+nodex.getDiag2().getH());
OpenSet.insert(nodex.getDiag2());
System.out.println("---G of Diag2:" +nodex.getDiag2().getG());
System.out.println("---H of Diag2:" +nodex.getDiag2().getH());
System.out.println("---F of Diag2:" +nodex.getDiag2().getF());
OpenSet.print();
}
}
if(nodex.getDiag3()!=null)
{
if(nodex.getDiag3().getValue()!="W")
{
System.out.println("Diag3 ("+nodex.getDiag3().getX()+","+nodex.getDiag3().getY()+")");
nodex.getDiag3().setG(G+diagG);
nodex.getDiag3().setH(heuristic(nodex.getDiag3(),Goal));
nodex.getDiag3().setF(nodex.getDiag3().getG()+nodex.getDiag3().getH());
OpenSet.insert(nodex.getDiag3());
System.out.println("---G of Diag3:" +nodex.getDiag3().getG());
System.out.println("---H of Diag3:" +nodex.getDiag3().getH());
System.out.println("---F of Diag3:" +nodex.getDiag3().getF());
OpenSet.print();
}
}
if(nodex.getDiag4()!=null)
{
if(nodex.getDiag4().getValue()!="W")
{
System.out.println("Diag4 ("+nodex.getDiag4().getX()+","+nodex.getDiag4().getY()+")");
nodex.getDiag4().setG(G+diagG);
nodex.getDiag4().setH(heuristic(nodex.getDiag4(),Goal));
nodex.getDiag4().setF(nodex.getDiag4().getG()+nodex.getDiag4().getH());
OpenSet.insert(nodex.getDiag4());
System.out.println("---G of Diag4:" +nodex.getDiag4().getG());
System.out.println("---H of Diag4:" +nodex.getDiag4().getH());
System.out.println("---F of Diag4:" +nodex.getDiag4().getF());
OpenSet.print();
}
}
ClosedSetCnt++;
OpenSet.print();
System.out.println("Test G:"+G);
// System.out.println(OpenSet.getMin().getF());
System.out.println();
nodex=null;
}
return "Not found";
}
public void getPath()
{
System.out.println("PATH:");
for(int i=0;i<(8*dim*dim);i++)
{
if(ClosedSet[i]!=null)
{
int x=ClosedSet[i].getX();
int y=ClosedSet[i].getY();
System.out.print("("+x+","+y+")");
}
}
}
public int heuristic(Node s, Node g)
{
int currentX=s.getX()+1;
int targetX=g.getX()+1;
int currentY=s.getY()+1;
int targetY=g.getY()+1;
int H;
// System.out.println("Start is: ("+s.getX()+","+s.getY()+")");
// System.out.println("Goal is: ("+g.getX()+","+g.getY()+")");
int xDis=Math.abs(currentX-targetX);
int yDis=Math.abs(currentY-targetY);
if(xDis>yDis)
{
H = 14*yDis + 10*(xDis-yDis);
return H;
}
else
{
H = 14*xDis + 10*(yDis-xDis);
return H;
}
}
}
// Main.java
//
public class Main {
public static void main(String[] args){
Board mapa=new Board();
// mapa.printBoard();
Node start=mapa.getNode(1,0);
Node goal=mapa.getNode(1,3);
Node wall=mapa.getNode(1,1);
Node wall2=mapa.getNode(2,1);
mapa.setStart(start);
mapa.setGoal(goal);
// mapa.setWall(wall);
// mapa.setWall(wall2);
// System.out.println(mapa.getNode(1,3).getDiag1().getX()+","+mapa.getNode(1,3).getDiag1().getY());
// System.out.println();
mapa.printBoard();
AStarSearch Search = new AStarSearch(start,goal,mapa);
System.out.println(Search.search());
Search.getPath();
}
}