- 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

10 files changed, 765 insertions, 16 deletions

diff --git a/src/main/java/kr/syeyoung/dungeonsguide/DungeonsGuide.java b/src/main/java/kr/syeyoung/dungeonsguide/DungeonsGuide.java
index ac860e78..b3467132 100755
--- a/src/main/java/kr/syeyoung/dungeonsguide/DungeonsGuide.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/DungeonsGuide.java
@@ -38,12 +38,16 @@ import kr.syeyoung.dungeonsguide.stomp.StompClient;
 import kr.syeyoung.dungeonsguide.stomp.StompInterface;
 import kr.syeyoung.dungeonsguide.utils.AhUtils;
 import kr.syeyoung.dungeonsguide.utils.TimeScoreUtil;
+import kr.syeyoung.dungeonsguide.utils.cursor.GLCursors;
 import kr.syeyoung.dungeonsguide.wsresource.StaticResourceCache;
 import lombok.Getter;
 import net.minecraft.client.Minecraft;
 import net.minecraft.client.gui.GuiButton;
 import net.minecraft.client.gui.GuiErrorScreen;
 import net.minecraft.client.gui.GuiScreen;
+import net.minecraft.client.resources.IReloadableResourceManager;
+import net.minecraft.client.resources.IResourceManager;
+import net.minecraft.client.resources.IResourceManagerReloadListener;
 import net.minecraft.client.resources.IResourcePack;
 import net.minecraft.launchwrapper.LaunchClassLoader;
 import net.minecraft.util.IChatComponent;
@@ -166,6 +170,8 @@ public class DungeonsGuide implements DGInterface, CloseListener {
         TimeScoreUtil.init();
 
         ProgressManager.pop(progressbar);
+
+        ((IReloadableResourceManager)Minecraft.getMinecraft().getResourceManager()).registerReloadListener(resourceManager -> GLCursors.setupCursors());
     }
     @Getter
     private boolean firstTimeUsingDG = false;
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/dungeon/roomfinder/DungeonRoom.java b/src/main/java/kr/syeyoung/dungeonsguide/dungeon/roomfinder/DungeonRoom.java
index 4052d2a0..2419f50a 100755
--- a/src/main/java/kr/syeyoung/dungeonsguide/dungeon/roomfinder/DungeonRoom.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/dungeon/roomfinder/DungeonRoom.java
@@ -19,7 +19,6 @@
 package kr.syeyoung.dungeonsguide.dungeon.roomfinder;
 
 import com.google.common.collect.Sets;
-import kr.syeyoung.dungeonsguide.DungeonsGuide;
 import kr.syeyoung.dungeonsguide.dungeon.DungeonContext;
 import kr.syeyoung.dungeonsguide.dungeon.MapProcessor;
 import kr.syeyoung.dungeonsguide.dungeon.data.DungeonRoomInfo;
@@ -29,9 +28,8 @@ import kr.syeyoung.dungeonsguide.dungeon.events.DungeonStateChangeEvent;
 import kr.syeyoung.dungeonsguide.dungeon.mechanics.DungeonMechanic;
 import kr.syeyoung.dungeonsguide.dungeon.mechanics.DungeonRoomDoor;
 import kr.syeyoung.dungeonsguide.features.FeatureRegistry;
-import kr.syeyoung.dungeonsguide.pathfinding.CachedWorld;
-import kr.syeyoung.dungeonsguide.pathfinding.JPSPathfinder;
-import kr.syeyoung.dungeonsguide.pathfinding.NodeProcessorDungeonRoom;
+import kr.syeyoung.dungeonsguide.features.impl.secret.FeaturePathfindStrategy;
+import kr.syeyoung.dungeonsguide.pathfinding.*;
 import kr.syeyoung.dungeonsguide.roomedit.EditingContext;
 import kr.syeyoung.dungeonsguide.roomprocessor.ProcessorFactory;
 import kr.syeyoung.dungeonsguide.roomprocessor.RoomProcessor;
@@ -102,8 +100,13 @@ public class DungeonRoom {
         this.currentState = currentState;
     }
 
+    private Map<BlockPos, AStarFineGrid> activeBetterAStar = new HashMap<>();
+    private Map<BlockPos, AStarCornerCut> activeBetterAStarCornerCut = new HashMap<>();
+    private Map<BlockPos, ThetaStar> activeThetaStar = new HashMap<>();
+
     public ScheduledFuture<List<Vec3>> createEntityPathTo(IBlockAccess blockaccess, Entity entityIn, BlockPos targetPos, float dist, int timeout) {
-        if (FeatureRegistry.SECRET_PATHFIND_STRATEGY.isEnabled()) {
+        FeaturePathfindStrategy.PathfindStrategy pathfindStrategy = FeatureRegistry.SECRET_PATHFIND_STRATEGY.getPathfindStrat();
+        if (pathfindStrategy == FeaturePathfindStrategy.PathfindStrategy.JPS_LEGACY) {
             ScheduledFuture<List<Vec3>> sf =  asyncPathFinder.schedule(() -> {
                 BlockPos min = new BlockPos(getMin().getX(), 0, getMin().getZ());
                 BlockPos max=  new BlockPos(getMax().getX(), 255, getMax().getZ());
@@ -112,6 +115,30 @@ public class DungeonRoom {
                 return pathFinder.getRoute();
             }, 0, TimeUnit.MILLISECONDS);
             return sf;
+        } else if (pathfindStrategy == FeaturePathfindStrategy.PathfindStrategy.A_STAR_FINE_GRID) {
+            ScheduledFuture<List<Vec3>> sf =  asyncPathFinder.schedule(() -> {
+                AStarFineGrid pathFinder =
+                        activeBetterAStar.computeIfAbsent(targetPos, (pos) -> new AStarFineGrid(this, new Vec3(pos.getX(), pos.getY(), pos.getZ()).addVector(0.5, 0.5, 0.5)));
+                pathFinder.pathfind(entityIn.getPositionVector(),timeout);
+                return pathFinder.getRoute();
+            }, 0, TimeUnit.MILLISECONDS);
+            return sf;
+        }else if (pathfindStrategy == FeaturePathfindStrategy.PathfindStrategy.A_STAR_DIAGONAL) {
+            ScheduledFuture<List<Vec3>> sf =  asyncPathFinder.schedule(() -> {
+                AStarCornerCut pathFinder =
+                        activeBetterAStarCornerCut.computeIfAbsent(targetPos, (pos) -> new AStarCornerCut(this, new Vec3(pos.getX(), pos.getY(), pos.getZ()).addVector(0.5, 0.5, 0.5)));
+                pathFinder.pathfind(entityIn.getPositionVector(),timeout);
+                return pathFinder.getRoute();
+            }, 0, TimeUnit.MILLISECONDS);
+            return sf;
+        } else if (pathfindStrategy == FeaturePathfindStrategy.PathfindStrategy.THETA_STAR) {
+            ScheduledFuture<List<Vec3>> sf =  asyncPathFinder.schedule(() -> {
+                ThetaStar pathFinder =
+                        activeThetaStar.computeIfAbsent(targetPos, (pos) -> new ThetaStar(this, new Vec3(pos.getX(), pos.getY(), pos.getZ()).addVector(0.5, 0.5, 0.5)));
+                pathFinder.pathfind(entityIn.getPositionVector(),timeout);
+                return pathFinder.getRoute();
+            }, 0, TimeUnit.MILLISECONDS);
+            return sf;
         } else {
             ScheduledFuture<List<Vec3>> sf =  asyncPathFinder.schedule(() -> {
                 PathFinder pathFinder = new PathFinder(nodeProcessorDungeonRoom);
@@ -276,6 +303,7 @@ public class DungeonRoom {
 
 
     long arr[];
+    // These values are doubled
     private final int minx, miny, minz, maxx, maxy, maxz;
     private final int lenx, leny, lenz;
     private static final float playerWidth = 0.3f;
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/eventlistener/DungeonListener.java b/src/main/java/kr/syeyoung/dungeonsguide/eventlistener/DungeonListener.java
index 2edf4d83..7997aec4 100755
--- a/src/main/java/kr/syeyoung/dungeonsguide/eventlistener/DungeonListener.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/eventlistener/DungeonListener.java
@@ -47,6 +47,8 @@ import net.minecraft.client.gui.GuiErrorScreen;
 import net.minecraft.client.gui.GuiScreen;
 import net.minecraft.client.renderer.GlStateManager;
 import net.minecraft.entity.passive.EntityBat;
+import net.minecraft.util.AxisAlignedBB;
+import net.minecraft.util.BlockPos;
 import net.minecraft.util.ChatComponentText;
 import net.minecraft.util.Vec3;
 import net.minecraftforge.client.event.ClientChatReceivedEvent;
@@ -339,6 +341,22 @@ public class DungeonListener {
                     }
                 }
 
+                if (FeatureRegistry.DEBUG.isEnabled() && dungeonRoom !=null) {
+
+                    Vec3 player = Minecraft.getMinecraft().thePlayer.getPositionVector();
+                    BlockPos real = new BlockPos(player.xCoord * 2, player.yCoord * 2, player.zCoord * 2);
+                    for (BlockPos allInBox : BlockPos.getAllInBox(real.add(-1, -1, -1), real.add(1, 1, 1))) {
+                        boolean blocked = dungeonRoom.isBlocked(allInBox.getX(), allInBox.getY(), allInBox.getZ());
+
+                            RenderUtils.highlightBox(
+                                    AxisAlignedBB.fromBounds(
+                                            allInBox.getX() / 2.0 - 0.1, allInBox.getY() / 2.0 - 0.1, allInBox.getZ() / 2.0 - 0.1,
+                                            allInBox.getX() / 2.0 + 0.1, allInBox.getY() / 2.0 + 0.1, allInBox.getZ() / 2.0 + 0.1
+                                    ), blocked ? new Color(0x55FF0000, true) : new Color(0x3300FF00, true), renderWorldLastEvent.partialTicks, false);
+
+                    }
+                }
+
             }
 
             if (EditingContext.getEditingContext() != null) {
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/features/FeatureRegistry.java b/src/main/java/kr/syeyoung/dungeonsguide/features/FeatureRegistry.java
index 16201dcb..7ef483e1 100644
--- a/src/main/java/kr/syeyoung/dungeonsguide/features/FeatureRegistry.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/features/FeatureRegistry.java
@@ -83,7 +83,7 @@ public class FeatureRegistry {
     public static final PathfindLineProperties SECRET_LINE_PROPERTIES_SECRET_BROWSER = register(new PathfindLineProperties("Dungeon Secrets.Secret Browser", "Line Settings", "Line Settings when pathfinding using Secret Browser", "secret.lineproperties.secretbrowser", true, SECRET_LINE_PROPERTIES_GLOBAL));
     public static final FeatureActions SECRET_ACTIONS = register(new FeatureActions());
 
-    public static final SimpleFeature SECRET_PATHFIND_STRATEGY = register(new SimpleFeature("Dungeon Secrets", "Use NEW JPS Optimization instead of standard A Star", "Faster, and accurate routes.", "secret.secretpathfind.algorithm", true));
+    public static final FeaturePathfindStrategy SECRET_PATHFIND_STRATEGY = register(new FeaturePathfindStrategy());
     public static final FeatureTogglePathfind SECRET_TOGGLE_KEY = register(new FeatureTogglePathfind());
     public static final FeatureFreezePathfind SECRET_FREEZE_LINES = register(new FeatureFreezePathfind());
     public static final FeatureCreateRefreshLine SECRET_CREATE_REFRESH_LINE = register(new FeatureCreateRefreshLine());
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/features/impl/secret/FeaturePathfindStrategy.java b/src/main/java/kr/syeyoung/dungeonsguide/features/impl/secret/FeaturePathfindStrategy.java
new file mode 100644
index 00000000..9c732dbf
--- /dev/null
+++ b/src/main/java/kr/syeyoung/dungeonsguide/features/impl/secret/FeaturePathfindStrategy.java
@@ -0,0 +1,110 @@
+/*
+ * 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.features.impl.secret;
+
+import com.google.common.base.Supplier;
+import com.google.gson.JsonObject;
+import kr.syeyoung.dungeonsguide.config.guiconfig.ConfigPanelCreator;
+import kr.syeyoung.dungeonsguide.config.guiconfig.MFeatureEdit;
+import kr.syeyoung.dungeonsguide.config.guiconfig.MParameterEdit;
+import kr.syeyoung.dungeonsguide.config.guiconfig.RootConfigPanel;
+import kr.syeyoung.dungeonsguide.features.FeatureParameter;
+import kr.syeyoung.dungeonsguide.features.SimpleFeature;
+import kr.syeyoung.dungeonsguide.features.text.PanelTextParameterConfig;
+import kr.syeyoung.dungeonsguide.features.text.StyledTextRenderer;
+import kr.syeyoung.dungeonsguide.features.text.TextHUDFeature;
+import kr.syeyoung.dungeonsguide.gui.MPanel;
+import kr.syeyoung.dungeonsguide.gui.elements.MStringSelectionButton;
+import lombok.Data;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+
+import java.awt.*;
+import java.util.Arrays;
+import java.util.stream.Collectors;
+
+public class FeaturePathfindStrategy extends SimpleFeature {
+    public FeaturePathfindStrategy() {
+        super("Dungeon Secrets", "Pathfind Algorithm", "Select pathfind algorithm used by paths", "secret.secretpathfind.algorithm", true);
+        this.parameters.put("strategy", new FeatureParameter<String>("strategy", "Pathfind Strategy", "Pathfind Strategy", "THETA_STAR", "string"));
+
+    }
+
+    @Override
+    public boolean isDisyllable() {
+        return false;
+    }
+
+    @Override
+    public String getEditRoute(RootConfigPanel rootConfigPanel) {
+        ConfigPanelCreator.map.put("base." + getKey() , new Supplier<MPanel>() {
+            @Override
+            public MPanel get() {
+
+                MFeatureEdit featureEdit = new MFeatureEdit(FeaturePathfindStrategy.this, rootConfigPanel);
+                PathfindStrategy alignment = getPathfindStrat();
+                MStringSelectionButton mStringSelectionButton = new MStringSelectionButton(Arrays.stream(PathfindStrategy.values()).map(Enum::name).collect(Collectors.toList()), alignment.name()) {
+                    @Override
+                    public Dimension getPreferredSize() {
+                        return new Dimension(150, 20);
+                    }
+                };
+                mStringSelectionButton.setOnUpdate(() -> {
+                    FeaturePathfindStrategy.this.<String>getParameter("strategy").setValue(mStringSelectionButton.getSelected());
+                    FeaturePathfindStrategy.this.<String>getParameter("strategy").setDescription(getPathfindStrat().getDescription());
+                    featureEdit.removeParameterEdit(null);
+                });
+                featureEdit.addParameterEdit("strategy", new MParameterEdit(FeaturePathfindStrategy.this, FeaturePathfindStrategy.this.<String>getParameter("strategy"), rootConfigPanel, mStringSelectionButton, (a) -> false));
+
+                for (FeatureParameter parameter: getParameters()) {
+                    if (parameter.getKey().equals("strategy")) continue;
+                    featureEdit.addParameterEdit(parameter.getKey(), new MParameterEdit(FeaturePathfindStrategy.this, parameter, rootConfigPanel));
+                }
+                return featureEdit;
+            }
+        });
+        return "base." + getKey() ;
+    }
+
+    @Getter @RequiredArgsConstructor
+    public enum PathfindStrategy {
+        THETA_STAR("The default pathfinding algorithm. It will generate sub-optimal path quickly."),
+        A_STAR_DIAGONAL("New pathfinding algorithm. It will generate path that looks like the one JPS generates"),
+        A_STAR_FINE_GRID("New pathfinding algorithm. It will generate path that kind of looks like stair"),
+        JPS_LEGACY("The improved pathfinding algorithm. Not suggested for usage. It will have problems on diagonal movements, thus giving wrong routes"),
+        A_STAR_LEGACY("The first pathfinding algorithm. It may have problem on navigating through stairs. This is the one used by Minecraft for mob pathfind.");
+
+        private final String description;
+    }
+
+    @Override
+    public void loadConfig(JsonObject jsonObject) {
+        super.loadConfig(jsonObject);
+        FeaturePathfindStrategy.PathfindStrategy alignment;
+        try {
+            alignment = PathfindStrategy.valueOf(FeaturePathfindStrategy.this.<String>getParameter("strategy").getValue());
+        } catch (Exception e) {alignment = PathfindStrategy.THETA_STAR;}
+        FeaturePathfindStrategy.this.<String>getParameter("strategy").setValue(alignment.name());
+        FeaturePathfindStrategy.this.<String>getParameter("strategy").setDescription(alignment.getDescription());
+    }
+
+    public PathfindStrategy getPathfindStrat() {
+        return PathfindStrategy.valueOf(FeaturePathfindStrategy.this.<String>getParameter("strategy").getValue());
+    }
+}
diff --git a/src/main/java/kr/syeyoung/dungeonsguide/gui/elements/MStringSelectionButton.java b/src/main/java/kr/syeyoung/dungeonsguide/gui/elements/MStringSelectionButton.java
index 7666865a..f2dcadce 100755
--- a/src/main/java/kr/syeyoung/dungeonsguide/gui/elements/MStringSelectionButton.java
+++ b/src/main/java/kr/syeyoung/dungeonsguide/gui/elements/MStringSelectionButton.java
@@ -54,8 +54,8 @@ public class MStringSelectionButton extends MPanel {
         dec.setOnActionPerformed(new Runnable() {
             @Override
             public void run() {
-                selectedIndex++;
-                if (selectedIndex >= possible.size()) selectedIndex = 0;
+                selectedIndex --;
+                if (selectedIndex < 0) selectedIndex = possible.size() - 1;
                 updateSelected();
                 onUpdate.run();
             }
@@ -63,8 +63,8 @@ public class MStringSelectionButton extends MPanel {
         inc.setOnActionPerformed(new Runnable() {
             @Override
             public void run() {
-                selectedIndex --;
-                if (selectedIndex < 0) selectedIndex = possible.size() - 1;
+                selectedIndex++;
+                if (selectedIndex >= possible.size()) selectedIndex = 0;
                 updateSelected();
                 onUpdate.run();
             }
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;
+    }
+}