A* path finding algorithm. Movement Cost and Heuristic is Inaccurate

Question

My problem is that the movement cost(G cost) of my node and heuristic is inaccurate it does not match with the picture.

Here is the image of what I'm following.There are three labels here and the movement cost is labelled at the bottom left and the heuristic is at bottom right. Label at top-left is the F = H + G

Here is my output. As you can see the movement cost is not the same as the desired output. The red circle is the goal node.

Also the same with my Heuristic cost.

public class AStarPathFinder implements PathFinder {

private List<Node> open = new ArrayList<Node>();
private List<Node> close = new ArrayList<Node>();
private Node[][] nodes;

private TileMap map;
private Heuristic heuristic;

public AStarPathFinder(TiledMapStage mapStage, Heuristic heuristic) {
    this.heuristic = heuristic;
    nodes = mapStage.getNodes();
    map = mapStage.getMap();
}

@Override
public Path findPath(int startX, int startY, int goalX, int goalY) {
    clearNodes();

    Node goal = nodes[goalX][goalY];
    Node current = nodes[startX][startY];

    open.add(current);
    while (!open.isEmpty()) {

        current = getLowestFcost(open);
        open.remove(current);
        close.add(current);

        if (current == goal) {
            Path path = new Path();
            while (current != null) {
                path.add(current);
                current = current.parent;
            }
            return path;
        }
        // neighbors of current
        for (int x = -1; x < 2; x++) {
            for (int y = -1; y < 2; y++) {
                int dx = current.x + x;
                int dy = current.y + y;

                if (map.isValidLocation(dx, dy)) {
                    if (!map.isWalkable(nodes[dx][dy], x, y) || close.contains(nodes[dx][dy]))
                        continue;

                    float newScore = movementCost(current.g, isDiagonal(x, y));
                    if (!open.contains(nodes[dx][dy])) {
                        open.add(nodes[dx][dy]);
                    } else if (newScore >= nodes[dx][dy].g) continue;

                    nodes[dx][dy].g = newScore;
                    nodes[dx][dy].h = heuristic.estimate(nodes[dx][dy], goal);
                    nodes[dx][dy].f = nodes[dx][dy].g + nodes[dx][dy].h;
                    nodes[dx][dy].parent = current;
                    nodes[dx][dy].label.setText((int) nodes[dx][dy].g + "");
                }
            }
        }
    }
    return null;
}

private Node getLowestFcost(List<Node> open) {
    Node lowestNode = open.get(0);
    for (int i = 0; i < open.size(); i++) {
        if (open.get(i).f <= lowestNode.f && open.get(i).h < lowestNode.h) {
            lowestNode = open.get(i);
        }
    }
    return lowestNode;
}

private boolean isDiagonal(int x, int y) {
    return (x == -1 && y == 1 ||
            x == 1 && y == 1 ||
            x == 1 && y == -1 ||
            x == -1 && y == -1);
}

private float movementCost(float cost, boolean diagonal) {
    return diagonal ? cost + 14 : cost + 10;
}

@Override
public void clearNodes() {
    for (int i = 0; i < map.getTileWidth(); i++) {
        for (int j = 0; j < map.getTileHeight(); j++) {
            if (nodes[i][j].cell != null) {
                nodes[i][j].label.setText("");
                nodes[i][j].f = 0;
                nodes[i][j].h = 0;
                nodes[i][j].g = 0;
                nodes[i][j].arrow.setDrawable("cursor");
                nodes[i][j].arrow.setVisible(false);
                nodes[i][j].parent = null;
            }
        }
    }
    close.clear();
    open.clear();
}
 }

Here is the pseudocode that I'm following. Also my heuristic is a diagonal distance

Answer 1

It looks like your problem is in the isWalkable method of your TileMap map variable.

The image you're following doesn't allow to pass diagonally alongside a wall, where your algorithm does.

You can see this because the score gets added with 14 as follows: 14 + 14 = 28. While you expected it to go as follows: 14 + 10 (going down first) + 10 (going right) = 34.

I hope I explained your problem clearly. I don't know your implementation of isWalkable, so I can't provide a full solution but I hope I have pointed you in the right direction.