A* Pathfinding.

[DarkLightning7]

Ive been working on pathfinding algorithms for a couple weeks now and A Star has me stumped. Right now i am attempting to Pathfind over a maze in a matrix of the form

boolean map[x][y][5]; where index 0-3 are the walls on the cells starting on the right with 0, up being held in 1 and so on, where true means there is a wall. Index 4 is to mark the cell as visited or unvisited and is not used by my implementation of A.

For some reason I cant comprehend the search gets stuck within 10 spaces of the start and just cycles through those cells. The problem is inside the while(!openSet.isEmpty()) loop and appears to be connected to elements of the closed set being added to the open set. I based this implementation directly on the below psuedocode and it is almost a line by line match.

Here is my code:

    public Stack AStar(int[] start, int[] end){
        ArrayList<int[]> closedSet = new ArrayList<int[]>();  //stores the set of already visited cells.
        ArrayList<int[]> openSet = new ArrayList<int[]>();    //stores the set of yet to be evaluated nodes which are next to evaluated nodes.
        HashMap<int[], Integer> gScore = new HashMap<int[], Integer>();   //stores the cost to get to the cell stored in the form int[0] = x and int[1] = y.
        HashMap<int[], Integer> hScore = new HashMap<int[], Integer>();   //stores the minimum cost to get from current cell to final cell 
        HashMap<int[], Integer> fScore = new HashMap<int[], Integer>();    //stores fscore = gscore + hscore
        HashMap<int[], int[]> path = new HashMap<int[], int[]>();          //stores the traveled routes where first int[] is current cell and second is cell we came from
        gScore.put(start, 0);
        hScore.put(start, HCost(start, end));
        fScore.put(start, HCost(start, end));
        openSet.add(start);

        while(!openSet.isEmpty()){
            int[] current = openSet.get(0);

            if(current.equals(end)){
                Stack sPath = new Stack();
                sPath.push(current);
                while(current != start){
                    current = path.get(current);
                    sPath.push(current);
                }
                return sPath;
            }

            openSet.remove(current);
            closedSet.add(current);
            int[][] neighbors = getNeighbors(current);
            for(int i = 0; i < neighbors.length; i++){
                int tgScore = (int)gScore.get(current) + 1;
                int[] n = {neighbors[i][0], neighbors[i][1]};
                if(closedSet.contains(n)){
                    if(tgScore >= (int)gScore.get(n))
                        continue;
                }
                if((!openSet.contains(n) && !closedSet.contains(n)) || tgScore < (int)gScore.get(n) && closedSet.contains(n)){
                    path.put(n, current);
                    gScore.put(n, tgScore);
                    fScore.put(n, tgScore + HCost(n, end));
                    if(!openSet.contains(n)){
                        AddToOpen(n, fScore, openSet);
                    }
                }
            }
        }
    return null;  //failed to find a path.
    }

    private int[][] getNeighbors(int[] cell){
        int[][] open;
        boolean a = false,b = false,c = false,d = false;
        byte size = 0;
        if((cell[0] + 1) <= x && !map[cell[0] + 1][cell[1]][0]) {
            size++;
            a = true;
        }
        if((cell[1] + 1) <= y && !map[cell[0]][cell[1] + 1][1]) {
            size++;
            b = true;
        }
        if((cell[0] - 1) >= 0 && !map[cell[0] - 1][cell[1]][2]){
            size++;
            c = true;
        }
        if((cell[1] - 1) >= 0 && !map[cell[0]][cell[1] - 1][3]) {
            size++;
            d = true;
        }
        open = new int[size][2];
        if(d){
            size--;
            open[size][0] = cell[0];
            open[size][1] = cell[1] - 1;
        }
        if(c){
            size--;
            open[size][0] = cell[0] - 1;
            open[size][1] = cell[1];
        }
        if(b){
            size--;
            open[size][0] = cell[0];
            open[size][1] = cell[1] + 1;
        }
        if(a){
            size--;
            open[size][0] = cell[0] + 1;
            open[size][1] = cell[1];
        }
    return open;
    }

private void AddToOpen(int[] n, HashMap<int[], Integer> fScore, ArrayList openSet) {

    boolean found = false;
    int index = 0;

    if(openSet.size() <= 0){
        openSet.add(n);
        return;
    }

    if(fScore.get(openSet.get(openSet.size() - 1)).compareTo(fScore.get(n)) <= 0){
        openSet.add(openSet.size() - 1, n);
        return;
    }

    while(true){
        int[] d = (int[])openSet.get(index);
        if(fScore.get(d).compareTo(fScore.get(n)) > 0){
            openSet.add(n);
            return;
        }
    }


public int HCost(int[] start, int[] end){
    int cost = (int) Math.sqrt((end[0] - start[0])^2 + (end[1] - start[1])^2);
    return cost;
}

Here is the psuedocode:

 function A*(start,goal)
     closedset := the empty set    // The set of nodes already evaluated.
     openset := {start}    // The set of tentative nodes to be evaluated, initially containing the start node
     came_from := the empty map    // The map of navigated nodes.

     g_score[start] := 0    // Cost from start along best known path.
     // Estimated total cost from start to goal through y.
     f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal)

     while openset is not empty
         current := the node in openset having the lowest f_score[] value
         if current = goal
             return reconstruct_path(came_from, goal)

         remove current from openset
         add current to closedset
         for each neighbor in neighbor_nodes(current)
             tentative_g_score := g_score[current] + dist_between(current,neighbor)
             if neighbor in closedset
                 if tentative_g_score >= g_score[neighbor]
                     continue

             if neighbor not in openset or tentative_g_score < g_score[neighbor] 
                 came_from[neighbor] := current
                 g_score[neighbor] := tentative_g_score
                 f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
                 if neighbor not in openset
                     add neighbor to openset

     return failure

 function reconstruct_path(came_from, current_node)
     if current_node in came_from
         p := reconstruct_path(came_from, came_from[current_node])
         return (p + current_node)
     else
         return current_node

Any and all help is greatly apreciated.

Thanks.

[bguild]

On line 31 you are creating a new array for the reference n. On line 32, you are testing if n is contained in closedSet. Since the n array never existed before that moment, there is no way it could be in closedSet. That will surely prevent your algorithm from performing as intended.

[DarkLightning7]

So the contains method checks if it contains the same array not an equivalent array?

[jwenting]

yes, the contains() method checks whether a Collection contains a specific item, not an item that's just of the same type.

It's like putting marbles in a jar, each marked with a number. You check the numbers when retrieving marbles, not whether you got a marble.

[DarkLightning7]

Ok so i looked into the implementation of contains and it makes use of object.equals() which as you said only checks if the references point to the same object same as == does. Seems kind of pointless to have two ways to check if the object references are equal and no way to see if the contents are. Well i guess ill work on my new implementation of the search using connected nodes instead of a boolean array. Should work out much better in terms of object comparisins and array lists.

Thanks for the help jwenting and bguild.

[bguild]

You can use java.util.Arrays.equals to check if the contents of two arrays are equal.

Like many methods, Object.equals is a little fuzzy in exactly what it does. The meaning of equals isn't exactly the same for each class. There are rules that all classes are supposed to follow, but they are quite loose.

Like most operators, == does exactly one very specific thing, so you always know precisely what you are getting when you use it. Clearly it is not always the same as Object.equals. For example, java.util.Collection.equals tests that two collections contain the same elements by testing each element by equals, while the == operator would tell you if two references pointed to the same collection object, which is a big distinction when you call methods that modify collections like add.

[DarkLightning7]

I was talking about the object class not the general idea of (some object type).equals() as each object type can redefine the equals() method which, as you said, makes for a large varity of different implementations. Oh and sorry forgot to mark this thread as solved :) Thanks again.