Не работает алгоритм A*

Метод получает на вход координаты начальной точки и конечной.

heuristic( int dx , int dy ) { return dx + dy }.

getNeighbors возвращает соседние существующие и проходимые узлы.

findMinF возвращает узел из списка с наименьшим значением f.

backtrace находит маршрут от последнего узла к первому, преобразует его в массив с точками [x, y].

INode имеет поля :double f, double g, double h, int x, int y, boolean walkable, boolean opened, boolean closed,INode parent.

    public int[][] findPath( int startX, int startY, int endX, int endY) {
    ArrayList<INode> openList = new ArrayList<INode>();
    INode startNode = grid.getNode(startX, startY);
    double SQRT2 = Math.sqrt(2);
    INode node, neighbor;
    INode[] neighbors;
    int x, y;
    double ng;
    startNode.setG(0);
    startNode.setH( heuristic( Math.abs(startX - endX), Math.abs(startY - endY) ) );
    openList.add(startNode);
    startNode.open();
    while ( !openList.isEmpty() ) {
        node = findMinF( openList );
        openList.remove(node);
        node.close();
        if ( node.getX() == endX && node.getY() == endY ) {
            //Success
            return Util.backtrace(node);
        }
        neighbors = grid.getNeighbors( node.getX(), node.getY() );
        for ( int i = 0; i < neighbors.length; i++ ) {
            neighbor = neighbors[i];
            if ( neighbor.isClosed() ) {
                continue;
            }
            x = neighbor.getX();
            y = neighbor.getY();
            ng = node.getG() + ( ((x - node.getX())&(y - node.getY())) == 0 ? 1.0 : SQRT2);
            if ( !neighbor.isOpened() || ng < neighbor.getG() ) {
                neighbor.setG( ng );
                neighbor.setH( Weight * heuristic(Math.abs(x - endX), Math.abs(y - endY)) );
                neighbor.setParent( node );
            }
            if ( !neighbor.isOpened() ) {
                openList.add(neighbor);
                neighbor.open();
            }
        }

    }
    //fail 
    System.out.println("fail");
    return new int[0][0];}

Но при любых входных параметрах ( за исключением совпадающих и соседних узлов ) он указывает путь раз в 6-8 больше кратчайшего, значения f при этом достигают 500-700, при размере карты 32*32( при пустой карте). Может быть где-нибудь ошибка логики?

p.s. Причем если не прибавлять к ng node.getG() , то алгоритм работает на прямое движение, но диагональное тогда не работает вовсе.