Upload files to "/"

AStarPathfinder.java

@@ -19,7 +19,8 @@ public class AStarPathfinder {
     };
     private static final double SQRT2 = 1.41421356;
 
-    public static List<BlockPos> find(ClientWorld world, BlockPos start, BlockPos goal, boolean flying) {
+
+    public static List<BlockPos> find(ClientWorld world, BlockPos start, BlockPos goal, boolean flying, java.util.Set<BlockPos> blacklistedNodes) {
         PriorityQueue<PathNode> open = new PriorityQueue<>();
         Map<BlockPos, Double> gScore = new HashMap<>();
 
@@ -36,7 +37,7 @@ public class AStarPathfinder {
                 return smooth(world, raw, flying);
             }
 
-            for (PathNode nb : neighbours(world, current, goal, flying)) {
+            for (PathNode nb : neighbours(world, current, goal, flying, blacklistedNodes)) {
                 double existing = gScore.getOrDefault(nb.pos, Double.MAX_VALUE);
                 if (nb.g < existing) {
                     gScore.put(nb.pos, nb.g);
@@ -120,16 +121,15 @@ public class AStarPathfinder {
         int steps = Math.max(dx, Math.max(dy, dz));
 
         for (int i = 0; i < steps; i++) {
-            BlockPos pos = new BlockPos(cx, cy, cz);
+            if (!isPassable(world, new BlockPos(cx, cy, cz))) return false;
-            // Sprawdź czy gracz (2 bloki wysokości) przejdzie
+            if (!isPassable(world, new BlockPos(cx, cy + 1, cz))) return false;
-            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 > -dz) { errXZ -= dz; cx += sx; }
             if (e2XZ <  dx) { errXZ += dx; cz += sz; }
         }
         return true;
@@ -160,9 +160,10 @@ public class AStarPathfinder {
         return true;
     }
 
-    private static List<PathNode> neighbours(ClientWorld world, PathNode current, BlockPos goal, boolean flying) {
+    private static List<PathNode> neighbours(ClientWorld world, PathNode current, BlockPos goal, boolean flying, java.util.Set<BlockPos> blacklist) {
         List<PathNode> result = new ArrayList<>();
         BlockPos pos = current.pos;
+        if (blacklist.contains(pos)) return result;
 
         if (flying) {
             // ── Tryb lotu — ruch we wszystkich 26 kierunkach ──────────────────

PathExecutor.java

@@ -8,40 +8,86 @@ import net.minecraft.world.RaycastContext;
 import net.minecraft.util.hit.BlockHitResult;
 import net.minecraft.util.hit.HitResult;
 
+import java.util.HashMap;
+import java.util.HashSet;
 import java.util.List;
+import java.util.Map;
+import java.util.Random;
+import java.util.Set;
 
 public class PathExecutor {
-    private static final double SPRINT_ANGLE = 35.0;   // max yaw diff to allow sprint
 
-    // ── Pola ──────────────────────────────────────────────────────────────────
+    // ══════════════════════════════════════════════════════════════════════════
-    private float yawVelocity     = 0f;
+    // STAŁE
-    private float pitchVelocity   = 0f;
+    // ══════════════════════════════════════════════════════════════════════════
-    private Vec3d smoothLookPoint = null;
-    private boolean flying = false;
 
-    // ── Stałe ─────────────────────────────────────────────────────────────────
+    private static final double SPRINT_ANGLE    = 35.0;   // max kąt yaw do sprintu
     private static final float  SPRING_STIFFNESS = 0.18f;
     private static final float  SPRING_DAMPING   = 0.72f;
     private static final float  MAX_VELOCITY     = 18.0f;
     private static final float  PITCH_LOOK_AHEAD = 6.0f;
     private static final float  LOOK_POINT_LERP  = 0.08f;
-    private volatile float prevYaw   = 0f;
+
-    private volatile float prevPitch = 0f;
+    // Lookahead
-    private volatile float nextYaw   = 0f;
+    private static final int   HOLD_TICKS     = 10;
-    private volatile float nextPitch = 0f;
+    private static final float NEAR_THRESHOLD = 3.5f;
+
+    // Stuck detection
+    private static final int   STUCK_TICKS    = 26;   // ~1.3s przy 20 TPS
+    private static final float STUCK_MIN_DIST = 0.3f;
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // INTERPOLACJA KAMERY (atomic frame dla render threadu)
+    // ══════════════════════════════════════════════════════════════════════════
+
+    private record YawPitchFrame(float prevYaw, float prevPitch,
+                                 float nextYaw, float nextPitch) {}
+
+    private volatile YawPitchFrame frame = new YawPitchFrame(0, 0, 0, 0);
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // STAN KAMERY
+    // ══════════════════════════════════════════════════════════════════════════
 
     private float   cameraYaw         = 0f;
     private float   cameraPitch       = 0f;
     private boolean cameraInitialized = false;
+    private float   currentMoveYaw    = 0f;  // yaw do aktualnego waypointu (nie lookahead)
+
+    private float yawVelocity   = 0f;
+    private float pitchVelocity = 0f;
+    private Vec3d smoothLookPoint = null;
+
+    // Lookahead state
+    private int lookTargetIndex = 0;
+    private int lookHoldTicks   = 0;
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // STAN PATHFINDINGU
+    // ══════════════════════════════════════════════════════════════════════════
 
     private List<BlockPos> path;
     private int     waypointIndex = 0;
     private boolean active        = false;
+    private boolean flying        = false;
 
-    // Read by MixinKeyboardInput each tick
     private volatile PathInput currentInput = PathInput.NONE;
 
-    // ── Public API ─────────────────────────────────────────────────────────
+    // Waypoint offsety (randomizacja pozycji w bloku)
+    private final Random              rng             = new Random();
+    private final Map<BlockPos, Vec3d> waypointOffsets = new HashMap<>();
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // STUCK DETECTION
+    // ══════════════════════════════════════════════════════════════════════════
+
+    private Vec3d lastStuckCheckPos = null;
+    private int   stuckTickCounter  = 0;
+    private final Set<BlockPos> blacklistedNodes = new HashSet<>();
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // PUBLIC API
+    // ══════════════════════════════════════════════════════════════════════════
 
     public synchronized void start(List<BlockPos> newPath) {
         this.path         = newPath;
@@ -50,35 +96,61 @@ public class PathExecutor {
         this.currentInput = PathInput.NONE;
     }
 
+    /** Zatrzymaj pathfinding — blacklista zostaje (potrzebna przy retry po stucku). */
     public synchronized void stop(MinecraftClient client) {
         active            = false;
         currentInput      = PathInput.NONE;
-        yawVelocity   = 0f;  // ← dodaj
+        yawVelocity       = 0f;
-        pitchVelocity = 0f;  // ← dodaj
+        pitchVelocity     = 0f;
-        smoothLookPoint = null;  // ← dodaj
+        smoothLookPoint   = null;
+        cameraInitialized = false;
+        currentMoveYaw    = 0f;
+        lookTargetIndex   = 0;
+        lookHoldTicks     = 0;
+        waypointIndex     = 0;
+        waypointOffsets.clear();
+        lastStuckCheckPos = null;
+        stuckTickCounter  = 0;
         if (client.player != null) client.player.setSprinting(false);
     }
 
+    /** Pełny reset — wywołuj tylko przy ręcznym zatrzymaniu przez gracza. */
+    public synchronized void reset(MinecraftClient client) {
+        stop(client);
+        blacklistedNodes.clear();
+    }
+
+    public synchronized void clearBlacklist() {
+        blacklistedNodes.clear();
+    }
+
+    public Set<BlockPos> getBlacklist() { return blacklistedNodes; }
+
     public synchronized boolean  isActive()         { return active; }
-    public synchronized int     getWaypointIndex(){ return waypointIndex; }
+    public synchronized int      getWaypointIndex() { return waypointIndex; }
     public synchronized int      getPathLength()    { return path != null ? path.size() : 0; }
+    public synchronized List<BlockPos> getPath()    { return path; }
     public synchronized BlockPos getGoal() {
         return (path != null && !path.isEmpty()) ? path.get(path.size() - 1) : null;
     }
 
-    public synchronized List<BlockPos> getPath()  { return path; }
-
     public PathInput getCurrentInput() {
         return active ? currentInput : null;
     }
 
+    public boolean isFlying()              { return flying; }
+    public void    setFlying(boolean f)    { this.flying = f; }
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // TICK GŁÓWNY
+    // ══════════════════════════════════════════════════════════════════════════
+
     public synchronized void tick(MinecraftClient client) {
         if (!active || path == null || client.player == null) {
             currentInput = PathInput.NONE;
             return;
         }
 
-        // ── Włącz/wyłącz latanie ──────────────────────────────────────────────
         if (flying) {
             client.player.getAbilities().flying      = true;
             client.player.getAbilities().allowFlying = true;
@@ -91,16 +163,22 @@ public class PathExecutor {
             return;
         }
 
+        checkStuck(client.player, client);
+
         BlockPos target = path.get(waypointIndex);
         rotateCamera(client.player, target, client);
         currentInput = computeInput(client.player, target);
 
         boolean shouldSprint = currentInput.sprint()
                 && !currentInput.jumping()
-                && (flying || client.player.isOnGround()); // ← flying nie wymaga gruntu
+                && (flying || client.player.isOnGround());
         client.player.setSprinting(shouldSprint);
     }
 
+    // ══════════════════════════════════════════════════════════════════════════
+    // KAMERA — rotacja i interpolacja
+    // ══════════════════════════════════════════════════════════════════════════
+
     private void rotateCamera(ClientPlayerEntity player, BlockPos target, MinecraftClient client) {
         if (!cameraInitialized) {
             cameraYaw         = player.getYaw();
@@ -108,6 +186,7 @@ public class PathExecutor {
             cameraInitialized = true;
         }
 
+        // ── Look target (lookahead) ───────────────────────────────────────────
         BlockPos lookTarget = chooseLookTarget(player, client);
         Vec3d rawLookPoint  = Vec3d.ofCenter(lookTarget).add(0, 0.5, 0);
 
@@ -117,109 +196,202 @@ public class PathExecutor {
             smoothLookPoint = smoothLookPoint.lerp(rawLookPoint, LOOK_POINT_LERP);
         }
 
-        Vec3d playerPos = new Vec3d(player.getX(), player.getEyeY(), player.getZ());
+        // ── Oblicz target yaw/pitch z smoothLookPoint ─────────────────────────
-        double dx = smoothLookPoint.x - playerPos.x;
+        Vec3d playerEye = new Vec3d(player.getX(), player.getEyeY(), player.getZ());
-        double dz = smoothLookPoint.z - playerPos.z;
+        double dx = smoothLookPoint.x - playerEye.x;
-        double dy = smoothLookPoint.y - playerPos.y;
+        double dz = smoothLookPoint.z - playerEye.z;
+        double dy = smoothLookPoint.y - playerEye.y;
 
         float targetYaw   = (float) Math.toDegrees(Math.atan2(-dx, dz));
         float targetPitch = (float) -Math.toDegrees(
                 Math.atan2(dy, Math.sqrt(dx * dx + dz * dz)));
         targetPitch = Math.max(-90f, Math.min(90f, targetPitch - PITCH_LOOK_AHEAD));
 
+        // ── Spring damper — yaw ───────────────────────────────────────────────
         float yawError = (float) angleDiff(targetYaw, cameraYaw);
         float yawAccel = SPRING_STIFFNESS * yawError - SPRING_DAMPING * yawVelocity;
         yawVelocity   += yawAccel;
         yawVelocity    = Math.max(-MAX_VELOCITY, Math.min(MAX_VELOCITY, yawVelocity));
 
+        // ── Spring damper — pitch ─────────────────────────────────────────────
         float pitchError = targetPitch - cameraPitch;
         float pitchAccel = SPRING_STIFFNESS * pitchError - SPRING_DAMPING * pitchVelocity;
         pitchVelocity   += pitchAccel;
         pitchVelocity    = Math.max(-MAX_VELOCITY, Math.min(MAX_VELOCITY, pitchVelocity));
 
-        prevYaw   = cameraYaw;
+        float oldYaw   = cameraYaw;
-        prevPitch = cameraPitch;
+        float oldPitch = cameraPitch;
 
         cameraYaw   += yawVelocity;
         cameraPitch += pitchVelocity;
         cameraPitch  = Math.max(-90f, Math.min(90f, cameraPitch));
 
-        nextYaw   = cameraYaw;
+        // Atomowy zapis dla render threadu
-        nextPitch = cameraPitch;
+        frame = new YawPitchFrame(oldYaw, oldPitch, cameraYaw, cameraPitch);
 
         player.setYaw(cameraYaw);
         player.setPitch(cameraPitch);
-    }
+
-    public float getInterpolatedYaw(float tickDelta) {
+        // ── moveYaw — osobny kąt dla nóg (do aktualnego waypointu, nie lookahead) ──
-        float diff = (float) angleDiff(nextYaw, prevYaw);
+        Vec3d playerFeet = player.getEntityPos();
-        return prevYaw + diff * tickDelta;
+        Vec3d wpTarget   = getWaypointTarget(path.get(waypointIndex));
-    }
+        double mdx = wpTarget.x - playerFeet.x;
-    public float getInterpolatedPitch(float tickDelta) {
+        double mdz = wpTarget.z - playerFeet.z;
-        return prevPitch + (nextPitch - prevPitch) * tickDelta;
+        float rawMoveYaw = (float) Math.toDegrees(Math.atan2(-mdx, mdz));
+        float moveDiff   = (float) angleDiff(rawMoveYaw, currentMoveYaw);
+        currentMoveYaw  += moveDiff * 0.3f; // lerp 30% — wygładza drobne wahania
     }
 
+    public float getInterpolatedYaw(float tickDelta) {
+        YawPitchFrame f = frame;
+        float diff = (float) angleDiff(f.nextYaw(), f.prevYaw());
+        return f.prevYaw() + diff * tickDelta;
+    }
+
+    public float getInterpolatedPitch(float tickDelta) {
+        YawPitchFrame f = frame;
+        return f.prevPitch() + (f.nextPitch() - f.prevPitch()) * tickDelta;
+    }
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // LOOKAHEAD — wybór punktu do patrzenia
+    // ══════════════════════════════════════════════════════════════════════════
 
     private BlockPos chooseLookTarget(ClientPlayerEntity player, MinecraftClient client) {
         Vec3d eyePos    = player.getEyePos();
+        Vec3d playerPos = player.getEntityPos();
+        int   MAX_LOOKAHEAD = 4;
+        int   bestIdx       = waypointIndex;
+
+        for (int i = 1; i <= MAX_LOOKAHEAD; i++) {
+            int idx = waypointIndex + i;
+            if (idx >= path.size()) break;
 
-        int LOOKAHEAD_STEPS = 3;
-        float MIN_LOOKAHEAD_DIST = 4.0f;
-        for (int i = LOOKAHEAD_STEPS; i >= 1; i--) {
-            int idx = Math.min(waypointIndex + i, path.size() - 1);
             BlockPos candidate    = path.get(idx);
             Vec3d   candidateVec  = Vec3d.ofCenter(candidate).add(0, 0.5, 0);
+            double  distToCandidate = eyePos.distanceTo(candidateVec);
 
-            if (eyePos.distanceTo(candidateVec) < MIN_LOOKAHEAD_DIST) continue;
+            // ── Sprawdź czy między aktualnym a kandydatem jest zmiana wysokości ──
+            // Jeśli którykolwiek waypoint po drodze ma inną Y — zatrzymaj lookahead
+            boolean heightChangeOnPath = false;
+            for (int j = waypointIndex; j < idx; j++) {
+                int yA = path.get(j).getY();
+                int yB = path.get(j + 1).getY();
+                if (Math.abs(yA - yB) >= 1) {
+                    heightChangeOnPath = true;
+                    break;
+                }
+            }
+
+            // Jeśli na drodze do kandydata jest zmiana wysokości —
+            // patrz tylko na pierwszy waypoint tej zmiany, nie dalej
+            if (heightChangeOnPath) {
+                // Znajdź pierwszy waypoint ze zmianą Y i patrz na niego
+                for (int j = waypointIndex; j < idx; j++) {
+                    if (Math.abs(path.get(j).getY() - path.get(j + 1).getY()) >= 1) {
+                        bestIdx = j + 1; // patrz na wyższy/niższy waypoint
+                        break;
+                    }
+                }
+                break; // nie idź dalej w lookahead
+            }
+
+            // Brak zmiany wysokości — normalna logika
+            if (distToCandidate < 2.5) continue;
+
+            double angleToCandidate = angleToTarget(player, candidateVec);
+            if (Math.abs(angleToCandidate) > 70.0) break;
 
             BlockHitResult hit = client.world.raycast(new RaycastContext(
-                    eyePos,
+                    eyePos, candidateVec,
-                    candidateVec,
+                    RaycastContext.ShapeType.OUTLINE,
-                    RaycastContext.ShapeType.COLLIDER,
                     RaycastContext.FluidHandling.NONE,
                     player
             ));
 
             if (hit.getType() == HitResult.Type.MISS) {
-                return candidate;
+                bestIdx = idx;
+            } else {
+                break;
             }
         }
 
-        return path.get(waypointIndex);
+        return path.get(bestIdx);
     }
 
-    // ── Internal ───────────────────────────────────────────────────────────
+    /** Kąt między aktualnym kierunkiem ruchu gracza a punktem docelowym. */
+    private double angleToTarget(ClientPlayerEntity player, Vec3d target) {
+        Vec3d pos = player.getEntityPos();
+        double dx = target.x - pos.x;
+        double dz = target.z - pos.z;
+        double targetYaw = Math.toDegrees(Math.atan2(-dx, dz));
+        return angleDiff(targetYaw, currentMoveYaw);
+    }
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // RUCH — waypoints i input
+    // ══════════════════════════════════════════════════════════════════════════
 
     private void advanceWaypoints(ClientPlayerEntity player) {
         while (waypointIndex < path.size()) {
-            Vec3d center = Vec3d.ofCenter(path.get(waypointIndex));
+            BlockPos wpBlock = path.get(waypointIndex);
+            Vec3d    center  = getWaypointTarget(wpBlock);
             Vec3d    pPos    = player.getEntityPos();
 
-            double dist;
-            double reach;
-
             if (flying) {
-                dist  = pPos.distanceTo(center);           // pełny 3D
+                double dist = pPos.distanceTo(center);
-                reach = 2.5;                               // większy reach w locie
+                if (dist < 2.5) waypointIndex++;
+                else break;
             } else {
-                dist  = horizontalDist(pPos, center);      // poziomy 2D
+                double horizDist = horizontalDist(pPos, center);
-                reach = Math.min(1.8, 0.55 + dist * 0.15);
+                double vertDiff  = pPos.y - center.y; // dodatnie = gracz wyżej niż cel
+
+                // ── Cel jest WYŻEJ niż gracz (skok w górę) ───────────────────────
+                // Nie zaliczaj dopóki gracz faktycznie nie wejdzie na ten poziom
+                if (wpBlock.getY() > Math.floor(pPos.y) + 0.1) {
+                    // Gracz musi być na tym samym Y i poziomo blisko
+                    if (horizDist < 1.0 && Math.abs(vertDiff) < 0.6) {
+                        waypointIndex++;
+                    } else {
+                        break;
+                    }
+                    continue;
                 }
 
-            if (dist < reach) waypointIndex++;
+                // ── Cel jest NIŻEJ niż gracz (schodzenie/spadanie) ───────────────
+                if (vertDiff > 0.8 && horizDist < 1.2) {
+                    waypointIndex++;
+                    continue;
+                }
+
+                // ── Ten sam poziom — normalne zaliczanie ─────────────────────────
+                double reach = Math.min(1.5, 0.5 + horizDist * 0.1);
+                if (horizDist < reach) waypointIndex++;
                 else break;
             }
         }
+    }
 
     private PathInput computeInput(ClientPlayerEntity player, BlockPos target) {
         Vec3d playerPos = new Vec3d(player.getX(), player.getY(), player.getZ());
-        Vec3d targetPos = Vec3d.ofCenter(target);
+        Vec3d targetPos = getWaypointTarget(target);
+
+        // Jeśli cel jest niżej i gracz jest poziomo nad nim — patrz na następny
+        // waypoint żeby nie wchodzić w ścianę przy schodzeniu
+        double horizDist = horizontalDist(playerPos, targetPos);
+        double vertDiff  = playerPos.y - targetPos.y;
+        if (vertDiff > 0.8 && horizDist < 1.5 && waypointIndex + 1 < path.size()) {
+            targetPos = getWaypointTarget(path.get(waypointIndex + 1));
+        }
 
         double dx = targetPos.x - playerPos.x;
         double dz = targetPos.z - playerPos.z;
 
-        double targetYaw = Math.toDegrees(Math.atan2(-dx, dz));
+        double moveYaw = Math.toDegrees(Math.atan2(-dx, dz));
-        double diff      = angleDiff(targetYaw, player.getYaw());
 
+        // Użyj currentMoveYaw (kąt do waypointu) zamiast player.getYaw()
+        // (który jest ustawiony przez lookahead kamery)
+        double diff     = angleDiff(moveYaw, currentMoveYaw);
         double rad      = Math.toRadians(diff);
         float  forward  = (float) Math.cos(rad);
         float  sideways = (float) -Math.sin(rad);
@@ -227,50 +399,27 @@ public class PathExecutor {
         if (Math.abs(forward)  < 0.05f) forward  = 0;
         if (Math.abs(sideways) < 0.05f) sideways = 0;
 
-        // ── Oblicz mnożnik prędkości ──────────────────────────────────────────
+        // ── Hamowanie przed zakrętem ──────────────────────────────────────────
         float speedMult = 1.0f;
 
-        // 1. Hamowanie przed celem (ostatnie N bloków)z
-        boolean isLastWaypoint = (waypointIndex == path.size() - 1);
-       // if (flying) {
-            //double distToTarget = playerPos.distanceTo(targetPos);
-           // if (isLastWaypoint) {
-                // Hamuj płynnie na ostatnich 6 blokach
-                //speedMult *= (float) Math.min(1.0, distToTarget / 0.1);
-         //   }
-        //} else {
-            //double distToTarget = horizontalDist(playerPos, targetPos);
-            //if (isLastWaypoint) {
-                //speedMult *= (float) Math.min(1.0, distToTarget / 4.0);
-            //}
-        //}
-
-        // 2. Hamowanie przed zakrętem — patrz na kąt do NASTĘPNEGO waypointu
         if (waypointIndex + 1 < path.size()) {
             BlockPos nextTarget  = path.get(waypointIndex + 1);
             Vec3d    nextPos     = Vec3d.ofCenter(nextTarget);
-
             double   ndx        = nextPos.x - targetPos.x;
             double   ndz        = nextPos.z - targetPos.z;
             double   nextYaw    = Math.toDegrees(Math.atan2(-ndx, ndz));
-            double turnAngle = Math.abs(angleDiff(nextYaw, targetYaw));
+            double   turnAngle  = Math.abs(angleDiff(nextYaw, moveYaw));
 
-            // Im większy zakręt tym mocniej hamuj (powyżej 30° zaczyna hamować)
             if (turnAngle > 30.0) {
                 float  turnMult   = (float) Math.max(0.3, 1.0 - (turnAngle - 30.0) / 120.0);
-
-                // Hamuj tylko gdy jesteś blisko zakrętu
                 double distToTurn = flying
                         ? playerPos.distanceTo(targetPos)
                         : horizontalDist(playerPos, targetPos);
-
+                if (distToTurn < 5.0) speedMult *= turnMult;
-                if (distToTurn < 5.0) {
-                    speedMult *= turnMult;
-                }
             }
         }
 
-        // 3. Hamowanie przy zmianie wysokości w locie
+        // ── Lot — sterowanie pionowe i hamowanie przy dużej zmianie Y ────────
         float   upward   = 0f;
         boolean jumping  = false;
         boolean sneaking = false;
@@ -279,31 +428,85 @@ public class PathExecutor {
             double dy = targetPos.y - playerPos.y;
             if      (dy >  0.4) upward =  1.0f;
             else if (dy < -0.4) upward = -1.0f;
-
+            if (Math.abs(dy) > 3.0) speedMult *= 0.5f;
-            // Jeśli duża zmiana wysokości — zwolnij ruch poziomy
-            if (Math.abs(dy) > 3.0) {
-                speedMult *= 0.5f;
-            }
         } else {
             jumping = target.getY() > Math.floor(playerPos.y) + 0.5;
         }
 
-        // Clamp speedMult żeby nie był ujemny
         speedMult = Math.max(0.0f, Math.min(1.0f, speedMult));
 
-        // Zastosuj mnożnik do forward/sideways
+        boolean isLastWaypoint = (waypointIndex == path.size() - 1);
-        forward  *= speedMult;
-        sideways *= speedMult;
-
         boolean sprint = !isLastWaypoint
                 && Math.abs(diff) < SPRINT_ANGLE
                 && forward > 0.5f
-                && speedMult > 0.7f; // nie sprintuj gdy hamujesz
+                && speedMult > 0.7f;
 
         return new PathInput(forward, sideways, upward, jumping, sprint, sneaking);
     }
 
-    // ── Utilities ──────────────────────────────────────────────────────────
+    // ══════════════════════════════════════════════════════════════════════════
+    // STUCK DETECTION
+    // ══════════════════════════════════════════════════════════════════════════
+
+    private void checkStuck(ClientPlayerEntity player, MinecraftClient client) {
+        Vec3d pos = player.getEntityPos();
+
+        if (lastStuckCheckPos == null) {
+            lastStuckCheckPos = pos;
+            return;
+        }
+
+        double moved = horizontalDist(pos, lastStuckCheckPos);
+
+        if (moved < STUCK_MIN_DIST) {
+            stuckTickCounter++;
+            if (stuckTickCounter >= STUCK_TICKS) {
+                stuckTickCounter  = 0;
+                lastStuckCheckPos = pos;
+                handleStuck(client);
+            }
+        } else {
+            stuckTickCounter  = 0;
+            lastStuckCheckPos = pos;
+        }
+    }
+
+    private void handleStuck(MinecraftClient client) {
+        if (path == null || client.player == null) return;
+
+        BlockPos goal    = path.get(path.size() - 1);
+        BlockPos stuckAt = client.player.getBlockPos();
+
+        for (int dx = -1; dx <= 1; dx++) {
+            for (int dz = -1; dz <= 1; dz++) {
+                BlockPos candidate = stuckAt.add(dx, 0, dz);
+                if (candidate.isWithinDistance(goal, 3.0)) continue;
+                blacklistedNodes.add(candidate);
+                blacklistedNodes.add(candidate.up());
+                blacklistedNodes.add(candidate.down());
+            }
+        }
+
+        stop(client);
+
+        PathfindingController.INSTANCE.goTo(
+                goal.getX(), goal.getY(), goal.getZ(),
+                flying, null
+        );
+    }
+
+    // ══════════════════════════════════════════════════════════════════════════
+    // POMOCNICZE
+    // ══════════════════════════════════════════════════════════════════════════
+
+    /** Losowy offset w bloku — zapobiega chodzeniu przez idealny środek. */
+    private Vec3d getWaypointTarget(BlockPos pos) {
+        return waypointOffsets.computeIfAbsent(pos, p -> {
+            double ox = (rng.nextDouble() - 0.5) * 0.6;
+            double oz = (rng.nextDouble() - 0.5) * 0.6;
+            return new Vec3d(p.getX() + 0.5 + ox, p.getY(), p.getZ() + 0.5 + oz);
+        });
+    }
 
     private static double horizontalDist(Vec3d a, Vec3d b) {
         double dx = a.x - b.x;
@@ -317,12 +520,4 @@ public class PathExecutor {
         while (diff < -180) diff += 360;
         return diff;
     }
-
-    public boolean isFlying() {
-        return flying;
-    }
-
-    public void setFlying(boolean flying) {
-        this.flying = flying;
-    }
 }

PathfindingController.java

@@ -85,7 +85,19 @@ public class PathfindingController {
         executor.setFlying(flying);
 
         threadPool.submit(() -> {
-            List<BlockPos> path = AStarPathfinder.find(client.world, start, goal, flying);
+            List<BlockPos> path = AStarPathfinder.find(
+                    client.world, start, goal, flying,
+                    executor.getBlacklist()
+            );
+
+            // Jeśli nie znalazł z blacklistą — spróbuj bez niej
+            if (path.isEmpty() && !executor.getBlacklist().isEmpty()) {
+                executor.clearBlacklist();
+                path = AStarPathfinder.find(
+                        client.world, start, goal, flying,
+                        executor.getBlacklist()
+                );
+            }
 
             if (path.isEmpty()) {
                 status = "No path found";
@@ -111,7 +123,7 @@ public class PathfindingController {
     /** Stop walking immediately. */
     public void stop() {
         MinecraftClient client = MinecraftClient.getInstance();
-        executor.stop(client);
+        executor.reset(client);
         status = "Stopped";
     }