A star algorithm not working

Question

I implemented this algorithm based on the pseudo code on the Wikipedia page link to page for A*, but it is not finding any paths. When I use it I never get to a point where the current node equals the goal. I think it might have something to do with my heuristic or the way that I am initializing the f and g scores, but I just can't seem to figure it out.

The size of the map I'm using is 1920 x 1080 and uses a cell size of 30.

private ArrayList<Vector2> aStar(Vector2 start, Vector2 goal) {
    HashSet<Vector2> closedSet = new HashSet<Vector2>();
    ArrayList<Vector2> openSet = new ArrayList<Vector2>();
    openSet.add(start);
    HashMap<Vector2,Vector2> cameFrom = new HashMap<Vector2, Vector2>();
    HashMap<Vector2,Float> gScore = new HashMap<Vector2, Float>();
    HashMap<Vector2,Float> fScore = new HashMap<Vector2, Float>();
    ArrayList<Vector2> neighbors = new ArrayList<Vector2>();
    neighbors.add(new Vector2(0,30));
    neighbors.add(new Vector2(0,-30));
    neighbors.add(new Vector2(30,0));
    neighbors.add(new Vector2(-30,0));
    for(int i = 0; i < 1920; i +=30){
        for(int j = 0; j < 1080; j +=30){
            gScore.put(new Vector2(i,j),Float.MAX_VALUE);
            fScore.put(new Vector2(i,j),Float.MAX_VALUE);
        }
    }
    gScore.put(start,0f);
    fScore.put(start,heuristic(start,goal));

    while(!openSet.isEmpty()){
        int low = 0;
        for(int i = 0; i < openSet.size(); i++){
            if(fScore.get(openSet.get(i))<fScore.get(openSet.get(low))){
                low = i;
            }
        }
        Vector2 current = openSet.get(low);
        if(current.equals(goal)){
            System.out.println("YES!");
            return null;
        }
        openSet.remove(current);
        closedSet.add(current);
        for(Vector2 neighbor:neighbors){
            Vector2 tst = new Vector2(neighbor.x + current.x,neighbor.y + current.y);
            if(tst.x > -30 && tst.x <1920 && tst.y > -30 && tst.y < 1080){
                neighbor.add(current);
                if(closedSet.contains(neighbor)){
                    continue;
                }
                if(!openSet.contains(neighbor)){
                    openSet.add(neighbor);
                }

                float tentative_gScore = gScore.get(current) + heuristic(current,neighbor);
                if(tentative_gScore >= gScore.get(neighbor)){
                    continue;
                }

                cameFrom.put(neighbor,current);
                gScore.put(neighbor,tentative_gScore);
                fScore.put(neighbor,gScore.get(neighbor)+heuristic(neighbor,goal));
            }
        }

    }

    return null;
}

private float heuristic(Vector2 begin, Vector2 end) {
    return  (Math.abs(begin.x - end.x) + Math.abs(begin.y - end.y)) ;
}

Answer 1

The line neighbor.add(current); changes the neighbour. Not only that, but you store that neighbour, meaning that this object reference will be kept and may be further distorted in future code.

Another problem is with your heuristic. You are using the Manhattan distance, meaning that there are a massive amount of paths that all have the same distance (so long as you never move away from the goal node, the distance will the same for each path). Try allowing diagonal neighbours, and using the euclidian distance instead of the manhattan distance.