- Add 3 new pathfinding algorithms

   - Theta*
   - A* with finegrid used by jps
   - A* with finegrid used by jps + diagonal routes
- Make MStringSelectionButton inc and dec button work as intended
- Add new "feature" for choosing pathfind strategy

4 files changed, 593 insertions, 6 deletions

diff --git a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarCornerCut.java b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarCornerCut.java
new file mode 100644
index 00000000..ee71f90b
--- /dev/null
+++ b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarCornerCut.java
@@ -0,0 +1,189 @@
+/*
+ * Dungeons Guide - The most intelligent Hypixel Skyblock Dungeons Mod
+ * Copyright (C) 2021  cyoung06
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package kr.syeyoung.dungeonsguide.pathfinding;
+
+import kr.syeyoung.dungeonsguide.dungeon.roomfinder.DungeonRoom;
+import lombok.Data;
+import lombok.EqualsAndHashCode;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+import net.minecraft.util.*;
+import net.minecraft.world.World;
+
+import java.util.*;
+
+public class AStarCornerCut {
+    private final BlockPos min, max;
+    private final World world;
+
+    int lastSx, lastSy, lastSz;
+    final int dx, dy, dz;
+    private DungeonRoom dungeonRoom;
+
+    @Getter
+    private AxisAlignedBB destinationBB;
+
+    public AStarCornerCut(DungeonRoom dungeonRoom, Vec3 destination) {
+        this.min = new BlockPos(dungeonRoom.getMinx(), 0, dungeonRoom.getMinz());
+        this.max = new BlockPos(dungeonRoom.getMaxx(), 255, dungeonRoom.getMaxz());
+
+        this.world = dungeonRoom.getCachedWorld();
+        this.dungeonRoom = dungeonRoom;
+
+        this.dx = (int) (destination.xCoord * 2);
+        this.dy = (int) (destination.yCoord * 2);
+        this.dz = (int) (destination.zCoord * 2);
+        destinationBB = AxisAlignedBB.fromBounds(dx-2, dy-2, dz-2, dx+2, dy+2, dz+2);
+    }
+
+    private Map<Node.Coordinate, Node> nodeMap = new HashMap<>();
+
+    private Node openNode(int x, int y, int z)
+    {
+        Node.Coordinate coordinate = new Node.Coordinate(x,y,z);
+        Node node = this.nodeMap.get(coordinate);
+
+        if (node == null)
+        {
+            node = new Node(coordinate);
+            this.nodeMap.put(coordinate, node);
+        }
+
+        return node;
+    }
+
+    @Getter
+    private LinkedList<Vec3> route = new LinkedList<>();
+
+    @Getter
+    private PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparing((Node a) -> a == null ? Float.MAX_VALUE : a.f).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.x).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.y).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.z));
+
+    private int pfindIdx = 0;
+
+    public boolean pathfind(Vec3 from, long timeout) {
+        pfindIdx ++;
+        if (lastSx != (int)Math.round(from.xCoord * 2) || lastSy != (int)Math.round(from.yCoord*2) || lastSz != (int)Math.round(from.zCoord * 2))
+            open.clear();
+
+        this.lastSx = (int) Math.round(from.xCoord * 2);
+        this.lastSy = (int) Math.round(from.yCoord * 2);
+        this.lastSz = (int) Math.round(from.zCoord * 2);
+
+        Node startNode = openNode(dx, dy, dz);
+        Node goalNode = openNode(lastSx, lastSy, lastSz);
+
+        if (goalNode.parent != null) {
+            LinkedList<Vec3> route = new LinkedList<>();
+            Node curr =goalNode;
+            while(curr.parent != null) {
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                curr = curr.parent;
+            }
+            route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+            this.route = route;
+            return true;
+        }
+
+        startNode.g = 0;
+        startNode.f = 0;
+        goalNode.g = Integer.MAX_VALUE; goalNode.f = Integer.MAX_VALUE;
+
+
+        open.add(startNode);
+
+        long end = System.currentTimeMillis() + timeout;
+
+        while (!open.isEmpty()) {
+            if (System.currentTimeMillis() > end) {
+                return false;
+            }
+
+            Node n = open.poll();
+            if (n.lastVisited == pfindIdx) continue;
+            n.lastVisited = pfindIdx;
+
+            if (n == goalNode) {
+                // route = reconstructPath(startNode)
+                LinkedList<Vec3> route = new LinkedList<>();
+                Node curr =goalNode;
+                while(curr.parent != null) {
+                    route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                    curr = curr.parent;
+                }
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                this.route = route;
+                return true;
+            }
+
+            for (int z = -1; z <= 1; z++) {for (int y = -1; y <= 1; y ++) { for(int x = -1; x <= 1; x++) {
+                if (x == 0 && y == 0 && z == 0) continue;
+                Node neighbor = openNode(n.coordinate.x +x, n.coordinate.y +y, n.coordinate.z + z);
+
+                // check blocked.
+                if (!((destinationBB.minX <= neighbor.coordinate.x && neighbor.coordinate.x <= destinationBB.maxX &&
+                        destinationBB.minY <= neighbor.coordinate.y && neighbor.coordinate.y <= destinationBB.maxY &&
+                        destinationBB.minZ <= neighbor.coordinate.z && neighbor.coordinate.z <= destinationBB.maxZ) // near destination
+                 || !dungeonRoom.isBlocked(neighbor.coordinate.x, neighbor.coordinate.y, neighbor.coordinate.z))) { // not blocked
+                    continue;
+                }
+                if (neighbor.lastVisited == pfindIdx) continue;
+
+
+                float gScore = n.g + MathHelper.sqrt_float(x*x + y*y + z*z); // altho it's sq, it should be fine
+                if (gScore < neighbor.g ) {
+                    neighbor.parent = n;
+                    neighbor.g = gScore;
+                    neighbor.f = gScore + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                    open.add(neighbor);
+                } else if (neighbor.lastVisited != pfindIdx) {
+                    neighbor.f = gScore + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                    open.add(neighbor);
+                }
+            }}}
+        }
+        return true;
+    }
+
+    private int manhatten(int x, int y, int z) {return Math.abs(x)+ Math.abs(y)+ Math.abs(z);}
+    private float distSq(float x, float y, float z) {
+        return MathHelper.sqrt_float(x * x + y * y + z * z);
+    }
+
+    @RequiredArgsConstructor
+    @Data
+    public static final class Node {
+        @Data
+        @RequiredArgsConstructor
+        public static final class Coordinate {
+            private final int x, y, z;
+        }
+        private final Coordinate coordinate;
+
+        private float f = Float.MAX_VALUE, g = Float.MAX_VALUE;
+        private int lastVisited;
+
+        @EqualsAndHashCode.Exclude
+        private Node parent;
+
+        public static long makeHash(int x, int y, int z)
+        {
+            return y & 32767L | ((short)x & 32767L) << 16 | ((short)z & 32767L) << 32;
+        }
+    }
+}
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarFineGrid.java b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarFineGrid.java
new file mode 100644
index 00000000..94fa8a87
--- /dev/null
+++ b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/AStarFineGrid.java
@@ -0,0 +1,182 @@
+/*
+ * Dungeons Guide - The most intelligent Hypixel Skyblock Dungeons Mod
+ * Copyright (C) 2021  cyoung06
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package kr.syeyoung.dungeonsguide.pathfinding;
+
+import kr.syeyoung.dungeonsguide.dungeon.roomfinder.DungeonRoom;
+import lombok.Data;
+import lombok.EqualsAndHashCode;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+import net.minecraft.util.*;
+import net.minecraft.world.World;
+
+import java.util.*;
+
+public class AStarFineGrid {
+    private final BlockPos min, max;
+    private final World world;
+
+    int lastSx, lastSy, lastSz;
+    final int dx, dy, dz;
+    private DungeonRoom dungeonRoom;
+
+    @Getter
+    private AxisAlignedBB destinationBB;
+
+    public AStarFineGrid(DungeonRoom dungeonRoom, Vec3 destination) {
+        this.min = new BlockPos(dungeonRoom.getMinx(), 0, dungeonRoom.getMinz());
+        this.max = new BlockPos(dungeonRoom.getMaxx(), 255, dungeonRoom.getMaxz());
+
+        this.world = dungeonRoom.getCachedWorld();
+        this.dungeonRoom = dungeonRoom;
+
+        this.dx = (int) (destination.xCoord * 2);
+        this.dy = (int) (destination.yCoord * 2);
+        this.dz = (int) (destination.zCoord * 2);
+        destinationBB = AxisAlignedBB.fromBounds(dx-2, dy-2, dz-2, dx+2, dy+2, dz+2);
+    }
+
+    private Map<Node.Coordinate, Node> nodeMap = new HashMap<>();
+
+    private Node openNode(int x, int y, int z)
+    {
+        Node.Coordinate coordinate = new Node.Coordinate(x,y,z);
+        Node node = this.nodeMap.get(coordinate);
+
+        if (node == null)
+        {
+            node = new Node(coordinate);
+            this.nodeMap.put(coordinate, node);
+        }
+
+        return node;
+    }
+
+    @Getter
+    private LinkedList<Vec3> route = new LinkedList<>();
+
+    @Getter
+    private PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparing((Node a) -> a == null ? Float.MAX_VALUE : a.f).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.x).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.y).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.z));
+
+    private int pfindIdx = 0;
+
+    public boolean pathfind(Vec3 from, long timeout) {
+        pfindIdx ++;
+        if (lastSx != (int)Math.round(from.xCoord * 2) || lastSy != (int)Math.round(from.yCoord*2) || lastSz != (int)Math.round(from.zCoord * 2))
+            open.clear();
+
+        this.lastSx = (int) Math.round(from.xCoord * 2);
+        this.lastSy = (int) Math.round(from.yCoord * 2);
+        this.lastSz = (int) Math.round(from.zCoord * 2);
+
+        Node startNode = openNode(dx, dy, dz);
+        Node goalNode = openNode(lastSx, lastSy, lastSz);
+        startNode.g = 0;
+        startNode.f = 0;
+        goalNode.g = Integer.MAX_VALUE; goalNode.f = Integer.MAX_VALUE;
+        if (goalNode.parent != null) {
+            LinkedList<Vec3> route = new LinkedList<>();
+            Node curr =goalNode;
+            while(curr.parent != null) {
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                curr = curr.parent;
+            }
+            route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+            this.route = route;
+            return true;
+        }
+        open.add(startNode);
+
+        long end = System.currentTimeMillis() + timeout;
+
+        while (!open.isEmpty()) {
+            if (System.currentTimeMillis() > end) {
+                return false;
+            }
+
+            Node n = open.poll();
+            if (n.lastVisited == pfindIdx) continue;
+            n.lastVisited = pfindIdx;
+
+            if (n == goalNode) {
+                // route = reconstructPath(startNode)
+                LinkedList<Vec3> route = new LinkedList<>();
+                Node curr =goalNode;
+                while(curr.parent != null) {
+                    route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                    curr = curr.parent;
+                }
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                this.route = route;
+                return true;
+            }
+
+            for (EnumFacing value : EnumFacing.VALUES) {
+                Node neighbor = openNode(n.coordinate.x + value.getFrontOffsetX(), n.coordinate.y + value.getFrontOffsetY(), n.coordinate.z + value.getFrontOffsetZ());
+
+                // check blocked.
+                if (!((destinationBB.minX <= neighbor.coordinate.x && neighbor.coordinate.x <= destinationBB.maxX &&
+                        destinationBB.minY <= neighbor.coordinate.y && neighbor.coordinate.y <= destinationBB.maxY &&
+                        destinationBB.minZ <= neighbor.coordinate.z && neighbor.coordinate.z <= destinationBB.maxZ) // near destination
+                 || !dungeonRoom.isBlocked(neighbor.coordinate.x, neighbor.coordinate.y, neighbor.coordinate.z))) { // not blocked
+                    continue;
+                }
+
+                float gScore = n.g + 1; // altho it's sq, it should be fine
+                if (gScore < neighbor.g) {
+                    neighbor.parent = n;
+                    neighbor.g = gScore;
+                    neighbor.f = gScore + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                    open.add(neighbor);
+                } else if (neighbor.lastVisited != pfindIdx) {
+                    neighbor.f = gScore + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                    open.add(neighbor);
+                }
+            }
+        }
+        return true;
+    }
+
+    private int manhatten(int x, int y, int z) {return Math.abs(x)+ Math.abs(y)+ Math.abs(z);}
+    private float distSq(float x, float y, float z) {
+        return MathHelper.sqrt_float(x * x + y * y + z * z);
+    }
+
+    @RequiredArgsConstructor
+    @Data
+    public static final class Node {
+        @Data
+        @RequiredArgsConstructor
+        public static final class Coordinate {
+            private final int x, y, z;
+        }
+        private final Coordinate coordinate;
+
+        private float f = Float.MAX_VALUE, g = Float.MAX_VALUE;
+        private int lastVisited;
+
+        @EqualsAndHashCode.Exclude
+        private Node parent;
+
+        public static long makeHash(int x, int y, int z)
+        {
+            return y & 32767L | ((short)x & 32767L) << 16 | ((short)z & 32767L) << 32;
+        }
+    }
+}
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java
index 6c88d4cf..5d48ccb2 100644
--- a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java
@@ -72,7 +72,7 @@ public class JPSPathfinder {
     private LinkedList<Vec3> route = new LinkedList<>();
 
     @Getter
-    private PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparing((Node a) -> a.f).thenComparing(a -> a.x).thenComparing(a -> a.y).thenComparing(a -> a.z));
+    private PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparing((Node a) -> a == null ? Float.MAX_VALUE : a.f).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.x).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.y).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.z));
 
     private int tx, ty, tz;
 
@@ -111,11 +111,11 @@ public class JPSPathfinder {
         destinationBB = AxisAlignedBB.fromBounds((to.xCoord - within)* 2, (to.yCoord - within) * 2, (to.zCoord - within) * 2, (to.xCoord + within) * 2, (to.yCoord + within)* 2, (to.zCoord + within) *2);
         open.clear();
         Node start;
-        open.add(start = openNode((int)from.xCoord* 2 + 1, (int)from.yCoord* 2 + 1, (int)from.zCoord * 2 + 1));
+        open.add(start = openNode((int)from.xCoord * 2 + 1, (int)from.yCoord * 2, (int)from.zCoord * 2+1));
         start.g = 0; start.f = 0; start.h = (float) from.squareDistanceTo(to);
 
         Node end = null; float minDist = Float.MAX_VALUE;
-        long forceEnd = System.currentTimeMillis() + timeout;
+        long forceEnd = System.currentTimeMillis() + timeout + 999999999L;
         while(!open.isEmpty()) {
             if (forceEnd < System.currentTimeMillis() && timeout != -1) break;
             Node n = open.poll();
@@ -263,9 +263,13 @@ public class JPSPathfinder {
                     }
                 }
             } else if (determinant == 2) {
-                if ((dx != 0 && dungeonRoom.isBlocked(nx , y , z ) && !dungeonRoom.isBlocked(nx + dx, y , z))
-                        || (dy != 0 && dungeonRoom.isBlocked(x , ny  , z) && !dungeonRoom.isBlocked(x , ny+dy  , z))
-                        || (dz != 0 && dungeonRoom.isBlocked(x  , y , nz  ) && !dungeonRoom.isBlocked(x  , y , nz+dz))) return openNode(nx,ny,nz);
+                for (EnumFacing value : EnumFacing.VALUES) {
+                    if (value.getFrontOffsetX() == dx || value.getFrontOffsetY() == dy || value.getFrontOffsetZ() == dz) continue;
+                    int tx = nx + value.getFrontOffsetX();
+                    int ty = ny + value.getFrontOffsetY();
+                    int tz = nz + value.getFrontOffsetZ();
+                    if (dungeonRoom.isBlocked(tx, ty, tz)) return openNode(nx, ny, nz);
+                }
                 if (dx != 0 &&  expand(nx, ny, nz, dx, 0,0) != null) return openNode(nx,ny,nz);
                 if (dy != 0 && expand(nx, ny, nz, 0, dy,0) != null) return openNode(nx,ny,nz);
                 if (dz != 0 && expand(nx, ny, nz, 0, 0,dz) != null) return openNode(nx,ny,nz);
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/ThetaStar.java b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/ThetaStar.java
new file mode 100644
index 00000000..4e16cd4d
--- /dev/null
+++ b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/ThetaStar.java
@@ -0,0 +1,212 @@
+/*
+ * Dungeons Guide - The most intelligent Hypixel Skyblock Dungeons Mod
+ * Copyright (C) 2021  cyoung06
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package kr.syeyoung.dungeonsguide.pathfinding;
+
+import kr.syeyoung.dungeonsguide.dungeon.roomfinder.DungeonRoom;
+import lombok.*;
+import net.minecraft.util.*;
+import net.minecraft.world.World;
+
+import java.util.*;
+
+public class ThetaStar {
+    private final BlockPos min, max;
+    private final World world;
+
+    int lastSx, lastSy, lastSz;
+    final int dx, dy, dz;
+    private DungeonRoom dungeonRoom;
+
+    @Getter
+    private AxisAlignedBB destinationBB;
+
+    public ThetaStar(DungeonRoom dungeonRoom, Vec3 destination) {
+        this.min = new BlockPos(dungeonRoom.getMinx(), 0, dungeonRoom.getMinz());
+        this.max = new BlockPos(dungeonRoom.getMaxx(), 255, dungeonRoom.getMaxz());
+
+        this.world = dungeonRoom.getCachedWorld();
+        this.dungeonRoom = dungeonRoom;
+
+        this.dx = (int) (destination.xCoord * 2);
+        this.dy = (int) (destination.yCoord * 2);
+        this.dz = (int) (destination.zCoord * 2);
+        destinationBB = AxisAlignedBB.fromBounds(dx-2, dy-2, dz-2, dx+2, dy+2, dz+2);
+    }
+
+    private Map<Node.Coordinate, Node> nodeMap = new HashMap<>();
+
+    private Node openNode(int x, int y, int z)
+    {
+        Node.Coordinate coordinate = new Node.Coordinate(x,y,z);
+        Node node = this.nodeMap.get(coordinate);
+
+        if (node == null)
+        {
+            node = new Node(coordinate);
+            this.nodeMap.put(coordinate, node);
+        }
+
+        return node;
+    }
+
+    @Getter
+    private LinkedList<Vec3> route = new LinkedList<>();
+
+    @Getter
+    private PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparing((Node a) -> a == null ? Float.MAX_VALUE : a.f).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.x).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.y).thenComparing(a -> a == null ? Float.MAX_VALUE :  a.coordinate.z));
+
+    private int pfindIdx = 0;
+
+    public boolean pathfind(Vec3 from, long timeout) {
+
+        pfindIdx ++;
+        if (lastSx != (int)Math.round(from.xCoord * 2) || lastSy != (int)Math.round(from.yCoord*2) || lastSz != (int)Math.round(from.zCoord * 2))
+            open.clear();
+
+        this.lastSx = (int) Math.round(from.xCoord * 2);
+        this.lastSy = (int) Math.round(from.yCoord * 2);
+        this.lastSz = (int) Math.round(from.zCoord * 2);
+        if (dungeonRoom.isBlocked(lastSx, lastSy, lastSz)) return false;
+
+        Node startNode = openNode(dx, dy, dz);
+        Node goalNode = openNode(lastSx, lastSy, lastSz);
+        startNode.g = 0;
+        startNode.f = 0;
+        goalNode.g = Integer.MAX_VALUE; goalNode.f = Integer.MAX_VALUE;
+        if (goalNode.parent != null) {
+            LinkedList<Vec3> route = new LinkedList<>();
+            Node curr =goalNode;
+            while(curr.parent != null) {
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                curr = curr.parent;
+            }
+            route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+            this.route = route;
+            System.out.println("Route len: "+route.size());
+            return true;
+        }
+        open.add(startNode);
+
+        long end = System.currentTimeMillis() + timeout;
+
+        while (!open.isEmpty()) {
+            if (System.currentTimeMillis() > end) {
+                return false;
+            }
+            Node n = open.poll();
+            if (n.lastVisited == pfindIdx) continue;
+            n.lastVisited = pfindIdx;
+
+            if (n == goalNode) {
+                // route = reconstructPath(startNode)
+                LinkedList<Vec3> route = new LinkedList<>();
+                Node curr =goalNode;
+                while(curr.parent != null) {
+                    route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                    curr = curr.parent;
+                }
+                route.addLast(new Vec3(curr.coordinate.x / 2.0, curr.coordinate.y / 2.0 + 0.1, curr.coordinate.z/ 2.0));
+                this.route = route;
+                return true;
+            }
+
+            for (EnumFacing value : EnumFacing.VALUES) {
+                Node neighbor = openNode(n.coordinate.x + value.getFrontOffsetX(), n.coordinate.y + value.getFrontOffsetY(), n.coordinate.z + value.getFrontOffsetZ());
+
+                // check blocked.
+                if (!((destinationBB.minX <= neighbor.coordinate.x && neighbor.coordinate.x <= destinationBB.maxX &&
+                        destinationBB.minY <= neighbor.coordinate.y && neighbor.coordinate.y <= destinationBB.maxY &&
+                        destinationBB.minZ <= neighbor.coordinate.z && neighbor.coordinate.z <= destinationBB.maxZ) // near destination
+                 || !dungeonRoom.isBlocked(neighbor.coordinate.x, neighbor.coordinate.y, neighbor.coordinate.z))) { // not blocked
+                    continue;
+                }
+                if (neighbor.lastVisited == pfindIdx) continue;
+
+                boolean flag = false;
+                if (n. parent != null) {
+                    float tempGScore = n.parent.g + distSq(n.parent.coordinate.x - neighbor.coordinate.x, n.parent.coordinate.y - neighbor.coordinate.y, n.parent.coordinate.z - neighbor.coordinate.z);
+                    if (tempGScore < neighbor.g && lineofsight(n.parent, neighbor)) {
+                        neighbor.parent = n.parent;
+                        neighbor.g = tempGScore;
+                        neighbor.f = tempGScore + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                        open.add(neighbor);
+                        flag = true;
+                    }
+                }
+                if (!flag) {
+                    float gScore = n.g + 1; // altho it's sq, it should be fine
+                    if (gScore < neighbor.g) {
+                        neighbor.parent = n;
+                        neighbor.g = gScore;
+                        neighbor.f = gScore +distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                        open.add(neighbor);
+                    } else if (neighbor.lastVisited != pfindIdx) {
+                        neighbor.f = neighbor.g + distSq(goalNode.coordinate.x - neighbor.coordinate.x, goalNode.coordinate.y - neighbor.coordinate.y, goalNode.coordinate.z - neighbor.coordinate.z);
+                        open.add(neighbor);
+                    }
+                }
+            }
+        }
+        return true;
+    }
+
+    private boolean lineofsight(Node a, Node b) {
+        if (a == null || b == null) return false;
+        float sx = a.coordinate.x, sy = a.coordinate.y, sz = a.coordinate.z;
+        int ex = b.coordinate.x, ey = b.coordinate.y, ez = b.coordinate.z;
+
+        float dx = ex - sx, dy = ey - sy, dz = ez - sz;
+        float len = distSq(dx, dy, dz);
+        dx /= len; dy /= len; dz /= len;
+
+        for (int d = 0; d <= len; d += 1) {
+            if (dungeonRoom.isBlocked(Math.round(sx), (int) Math.ceil(sy), Math.round(sz))) return false;
+            if (dungeonRoom.isBlocked(Math.round(sx)+1, (int) Math.ceil(sy), Math.round(sz)+1)) return false;
+            if (dungeonRoom.isBlocked(Math.round(sx)-1, (int) Math.ceil(sy), Math.round(sz)-1)) return false;
+            if (dungeonRoom.isBlocked(Math.round(sx)+1, (int) Math.ceil(sy), Math.round(sz)-1)) return false;
+            if (dungeonRoom.isBlocked(Math.round(sx)-1, (int) Math.ceil(sy), Math.round(sz)+1)) return false;
+            sx += dx; sy += dy; sz += dz;
+        }
+        return true;
+    }
+
+    private int manhatten(int x, int y, int z) {return Math.abs(x)+ Math.abs(y)+ Math.abs(z);}
+    private float distSq(float x, float y, float z) {
+        return MathHelper.sqrt_float(x * x + y * y + z * z);
+    }
+
+    @RequiredArgsConstructor
+    @Data
+    public static final class Node {
+        @Data
+        @RequiredArgsConstructor
+        public static final class Coordinate {
+            private final int x, y, z;
+        }
+        
+        private final Coordinate coordinate;
+        
+        private float f = Float.MAX_VALUE, g = Float.MAX_VALUE;
+        private int lastVisited;
+
+        @EqualsAndHashCode.Exclude
+        @ToString.Exclude
+        private Node parent;
+    }
+}