A* implementation always returns same value

I seem to be either losing my mind or misimplementing the A* algorithm:

Below is my code, it seems that no matter what values I enter it will always return 360. Am I missing some critical piece of information here? Also before anyone asks yes this is related to a machine learning assignment I received.

public class A_Star {

private ArrayList<SearchNode> closedNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> openNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> siblingNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> obstacleNodes;
final SearchNode START_NODE = new SearchNode(0,115,655);
final SearchNode END_NODE = new SearchNode(0,380,560);
final int HEURISTIC = 1 * Math.abs((END_NODE.getxCoordinate() - START_NODE.getxCoordinate()) + (START_NODE.getyCoordinate() - END_NODE.getyCoordinate())) ;

private int cost = 0;
//Start: (115, 655) End: (380, 560)

public  int starSearch(SearchNode currentNode, Set<SearchNode> obstacleNodes) throws Exception  {
    //h(n) = D * (abs(n.x-goal.x) + abs(n.y-goal.y))

    boolean isMaxY = false;
    boolean isMaxX = false;
    int currentY = currentNode.getyCoordinate();
    int currentX = currentNode.getxCoordinate();
    System.out.println(currentNode.getxCoordinate() + " " + currentNode.getyCoordinate() + " Current Coordinates");

    int currentGScore = Math.abs(currentNode.getxCoordinate() - END_NODE.getxCoordinate()) + (currentNode.getyCoordinate() - END_NODE.getyCoordinate());

    currentNode.setgScore(currentGScore);
    System.out.println("Current node G score at entry: " + currentNode.getgScore());

    if(!closedNodes.contains(currentNode)){
        closedNodes.add(currentNode);

    }

    if(currentNode.getyCoordinate() == END_NODE.getyCoordinate()){
                isMaxY=true;
            }
         if(currentNode.getxCoordinate() == END_NODE.getxCoordinate())   {
                isMaxX =true;
            }

    SearchNode bottomCenter = new SearchNode(0,currentNode.getxCoordinate(), currentNode.getyCoordinate() -1);
   SearchNode middleRight = new SearchNode开发者_如何转开发(0,currentNode.getxCoordinate() +1, currentNode.getyCoordinate() );
   SearchNode middleLeft = new SearchNode(0,currentNode.getxCoordinate() -1, currentNode.getyCoordinate());
    SearchNode topCenter = new SearchNode(0,currentNode.getxCoordinate(), currentNode.getyCoordinate()+1);
     int middleRightGScore = Math.abs(middleRight.getxCoordinate() - END_NODE.getxCoordinate()) + (middleRight.getyCoordinate() - END_NODE.getyCoordinate());
     int bottomCenterGScore = Math.abs(bottomCenter.getxCoordinate() - END_NODE.getxCoordinate()) + (bottomCenter.getyCoordinate() - END_NODE.getyCoordinate());
     int middleLeftGScore = Math.abs(middleLeft.getxCoordinate() - END_NODE.getxCoordinate()) + (middleLeft.getyCoordinate() - END_NODE.getyCoordinate());
     int topCenterGScore = Math.abs(topCenter.getxCoordinate() - END_NODE.getxCoordinate()) + (topCenter.getyCoordinate() - END_NODE.getyCoordinate());
    middleRight.setgScore(middleRightGScore);
     middleLeft.setgScore(middleLeftGScore);
     bottomCenter.setgScore(bottomCenterGScore);
     topCenter.setgScore(topCenterGScore);
     for(SearchNode obstacleNode:obstacleNodes){
         int obstacleX = obstacleNode.getxCoordinate();
         int obstacleY = obstacleNode.getyCoordinate();

          if((middleRight != null) && (middleRight.getxCoordinate() == obstacleX)){
        if(middleRight.getyCoordinate() == obstacleY){

           // throw new Exception();
            System.out.println("REMOVING AND NULLING: " + middleRight.toString());
            siblingNodes.remove(middleRight);
                  middleRight = null;

        }
    }



      if((middleLeft!=null)&&(middleLeft.getxCoordinate() == obstacleX)){
        if(middleLeft.getyCoordinate() == obstacleY){

            System.out.println("REMOVING AND NULLING: " + middleLeft.toString());
            siblingNodes.remove(middleLeft);
                  middleLeft=null;
        }
    } if((bottomCenter!=null) &&(bottomCenter.getxCoordinate() == obstacleX)){
        if(bottomCenter.getyCoordinate() == obstacleY){

            System.out.println("REMOVING AND NULLING: " + bottomCenter.toString());
            siblingNodes.remove(bottomCenter);
                  bottomCenter = null;
        }
    } if((topCenter!=null) && (topCenter.getxCoordinate() == obstacleX)){
        if(topCenter.getyCoordinate() == obstacleY){
                  System.out.println("REMOVING AND NULLING: " + topCenter.toString());
            siblingNodes.remove(topCenter);
                  topCenter=null;
        }
    }

    if((middleRight != null) && (middleRight.getxCoordinate() != obstacleX)){
        if(middleRight.getyCoordinate() != obstacleY){
                   System.out.println("ADDING THE FOLLOWING:  " + middleRight.toString());
                  siblingNodes.add(middleRight);
        }
    }



      if((middleLeft != null) && (middleLeft.getxCoordinate() != obstacleX)){
        if(middleLeft.getyCoordinate() != obstacleY){

                  siblingNodes.add(middleLeft);
        }
    } if((bottomCenter != null) && (bottomCenter.getxCoordinate() != obstacleX)){
        if(bottomCenter.getyCoordinate() != obstacleY){

                  siblingNodes.add(bottomCenter);
        }
    } if((topCenter !=null) && (topCenter.getxCoordinate() != obstacleX)){
        if(topCenter.getyCoordinate() != obstacleY){

                  siblingNodes.add(topCenter);
        }
    }
    }

    int highestScore = currentNode.getgScore();
    for(SearchNode node: siblingNodes){
          if(node == null){
             continue;
          }
        if(node.getxCoordinate() == END_NODE.getxCoordinate() && node.getyCoordinate() == END_NODE.getyCoordinate()){
            System.out.println("Returning cost: " + ++cost + " of: " + node.toString());
            return cost;
        }
       // System.out.println("Current node size: " + siblingNodes.size());
         if(node.getgScore() < highestScore){

             highestScore = node.getgScore();
             currentNode=node;
             cost++;
             System.out.println("Changed highest score: " + highestScore);
             System.out.println("Removing node: " + node.toString());
             siblingNodes.remove(node);
             System.out.println("Incrementing cost the Current Cost: " + cost);
             starSearch(currentNode,obstacleNodes);
             break;
         }

    if(isMaxY && isMaxX){
                  return cost;
    }
    }
    return cost;
    //Always move diagonal right downwards
}

public static void main(String[] args) throws Exception{
    System.out.println("Hello");
     A_Star star = new A_Star();
    HashSet<SearchNode> obstacles = new HashSet<SearchNode>();
    obstacles.add(new SearchNode(0,311,530));
    obstacles.add(new SearchNode(0,311,559));
    obstacles.add(new SearchNode(0,339,578));
    obstacles.add(new SearchNode(0,361,560));
    obstacles.add(new SearchNode(0,361,528));
    obstacles.add(new SearchNode(0,116, 655));
   int cost = star.starSearch(star.START_NODE, obstacles);
    System.out.println(cost);

    //((311, 530), (311, 559), (339, 578), (361, 560), (361, 528), (336, 516))
}

}

and

public class SearchNode {

    private int xCoordinate;
    private int yCoordinate;
    private int cost;

public int getfScore() {
    return fScore;
}

public void setfScore(int fScore) {
    this.fScore = fScore;
}

private int fScore;

public int getgScore() {
    return gScore;
}

public void setgScore(int gScore) {
    this.gScore = gScore;
}

private int gScore;  

public SearchNode(int cost, int xCoordinate,int yCoordinate){

this.cost=cost;

this.xCoordinate =xCoordinate;

this.yCoordinate = yCoordinate;

}

public int getCost() { return cost; }

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



   public int getxCoordinate() {
       return xCoordinate;
   }

   public void setxCoordinate(int xCoordinate) {
       this.xCoordinate = xCoordinate;
   }

   public int getyCoordinate() {
       return yCoordinate;
   }

   public void setyCoordinate(int yCoordinate) {
       this.yCoordinate = yCoordinate;
   }

public String toString(){
    return "Y Coordinate: " + this.yCoordinate + " X Coordinate: " + this.xCoordinate + " G Score: " + this.gScore;
}

}

I am assuming the graph is a regular rectangular grid that has obstacle nodes in it through which none of the solutions should pass. Moreover I am assuming traveling from a node to a neighbour node is 1. Also I realized that Manhattan Distance is used as the heuristic.

Given these, I am afraid yours is a misimplementation.

First of all instead of a recursive one you should use an iterative approach. Given the size of your graph, you would definitely get Stackoverflows if it was a correct implementation.

Secondly, there seems to be a problem about the concepts of GValue, HValue, FValue and/or cost. I feel obliged to give an informal description of these terms.

The simple formula A* uses is F = G + H where

G is the currently calculated cost of travelling from the start node to the current node. So, for the start node the Gvalue should be 0, any node reachable from the startNode should have a G value of 1 (my assumption was so, traveling from a node to a neighbour node) I would like to stress the 'currently' term here, because the gValue of a node may change during the run of the algorithm.

H is the heuristic part of the cost, denoting the cost from the current node to the end node. Unlike the G part, H value of a node does not change at all (well I have a doubt here, may there be such a heuristic?, yours does not so let's continue), it should be computed only once. Your implementation seems to be using the Manhattan Distance as the heuristic which is definitely the best heuristic for such a graph. But beware my friend, there seems to be a small problem there as well: the absolute values of the differences should be taken seperately and then be summed.

And the F is the sum of these values denoting the possible cost of having a solution passing from the current node (Given your graph and heuristic any calculated F value should be equal or less than the actual cost, which is good).

With these at hand I would have used a SearchNode like this:

public class SearchNode {
    private int xCoordinate;
    private int yCoordinate;
    private double gScore;
    private double hScore;

    public double getfScore() {
        return gScore + hScore;
    }

    public double getgScore() {
        return gScore;
    }

    public void setgScore(int gScore) {
        this.gScore = gScore;
    }


    public SearchNode(int xCoordinate,int yCoordinate, double gScore, SearchNode endNode) {
        this.gScore=gScore;
        this.hScore = //Manhattan distance from this node to end node
        this.xCoordinate =xCoordinate;
        this.yCoordinate = yCoordinate;
    }

   public int getxCoordinate() {
       return xCoordinate;
   }

   public int getyCoordinate() {
       return yCoordinate;
   }
}

Then the algorithm may be implemented like :

private ArrayList<SearchNode> closedNodes = new ArrayList<SearchNode>();
private ArrayList<SearchNode> openNodes = new ArrayList<SearchNode>();
//create the start and end nodes
SearchNode end = new SearchNode(380, 560, -1, null);
SearchNode start = new SearchNode(115,655, 0, end);


// add start node to the openSet

openNodes.Add(start);

while(openNodes.Count > 0) // while there still is a node to test
{
    // I am afraid there is another severe problem here.
    // OpenSet should be PriorityQueue like collection, not a regular Collection.
    // I suggest you to take a look at a Minimum BinaryHeap implementation. It has a logN complexity
    // of insertion and deletion and Constant Complexity access.

   // take the Node with the smallest FValue from the openSet. (With BinHeap constant time!)
    SearchNode current = openNodes.GetSmallestFvaluedNode(); // this should both retrieve and remove the node fromt he openset.

    // if it is the endNode, then we are node. The FValue (or the Gvalue as well since h value is zero here) is equal to the cost.
    if (current.EqualTo(end)) // not reference equality, you should check the x,y values
{
    return current.getfScore();
}

   //check the neighbourNodes, they may have been created in a previous iteration and already present in the OpenNodes collection. If it is the case, their G values should be compared with the currently calculated ones.
 // dont forget to check the limit values, we probably do not need nodes with negative or greater than the grid size coordinate values, I am not writing it
 // also here is the right place to check for the blocking nodes with a simple for loop I am not writing it either

  double neighbourGValue = current.getgScore() + 1;
 if (openNodes.Contains(current.getXCoordinate(), current.getYCoordinate() + 1))
  {
     // then compare the gValue of it with the current calculated value.
     SearchNode neighbour = openNodess.getNode(current.getXCoordinate(), current.getYCoordinate() + 1);
     if(neighbour.getgScore() > neighbourGValue)
        neighbour.setgScore(neighbourGValue);
  }
  else if(!closedNodes.Contains(current.getXCoordinate(), current.getYCoordinate()))
  {
      // create and add a fresh Node
     SearchNode n = new SearchNode(current.getXCoordinate(), current.getYCoordinate() + 1, neighbourGValue, endNode);
     openNodes.Add(n);
  }
  // do the same for the other sides : [x+1,y - x-1,y - x, y-1]

  // lastly add the currentNode to the CloseNodes.
  closedNodes.Add(current);
}

// if the loop is terminated without finding a result, then there is no way from the given start node to the end node.
 return -1;

The only question pops to my mind about the above implementation is the line

if (openNodes.Contains(current.getXCoordinate(), current.getYCoordinate() + 1))

Even if the open set is implemented as Min Binary Heap there is no easy way to check the non-minimum f valued nodes. I cant remember the details now but I remember implementing this one with logN complexity. Moreover my implementation was accessing and changing the g value of that node (if it is necessary) in one call, so you dont spent time retrieving it again. No matter if g value is changed, if there is a node with the given coordinates, it was returning true so no new node is generated.

One last thing I realized when I finished writing all these. You said, with any input given your implementation was calculating the same result. Well if the input you are mentioning is the obstacle nodes, then it will most of the time be the case that it will find, no matter what implementation it is, the same distance since it is looking for the shortest possible distance. In the below image I tried to explain that.

Below is my code, it seems that no matter what values I enter it will always return 360.

My first guess is that you have a fixed heuristic cost for each node. So might that 360 come from?

final SearchNode START_NODE = new SearchNode(0,115,655);
final SearchNode END_NODE = new SearchNode(0,380,560);

Assuming you're using a Manhattan Distance heuristic, (380-115) + (655-560) = 265 + 95 = 360

The code is a little difficult to read because of its formatting. But my guess is you calculated the h-value for the start node and you are using that for every node thereafter. Remember that h(x) <= d(x,y) + h(y) and calculate it for each node expansion.