A* pathfinder gives Sub-optimal Path

Question

I wrote a pathfinder in java and in most cases, it works pretty well. However, i found a scenario in which it goes wrong. The heuristic i use is supposed to be consistent, and a consistent heuristic means that the algorithm should always find the closest route to the nodes it expands, as far as i know.

Here is the picture of the issue:

The start node is green, the numbers just represent the length of the path from each particular node to the goal represented in red.

My heuristic class:

package heuristics;
import pathcomponents.Direction;


public class Heuristics {

    public static int DO(int x1, int y1, int x2, int y2) {
        int dx = Math.abs(x1 - x2);
        int dy = Math.abs(y1 - y2);
        int D, O;
        if(dx > dy) {
            D = dy;
            O = dx - D;
        }
        else {
            D = dx;
            O = dy - D;
        }
        return D*Direction.D_COST + O*Direction.O_COST;
    }

}

Direction.D_COST = 14, Direction.O_COST = 10

The heuristic returns the following value: diagonal distance*14 + orthogonal distance*10.

The algorithm:

package pathfinders;


import java.util.LinkedList;
import pathcomponents.Direction;
import pathcomponents.Node;
import pathcomponents.Path;
import heuristics.Heuristics;


public class ProxyAStar {

    static private boolean contains(LinkedList<Node> list, int x, int y) {
        for(Node n : list)
            if(n.getX() == x && n.getY() == y) return true;
        return false;
    }

    public static Path buildPath(Node lcnode) {
        int cost = lcnode.getG();
        LinkedList<Direction> path = new LinkedList<Direction>();
        while(lcnode != lcnode.getParent()) {
            int dx = lcnode.getX() - lcnode.getParent().getX();
            int dy = lcnode.getY() - lcnode.getParent().getY();
            path.add(new Direction(dx, dy));
            lcnode = lcnode.getParent();
        }
        return new Path(path, cost);
    }

    public static Path search(boolean[][] map, int sx, int sy, int gx, int gy) {
        LinkedList<Node> openList   = new LinkedList<Node>();
        LinkedList<Node> closedList = new LinkedList<Node>();
        openList.add(new Node(sx, sy, 0, Heuristics.DO(sx, sy, gx, gy), null));
        while(!openList.isEmpty()) {
            Node lcnode = openList.peekFirst();
            for(Node n : openList) {
                if(n.getCost() < lcnode.getCost()) {
                    lcnode = n;
                }
            }
            int x = lcnode.getX();
            int y = lcnode.getY();
            if(x == gx && y == gy) {
                return buildPath(lcnode);
            }
            closedList.add (lcnode);
            openList.remove(lcnode);
            for(int i = -1; i <= 1; ++i) {
                for(int j = -1; j <= 1; ++j) {
                    int cx = x + i;
                    int cy = y + j;
                    if((i == 0 && j == 0) || map[cx][cy] == false)continue;
                    if(!contains(openList,cx,cy) && !contains(closedList,cx,cy)){
                        openList.add(new Node(cx, cy, lcnode.getG() + Heuristics.DO(x, y, cx, cy), Heuristics.DO(cx, cy, gx, gy), lcnode));
                    }
                }
            }
        }
        Node lcnode = closedList.peekFirst();
        for(Node n : closedList) {
            if(n.getH() < lcnode.getH()) {
                lcnode = n;
            }
        }
        openList   = null;
        closedList = null;
        return search(map, sx, sy, lcnode.getX(), lcnode.getY());
    }
}

Class Node has the usual G, H and F costs and a parent reference. When the constructor receives null as the parent parameter, it becomes its own parent. Thats why the path building loop stops when the condition "lcnode == lcnode.getParent()" is met in the buildPath function, since the first node expanded is parent to itself. The path is composed of Direction pieces, which consist of an x and y coordinate, each being either -1, 0, or 1. The reason for this is that i wanted the path to lead to the goal by relative coordinates. There is no map border checking, this is intentional. I substitute this by making the border nodes unwalkable.

Another picture:

This time it works out well. The difference has to do with the order i expand nodes around the last colsed node, since i search for the lowest cost closed node like this:

for(Node n : openList) {
    if(n.getCost() < lcnode.getCost()) {
        lcnode = n;
    }
}

If i change the inequality to "<=", the issue in the first picture is fixed and the second one gets messed up.

On a side note, I extended the A* a little so that if there is no path, it gets the lowest H cost node from the closed list and runs another search targeting that node. This way, it gets in proximity of the goal even if there is currently no path to the goal node.

I haven't included every class, I think the others are unrelated to this problem. If something is unclear i will inculde them, didn't want to make the question too long to read though.

As far as i know, theory dictates that a consistent heuristic guarantees node expansion with optimal cost, so i couldn't figure out yet how to fix the inaccuracy. Did i make any mistakes in the code? If not, how can i fix the problem?

---

EDIT: I included some of the missing code to make stuff clear:

class Direction:

package pathcomponents;
public class Direction {
    public static final int D_COST = 14;
    public static final int O_COST = 10;
    public static int signum(int n) {
        return (n < 0) ? -1 : ((n == 0) ? 0 : 1);
    }
    private final int x, y;
    public Direction(int x, int y) {
        this.x = signum(x);
        this.y = signum(y);
    }
    public Direction(Direction source) {
        this.x = source.x;
        this.y = source.y;
    }
    public int getX() {return x;}
    public int getY() {return y;}
}

class Node:

package pathcomponents;
public class Node {
    private final int    x, y;
    private       int       G;
    private final int       H;
    private       int       F;
    private       Node parent;
    public Node(int x, int y, int G, int H, Node parent) {
        this.x      =                                x;
        this.y      =                                y;
        this.G      =                                G;
        this.H      =                                H;
        this.F      =                            G + H;
        this.parent = (parent == null) ? this : parent;
    }
    public int  getX()      {return      x;}
    public int  getY()      {return      y;}
    public int  getG()      {return      G;}
    public int  getH()      {return      H;}
    public int  getCost()   {return      F;}
    public Node getParent() {return parent;}
    public void setParent(Node parent, int G) {
        this.parent = parent;
        this.G      =      G;
        F           = G +  H; 
    }
}

Answer 1

Here is the pseudo-code from wikipedia for A* with a consistent-heuristic:

1. while openset is not empty
2.     current := the node in openset having the lowest f_score[] value
3.     if current = goal
4.        return reconstruct_path(came_from, goal)
5. 
6.     remove current from openset
7.     add current to closedset
8.     for each neighbor in neighbor_nodes(current)
9.        tentative_g_score := g_score[current] + dist_between(current,neighbor)
10.       if neighbor in closedset and tentative_g_score >= g_score[neighbor]
11.          continue
12.
13.       if neighbor not in openset or tentative_g_score < g_score[neighbor] 
14.          came_from[neighbor] := current
15.          g_score[neighbor] := tentative_g_score
16.          f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
17.          if neighbor not in openset
18.             add neighbor to openset
19. 
20. return failure

The important line is 13 - if a neighbor of the current-node is already in the openset, you may need to update its g-score and parent-node.

This is true for both consistent and non-consistent heuristics. What a consistent heuristic gives you is not the ability to skip this check, but the ability to not have to re-queue nodes that have already been expanded (ie. that are in the closed-set).

---

LinkedList<Node> openList   = new LinkedList<Node>();

openList should be a PriorityQueue ordered by g(x) + h(x). You will get much better performance that way.

On a side note, I extended the A* a little so that if there is no path, it gets the lowest H cost node from the closed list and runs another search targeting that node.

There's no need to run a second search; see Tweaking AStar to find closest location to unreachable destination.

Answer 2

Are you sure your heuristic is admissible? I did find the code:

if(dx > dy) {
    D = dy;
    O = dx - D;
} else {
    D = dx;
    O = dy - D;
}
return D*Direction.D_COST + O*Direction.O_COST;

surprising.

In particular, let's suppose dx = 100, dy = 1. Then the real distance is 1000.05 (considering, as you did, each square with 10 length units) and your heuristic would estimate 1004, that is, it is not admissible (considering that you want to get the shortest path).