Pathfinding issues

Hey guys, I have been writing my own pathfinder today, and it should plot locations to get to the target. What I have found is that for some reason it never breaks, which should happen when the target is found (cancelling the 99999 loop). Does anyone know why the target is not found or why this is not working properly?

Code:

    public static Path calculate(Location start, Location target) {
        HashMap<Integer, int[]> locations = new HashMap<Integer, int[]>();
        World world = start.getWorld();
        Location current = start;
        for (int n = 0; n < 99999; n++) {
            int[] coordinates = new int[3];
            double H /*heuristic*/= 99999D;
            Location correct = null;
            for (int x = -1; x <= 1; x++) {
                for (int z = -1; z <= 1; z++) {
                    Location check = new Location(world, current.getX() + (double) x, current.getY(), current.getZ() + (double) z);
                    double newH = Math.sqrt(Math.pow((check.getX() - target.getX()), 2) + Math.pow((check.getZ() - target.getZ()), 2));
                    boolean allowsAndRequiresJump = world.getBlockAt(check.getBlockX(), check.getBlockY() + 1, check.getBlockZ()).isEmpty();
                    if (!check.getBlock().isEmpty())
                        newH += (allowsAndRequiresJump) ? 1D : 99999D;
                    if (newH <= H) {
                        H = newH;
                        correct = check;
                    }
                }
            }
            if (correct != null && H < 99998D) {
                coordinates[0] = correct.getBlockX();
                coordinates[1] = correct.getBlockY();
                coordinates[2] = correct.getBlockZ();
                locations.put(n, coordinates);
                System.out.println(n);//TODO remove
                current = correct;
            } else {
                coordinates[0] = current.getBlockX();
                coordinates[1] = current.getBlockY();
                coordinates[2] = current.getBlockZ();
                locations.put(n, coordinates);
            }
            if (target.getBlockX() == current.getBlockX() && target.getBlockY() == current.getBlockY() && target.getBlockZ() == current.getBlockZ()) {
                break;
            }
        }
        return new Path(start.getWorld(), locations);
    }

Don't worry about Path.java, it really does nothing but store the hashmap<integer, int[]>.

Thanks ahead of time!

 

Jnorr44 - you don't change the y values at all... are the locations you're targeting on the same y level as you?

 

Use a scheduler running every tick rather than a long loop. Also, you may want to consider having the path go within a radius of the target instead of right on it if the target is a mob or inaccessible.

Code:

BukkitTask bt = plugin.getServer().getScheduler().runTaskTimer(plugin, new Runnable() {
    @Override
    public void run() {
        // get coords
    }
}, 0, 1L);

Then, to cancel:

Code:

bt.cancel();