A* Algorithm Bug

Question

I tried creating the A* algorithm in java and I have this strange bug. I know the A* doesn't always find the best path, but here it seems to go against reason and choose a worse path, and I can't find the bug in the code causing this. It seems to find the optimal solution on other maps I created. Here is a picture of the bug, and a printout of the nodes

http://i.imgur.com/IudT7.png

Here is (part of) the code that I used.

import java.util.Iterator;
import java.util.LinkedList;
import java.util.PriorityQueue;
import java.util.Queue;

// Matt Bellis 7/31/2011
// A* Algorithm

public class AStar {
private PriorityQueue<Node> openQ;
private Queue<Node> closedQ;
private int [][] map;
private int startX, startY, endX, endY;
private Node endNode;

public AStar(int[][]map, int startX, int startY, int endX, int endY) {
    openQ = new PriorityQueue<Node>();
    closedQ = new LinkedList<Node>();
    this.map = map;
    this.startX = startX;
    this.startY = startY;
    this.endX = endX;
    this.endY = endY;
    endNode = new Node(endX, endY, 0, null); // for calculating the manhattan distances later
}

private int manhattanDist(Node curr, Node target) {
    int cX, tX, cY, tY;
    cX = curr.getX();
    tX = target.getX();
    cY = curr.getY();
    tY = target.getY();
    return 10*(Math.abs(cX - tX)+Math.abs(cY - tY));
}
private boolean onClosedList(Node node) {
    if(closedQ.isEmpty() == true)
        return false;
    Iterator<Node> it = closedQ.iterator();
    while(it.hasNext()) {
        Node nodeCheck = it.next();
        if(nodeCheck.getX() == node.getX() && nodeCheck.getY() == node.getY())
            return true;
    }
    return false;
}
private boolean checkAndReplaceOpen(Node node, Node curr, boolean diag) { // true means replaced
    Iterator<Node> it = openQ.iterator();
    while(it.hasNext()) {
        Node nodeCheck = it.next();
        if(nodeCheck.getX() == node.getX() && nodeCheck.getY() == node.getY()) {
            // they are the same node, check to see the g cost
            if(node.getG() < nodeCheck.getG()) { //entered node is better path
                if(diag == true)
                    node.setG(curr.getG()+14);
                else
                    node.setG(curr.getG()+10);
                node.setF(node.getG() + node.getH());
                node.setParent(curr);
                return true;
            }
            return false;
        }   
    }
    return false;
}
private void addNeighbors(Node node) {
        int x = node.getX();
        int y = node.getY();
        //System.out.println("Node: "+node);
        //System.out.println("Right: "+map[y][x+1]);
        if((x+1)< map[y].length && map[y][x+1] !=1) {
            Node newNode = new Node(x+1, y, map[y][x+1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+10);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, false) == false) 
                    openQ.add(newNode);
            }
            //System.out.println("1Added Node: "+newNode);
        }
        //System.out.println("Left: Y:"+y+" X:"+(x-1));
        if((x-1) >= 0 && map[y][x-1] !=1 ) {
            Node newNode = new Node(x-1, y, map[y][x-1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+10);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, false) == false) 
                    openQ.add(newNode);
            }
            //System.out.println("2Added Node: "+newNode);
        }
        //System.out.println("Up: "+map[y+1][x]);
        if((y+1) < map.length && map[y+1][x] !=1) {
            Node newNode = new Node(x, y+1, map[y+1][x], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+10);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, false) == false) 
                    openQ.add(newNode);
            }
            //System.out.println("3Added Node: "+newNode);
        }
        //System.out.println("Down: "+map[y-1][x]);
        if((y-1) > 0 && map[y-1][x] !=1) {
            Node newNode = new Node(x, y-1, map[y-1][x], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+10);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, false) == false) 
                    openQ.add(newNode);
            }
            //System.out.println("4Added Node: "+newNode);
        }

        // ADDING IN DIAGONAL
        // top right
        if((y+1) < map.length && (x+1) < map[y].length && map[y+1][x+1] !=1) {
            Node newNode = new Node(x+1, y+1, map[y+1][x+1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+14);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, true) == false) 
                    openQ.add(newNode);
            }
        }
        // top left
        if((y+1) < map.length && (x-1) >= 0 && map[y+1][x-1] !=1) {
            Node newNode = new Node(x-1, y+1, map[y+1][x-1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+14);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, true) == false) 
                    openQ.add(newNode);
            }
        }
        // bottom left
        if((y-1) > 0 && (x-1) >= 0 && map[y-1][x-1] !=1) {
            Node newNode = new Node(x-1, y-1, map[y-1][x-1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+14);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, true) == false) 
                    openQ.add(newNode);
            }
        }
        // bottom right
        if((y-1) >= 0 && (x+1) < map[y].length && map[y-1][x+1] !=1) {
            Node newNode = new Node(x+1, y-1, map[y-1][x+1], node);
            if(onClosedList(newNode) == false) {
                newNode.setG(node.getG()+14);
                newNode.setH(manhattanDist(newNode, endNode));
                newNode.setF(newNode.getG()+newNode.getH());
                if(checkAndReplaceOpen(newNode, node, true) == false) 
                    openQ.add(newNode);
            }
        }
}
private Node solve() {
    Node startNode = new Node(startX, startY, 0, null);
    startNode.setH(manhattanDist(startNode, endNode));
    startNode.setF(startNode.getG() + startNode.getH());
    openQ.add(startNode);
    while(openQ.isEmpty() == false) { // keep going until the queue is empty, or you find the end node
        Node currNode = openQ.remove();
        closedQ.add(currNode);
        if(currNode.getX() == endX && currNode.getY() == endY) {
            return currNode;
        }
        addNeighbors(currNode);
    }
    System.out.println("No solution found!");
    return startNode; // bad!
}
public LinkedList<Node> algorithm() {
    LinkedList<Node> pathr = new LinkedList<Node>();
    LinkedList<Node> path = new LinkedList<Node>();
    Node addNode = solve();
    while(addNode.getParent() != null) {
        pathr.add(addNode);
        addNode = addNode.getParent();
    }
    pathr.add(addNode);
    while(pathr.isEmpty() == false) 
        path.add(pathr.removeLast());
    return path;
}
public void printList(LinkedList<Node> list) {
    Iterator<Node> it = list.iterator();
    while(it.hasNext())
        System.out.println(it.next());
}
 }

Answer 1

From your image, it looks like you're not actually calculating the correct manhattan distance. Never mind, your manhatten method looks good.

If you allow for diagonal movement, then the next to last move is just as optimal as walking straight to the goal.

(I'll admit that I haven't read the code to closely, though.. too tired)

Answer 2

For starters, as far as I know, A* always finds the best way...

I haven't looked over all of your code, but your manhatten function is concerning me. A* uses an "admissible heuristic", meaning the heuristic function may not overestimate the cost to travel to the target node. If it does, you are actually having an A algorithm, which may not find the best path.

To exclude that, let you manhatten function return 0, which is an admissible heuristic, because it doesn't overestimate. If your problem still occures with that, the problem is somewhere else.

If I see that correctly, your cost for a diagonal movement is 10, where the cost for any other movement is 14. Giving that, your manhattan function returns 10 for horizontal or vertical movement, which is fine (10 < 14) but 20 for diagonal movement, which is not fine (20 > 10). That might be the error.

Answer 3

A* actually gives best path always if one requirement is met:

h(x,s) <= G(x,s) foreach x where:

x - some point

h() - heurestic function

s - stop point

G() - "oracle" function that gives you shortest distance between points (which is of course not known)

the trick is, that as always as heurestic function gives distances that are shorter or equal to magical function G() path will be the shortest. The one problem I see with your code, is that if you are fe. 1 diagonal jump from endpoint, your heurestic function will return 20 (mahnattan distance), while your actual cost G() is 14 (this is the cost of 1 diagonal jump) in your code. I don't know if this is the only bug, need to study the code further, but try fixing it by:

• Changing 14 to 20, so that it matches heurestic
• Use different metric, fe. euclidean.