Навигатор A * дает неоптимальный путь

Я написал указатель пути на Java, и в большинстве случаев он работает очень хорошо. Однако я нашел сценарий, в котором все идет не так. Насколько мне известно, эвристика, которую я использую, должна быть последовательной, а последовательная эвристика означает, что алгоритм всегда должен находить ближайший маршрут к узлам, которые он расширяет.

Вот картина проблемы:

Начальный узел зеленый, цифры просто представляют длину пути от каждого конкретного узла до цели, обозначенной красным.

Мой эвристический класс:

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;
    }

}

Направление.D_COST = 14, Направление.O_COST = 10

Эвристика возвращает следующее значение: диагональное расстояние*14 + ортогональное расстояние*10.

Алгоритм:

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());
    }
}

Класс Node имеет обычные стоимости G, H и F и родительскую ссылку. Когда конструктор получает null в качестве родительского параметра, он становится своим собственным родителем. Вот почему цикл построения пути останавливается, когда в функции buildPath выполняется условие «lcnode == lcnode.getParent()», поскольку первый расширенный узел является родителем самого себя. Путь состоит из частей направления, которые состоят из координат x и y, каждая из которых равна -1, 0 или 1. Причина этого в том, что я хотел, чтобы путь вел к цели по относительным координатам. Нет проверки границ карты, это сделано намеренно. Я заменил это, сделав граничные узлы непроходимыми.

Другая картинка:

На этот раз получается хорошо. Разница связана с порядком, в котором я расширяю узлы вокруг последнего закрытого узла, поскольку я ищу закрытый узел с наименьшей стоимостью следующим образом:

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

Если я изменю неравенство на «‹=», проблема на первом изображении будет устранена, а на втором испорчено.

Кстати, я немного расширил A*, чтобы, если пути нет, он получал узел с наименьшей стоимостью H из закрытого списка и запускал другой поиск, нацеленный на этот узел. Таким образом, он приближается к цели, даже если в настоящее время нет пути к целевому узлу.

Я не включил все классы, я думаю, что другие не имеют отношения к этой проблеме. Если что-то неясно, я их поддержу, хотя не хотел делать вопрос слишком длинным для чтения.

Насколько я знаю, теория диктует, что последовательная эвристика гарантирует расширение узла с оптимальной стоимостью, поэтому я пока не мог понять, как исправить неточность. Я сделал какие-либо ошибки в коде? Если нет, то как я могу исправить проблему?

РЕДАКТИРОВАТЬ: я включил часть недостающего кода, чтобы прояснить ситуацию:

Класс Направление:

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;}
}

узел класса:

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; 
    }
}