A* Pathfinding really slow

Question

I have an A* pathfinding algorithm that I've used for a Spigot plugin which worked fine. I then added a requirements system so it won't try and pathfind through places it shouldn't. Now it seems REALLY slow, and it looks like it has nothing to do with the requirements code itself, and more to do with the algorithm having many more incorrect paths when calculating. I seem to be getting 1500ms+ on this, which is definitely not good xD

Here is the pathfinder code:

    public Path calculate(PathfinderGoal goal, PathScorer scorer, List<PathRequirement> requirements, int maxNodes) {
        PathNode start = toNode(npc.getLocation());
        PathNode end = toNode(goal.getLocation());

        List<PathNode> open = new ArrayList<>() {{ add(start); }};
        List<PathNode> navigated = new ArrayList<>();
        start.setF(scorer.computeCost(start, end));

        Timer timer = new Timer().start();

        while (!open.isEmpty()) {
            PathNode current = null;
            for (PathNode node : open) {
                if (current == null || node.getH() < current.getH()) {
                    current = node;
                }
            }
            if (scorer.computeCost(current, end) < 1 || (navigated.size() >= maxNodes && maxNodes != -1)) {
                navigated.add(navigated.size() < maxNodes ? end : current);
                return reconstruct(navigated, navigated.size() - 1);
            }
            open.remove(current);
            current.close();
            for (PathNode node : current.getNeighbors()) {
                if (node.isClosed()) {
                    continue;
                }
                double tentG = current.getG() + scorer.computeCost(current, node);
                if (!open.contains(node) || tentG < node.getG()) {
                    boolean requirementsMet = true;
                    for (PathRequirement requirement : requirements) {
                        requirement.setNavigated(navigated);
                        if (!navigated.isEmpty() && !requirement.canMoveToNewNode(navigated.get(navigated.size() - 1), node)) {
                            requirementsMet = false;
                            break;
                        }
                    }
                    if (!navigated.contains(current)) {
                        navigated.add(current);
                    }
                    node.setG(tentG);
                    node.setH(scorer.computeCost(node, end));
                    node.setF(tentG + node.getH());
                    if (!open.contains(node) && requirementsMet) {
                        open.add(node);
                    }
                }
            }
            Bukkit.broadcastMessage("Open Set Size: " + open.size());
            Bukkit.broadcastMessage(timer.stop() + "ms");
        }
        return null;
    }

    private Path reconstruct(List<PathNode> navigated, int index) {
        final PathNode current = navigated.get(index);
        Path withCurrent = new Path(new ArrayList<>() {{ add(current); }});
        if (index > 0 && navigated.contains(current)) {
            return reconstruct(navigated, index - 1).append(withCurrent);
        }
        return withCurrent;
    }

And here is the PathNode class:

    public PathNode(Pathfinder pathfinder, int x, int y, int z) {
        this.pathfinder = pathfinder;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    @Override
    public boolean equals(Object other) {
        if (!(other instanceof PathNode otherNode)) {
            return false;
        }
        return otherNode.x == x && otherNode.y == y && otherNode.z == z;
    }

    public List<PathNode> getNeighbors() {
        return new ArrayList<>() {
            {
                for (int x = -1; x <= 1; x++) {
                    for (int y = -1; y <= 1; y++) {
                        for (int z = -1; z <= 1; z++) {
                            add(new PathNode(pathfinder, PathNode.this.x + x, PathNode.this.y + y, PathNode.this.z + z));
                        }
                    }
                }
            }
        };
    }

    public Location getLocation() {
        return new Location(pathfinder.getNPC().getLocation().getWorld(), x, y, z);
    }

    public double getF() {
        return F;
    }

    public void setF(double f) {
        this.F = f;
    }

    public double getG() {
        return G;
    }

    public void setG(double g) {
        this.G = g;
    }

    public double getH() {
        return H;
    }

    public void setH(double h) {
        this.H = h;
    }

    public boolean isClosed() {
        return closed;
    }

    public void close() {
        this.closed = true;
    }

Valid Requirements Class:

public class ValidPathRequirement extends PathRequirement {
    @Override
    public boolean canMoveToNewNode(PathNode from, PathNode to) {
        Block fromBlock = from.getLocation().getBlock();
        Block toBlock = to.getLocation().getBlock();
        boolean validHeight = toBlock.getType().isAir() && toBlock.getRelative(BlockFace.UP).getType().isAir(); // checks if is player height
        boolean validGround = toBlock.getRelative(BlockFace.DOWN).getType().isSolid(); // is there a block underneath that they can stand on?
        boolean validFromPrev = toBlock.getLocation().subtract(fromBlock.getLocation()).getY() <= 1; // is it max one block higher than the last one?

        // is this one causing issues?
        Location fromLocDist = from.getLocation().clone();
        Location toLocDist = to.getLocation().clone();
        toLocDist.setY(fromLocDist.getY());
        boolean validDistance = fromLocDist.distance(toLocDist) <= 1;

        return validHeight && validGround && validFromPrev;
    }
}

Answer 1

Without looking at the rest of the algorithm, the first thing that stands out is that your data structures are incorrect. The "open" list needs to be a Priority Queue, and "closed" (or "navigated") should be a set.