help/AStarPathfinder.java

package xyz.nodrop.farmingtools.client.pathfinding;

import net.minecraft.block.BlockState;
import net.minecraft.client.world.ClientWorld;
import net.minecraft.util.math.BlockPos;
import net.minecraft.block.*;
import net.minecraft.registry.tag.BlockTags;

import java.util.*;

public class AStarPathfinder {

    private static final int MAX_NODES = 100_000;
    private static final int MAX_FALL  = 3;

    private static final int[][] DIRS = {
            {1,0}, {-1,0}, {0,1}, {0,-1},
            {1,1}, {1,-1}, {-1,1}, {-1,-1}
    };
    private static final double SQRT2 = 1.41421356;

    public static List<BlockPos> find(ClientWorld world, BlockPos start, BlockPos goal, boolean flying) {
        PriorityQueue<PathNode> open = new PriorityQueue<>();
        Map<BlockPos, Double> gScore = new HashMap<>();

        open.add(new PathNode(start, null, 0, heuristic(start, goal)));
        gScore.put(start, 0.0);

        int expanded = 0;
        while (!open.isEmpty() && expanded < MAX_NODES) {
            PathNode current = open.poll();
            expanded++;

            if (current.pos.equals(goal)) {
                List<BlockPos> raw = reconstruct(current);
                return smooth(world, raw, flying);
            }

            for (PathNode nb : neighbours(world, current, goal, flying)) {
                double existing = gScore.getOrDefault(nb.pos, Double.MAX_VALUE);
                if (nb.g < existing) {
                    gScore.put(nb.pos, nb.g);
                    open.add(nb);
                }
            }
        }
        return Collections.emptyList();
    }

    private static List<BlockPos> smooth(ClientWorld world, List<BlockPos> raw, boolean flying) {
        if (raw.size() <= 2) return raw;

        if (flying) return smoothFly(world, raw);
        else        return smoothWalk(world, raw);
    }

    // ── Smoothing dla chodzenia (stary kod, tylko przemianowany) ──────────────
    private static List<BlockPos> smoothWalk(ClientWorld world, List<BlockPos> raw) {
        List<BlockPos> smoothed = new ArrayList<>();
        smoothed.add(raw.get(0));

        int anchor = 0;
        int i = 2;

        while (i < raw.size()) {
            BlockPos from = raw.get(anchor);
            BlockPos to   = raw.get(i);
            boolean yChanged = raw.get(i).getY() != raw.get(i - 1).getY();

            if (yChanged || !hasLineOfWalk(world, from, to)) {
                smoothed.add(raw.get(i - 1));
                anchor = i - 1;
            }
            i++;
        }

        smoothed.add(raw.get(raw.size() - 1));
        return smoothed;
    }

    // ── Smoothing dla lotu — ray cast 3D ─────────────────────────────────────
    private static List<BlockPos> smoothFly(ClientWorld world, List<BlockPos> raw) {
        List<BlockPos> smoothed = new ArrayList<>();
        smoothed.add(raw.get(0));

        int anchor = 0;
        int i = 2;

        while (i < raw.size()) {
            BlockPos from = raw.get(anchor);
            BlockPos to   = raw.get(i);

            if (!hasLineOfFlight(world, from, to)) {
                smoothed.add(raw.get(i - 1));
                anchor = i - 1;
            }
            i++;
        }

        smoothed.add(raw.get(raw.size() - 1));
        return smoothed;
    }

    // ── Ray cast 3D dla lotu (Bresenham 3D) ──────────────────────────────────
    private static boolean hasLineOfFlight(ClientWorld world, BlockPos from, BlockPos to) {
        int x0 = from.getX(), y0 = from.getY(), z0 = from.getZ();
        int x1 = to.getX(),   y1 = to.getY(),   z1 = to.getZ();

        int dx = Math.abs(x1 - x0), dy = Math.abs(y1 - y0), dz = Math.abs(z1 - z0);
        int sx = x0 < x1 ? 1 : -1;
        int sy = y0 < y1 ? 1 : -1;
        int sz = z0 < z1 ? 1 : -1;

        int cx = x0, cy = y0, cz = z0;

        // Błędy dla Bresenhama 3D
        int errXY = dx - dy;
        int errXZ = dx - dz;

        int steps = Math.max(dx, Math.max(dy, dz));

        for (int i = 0; i < steps; i++) {
            BlockPos pos = new BlockPos(cx, cy, cz);
            // Sprawdź czy gracz (2 bloki wysokości) przejdzie
            if (!isPassable(world, pos) || !isPassable(world, pos.up())) return false;

            int e2XY = 2 * errXY;
            int e2XZ = 2 * errXZ;

            if (e2XY > -dy) { errXY -= dy; cx += sx; }
            if (e2XY <  dx) { errXY += dx; cy += sy; }
            if (e2XZ > -dz) { errXZ -= dz; }
            if (e2XZ <  dx) { errXZ += dx; cz += sz; }
        }
        return true;
    }

    private static boolean hasLineOfWalk(ClientWorld world, BlockPos from, BlockPos to) {
        if (from.getY() != to.getY()) return false;

        int x0 = from.getX(), z0 = from.getZ();
        int x1 = to.getX(),   z1 = to.getZ();
        int y  = from.getY();

        int dx = Math.abs(x1 - x0), dz = Math.abs(z1 - z0);
        int sx = x0 < x1 ? 1 : -1,  sz = z0 < z1 ? 1 : -1;
        int err = dx - dz;

        int cx = x0, cz = z0;
        while (true) {
            if (cx == x1 && cz == z1) break;

            BlockPos pos = new BlockPos(cx, y, cz);
            if (!canPass(world, pos) || !isSolid(world, pos.down())) return false;

            int e2 = 2 * err;
            if (e2 > -dz) { err -= dz; cx += sx; }
            if (e2 <  dx) { err += dx; cz += sz; }
        }
        return true;
    }

    private static List<PathNode> neighbours(ClientWorld world, PathNode current, BlockPos goal, boolean flying) {
        List<PathNode> result = new ArrayList<>();
        BlockPos pos = current.pos;

        if (flying) {
            // ── Tryb lotu — ruch we wszystkich 26 kierunkach ──────────────────
            for (int dx = -1; dx <= 1; dx++) {
                for (int dy = -1; dy <= 1; dy++) {
                    for (int dz = -1; dz <= 1; dz++) {
                        if (dx == 0 && dy == 0 && dz == 0) continue;
                        BlockPos nb = pos.add(dx, dy, dz);
                        if (isPassable(world, nb) && isPassable(world, nb.up())) {
                            double cost = Math.sqrt(dx*dx + dy*dy + dz*dz);
                            add(result, nb, current, goal, cost);
                        }
                    }
                }
            }
            return result;
        }

        for (int[] d : DIRS) {
            int dx = d[0], dz = d[1];
            boolean diagonal = (dx != 0 && dz != 0);
            double baseCost = diagonal ? SQRT2 : 1.0;

            int nx = pos.getX() + dx;
            int nz = pos.getZ() + dz;

            if (diagonal) {
                if (!canPass(world, new BlockPos(pos.getX() + dx, pos.getY(), pos.getZ()))) continue;
                if (!canPass(world, new BlockPos(pos.getX(), pos.getY(), pos.getZ() + dz))) continue;
            }

            BlockPos flat = new BlockPos(nx, pos.getY(), nz);
            if (canStand(world, flat)) add(result, flat, current, goal, baseCost);

            BlockPos up = new BlockPos(nx, pos.getY() + 1, nz);
            if (canStand(world, up) && isPassable(world, pos.up()))
                add(result, up, current, goal, baseCost + 0.5);

            for (int drop = 1; drop <= MAX_FALL; drop++) {
                BlockPos fell = new BlockPos(nx, pos.getY() - drop, nz);
                if (canStand(world, fell)) {
                    add(result, fell, current, goal, baseCost + drop * 0.5);
                    break;
                }
                if (!isPassable(world, new BlockPos(nx, pos.getY() - drop, nz))) break;
            }
        }

        for (int drop = 1; drop <= MAX_FALL; drop++) {
            BlockPos fell = pos.down(drop);
            if (canStand(world, fell)) {
                add(result, fell, current, goal, drop * 0.5);
                break;
            }
            if (!isPassable(world, fell)) break;
        }

        return result;
    }

    private static void add(List<PathNode> list, BlockPos pos,
                            PathNode parent, BlockPos goal, double cost) {
        list.add(new PathNode(pos, parent, parent.g + cost, heuristic(pos, goal)));
    }

    private static boolean canStand(ClientWorld world, BlockPos feet) {
        return isSolid(world, feet.down())
                && isPassable(world, feet)
                && isPassable(world, feet.up());
    }

    public static boolean canPass(ClientWorld world, BlockPos pos) {
        return isPassable(world, pos) && isPassable(world, pos.up());
    }

    private static final Set<Block> BLOCKED_BLOCKS = Set.of(
            Blocks.COBWEB,
            Blocks.SWEET_BERRY_BUSH,
            Blocks.BAMBOO,
            Blocks.BAMBOO_SAPLING,
            Blocks.CACTUS,
            Blocks.POWDER_SNOW
    );

    private static boolean isPassable(ClientWorld world, BlockPos pos) {
        BlockState state = world.getBlockState(pos);
        Block block = state.getBlock();

        // Block blocks from tags
        if (state.isIn(BlockTags.LEAVES))   return false;
        if (state.isIn(BlockTags.FENCES))   return false;
        if (state.isIn(BlockTags.WALLS))    return false;

        // Block blocks from list
        if (BLOCKED_BLOCKS.contains(block)) return false;

        return !state.isSolidBlock(world, pos);
    }

    private static boolean isSolid(ClientWorld world, BlockPos pos) {
        BlockState state = world.getBlockState(pos);
        return state.isSolidBlock(world, pos);
    }

    private static double heuristic(BlockPos a, BlockPos b) {
        double dx = a.getX() - b.getX();
        double dy = a.getY() - b.getY();
        double dz = a.getZ() - b.getZ();
        return Math.sqrt(dx * dx + dy * dy + dz * dz);
    }

    private static List<BlockPos> reconstruct(PathNode node) {
        LinkedList<BlockPos> path = new LinkedList<>();
        for (PathNode n = node; n != null; n = n.parent) path.addFirst(n.pos);
        return path;
    }
}