- Waypoints? and new JPS Pathfinding Algorithm

1 files changed, 368 insertions, 0 deletions

diff --git a/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java
new file mode 100644
index 00000000..8ee1251f
--- /dev/null
+++ b/src/main/java/kr/syeyoung/dungeonsguide/pathfinding/JPSPathfinder.java
@@ -0,0 +1,368 @@
+/*
+ * 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 lombok.Data;
+import lombok.EqualsAndHashCode;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+import net.minecraft.block.Block;
+import net.minecraft.block.state.IBlockState;
+import net.minecraft.util.*;
+import net.minecraft.world.ChunkCache;
+import net.minecraft.world.World;
+
+import java.lang.reflect.Proxy;
+import java.util.*;
+
+public class JPSPathfinder {
+    private final BlockPos min, max;
+    private final World world;
+
+    private Vec3 start;
+    private Vec3 destination;
+
+    @Getter
+    private AxisAlignedBB destinationBB;
+
+    public JPSPathfinder(World world, BlockPos min, BlockPos max ){
+        this.min = min;
+        this.max = max;
+
+        ChunkCache chunkCache = new ChunkCache(world, min, max, 0);
+        this.world =  new CachedWorld(chunkCache);
+
+        minx = min.getX() * 2; miny = min.getY() * 2; minz = min.getZ() * 2;
+        maxx = max.getX() * 2 + 2; maxy = max.getY() * 2 + 2; maxz = max.getZ() * 2 + 2;
+
+        lenx = maxx - minx;
+        leny = maxy - miny;
+        lenz = maxz - minz;
+
+    }
+
+    private IntHashMap<Node> nodeMap = new IntHashMap();
+
+    private Node openNode(int x, int y, int z)
+    {
+        int i = Node.makeHash(x, y, z);
+        Node node = (Node)this.nodeMap.lookup(i);
+
+        if (node == null)
+        {
+            node = new Node(x, y, z);
+            this.nodeMap.addKey(i, node);
+        }
+
+        return node;
+    }
+
+    @Getter
+    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 int tx, ty, tz;
+
+    private final int minx, miny, minz, maxx, maxy, maxz;
+    private final int lenx, leny, lenz;
+
+    private Node addNode(Node parent, Node jumpPt, boolean addToOpen) {
+        float ng = parent.g + distSq(jumpPt.x - parent.x, jumpPt.y - parent.y, jumpPt.z - parent.z);
+
+        if (ng < jumpPt.g) {
+            if (addToOpen)
+            open.remove(jumpPt);
+            jumpPt.g = ng;
+            jumpPt.h = jumpPt.h == -1 ? distSq(tx - jumpPt.x, ty - jumpPt.y, tz - jumpPt.z) : jumpPt.h;
+            jumpPt.f = jumpPt.h + jumpPt.g;
+            jumpPt.parent = parent;
+            if (addToOpen)
+            open.add(jumpPt);
+        }
+        return jumpPt;
+    }
+
+
+    long arr[];
+
+    public boolean pathfind(Vec3 from, Vec3 to) {
+        route.clear(); nodeMap.clearMap();
+
+        this.start = from; this.destination = to;
+        tx = (int)(to.xCoord * 2);
+        ty = (int)(to.yCoord * 2);
+        tz = (int)(to.zCoord * 2);
+
+        arr = new long[lenx *leny * lenz * 2 / 8];
+
+        destinationBB = AxisAlignedBB.fromBounds((to.xCoord - 0.6)* 2, (to.yCoord - 0.6) * 2, (to.zCoord - 0.6) * 2, (to.xCoord + 0.6) * 2, (to.yCoord + 0.6)* 2, (to.zCoord + 0.6) *2);
+        open.clear();
+        Node start;
+        open.add(start = openNode((int)from.xCoord* 2 + 1, (int)from.yCoord* 2 + 1, (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() + 3000;
+        while(!open.isEmpty()) {
+//            if (forceEnd < System.currentTimeMillis()) break;
+            Node n = open.poll();
+            n.closed= true;
+            if (minDist > n.h) {
+                minDist = n.h;
+                end = n;
+            }
+            if (n.x > destinationBB.minX && n.x < destinationBB.maxX && n.y > destinationBB.minY && n.y < destinationBB.maxY && n.z > destinationBB.minZ && n.z < destinationBB.maxZ) {
+                break;
+            }
+
+            for (Node neighbor : getNeighbors(n.parent == null ? n : n.parent, n)) {
+                expand(n.x, n.y, n.z, neighbor.x - n.x, neighbor.y - n.y, neighbor.z - n.z, n);
+            }
+        }
+
+        if (end == null) return false;
+        Node p = end;
+        while (p != null) {
+            route.addLast(new Vec3(p.x / 2.0f, p.y / 2.0f, p.z / 2.0f));
+            p = p.parent;
+        }
+
+
+        return true;
+    }
+
+    private float distSq(float x, float y, float z) {
+        return MathHelper.sqrt_float(x * x + y * y + z * z);
+    }
+
+    public Set<Node> getNeighbors(Node prevN, Node n) {
+//        if (true) throw new RuntimeException("ah");
+        int dx = MathHelper.clamp_int(n.x - prevN.x, -1, 1);
+        int dy = MathHelper.clamp_int(n.y - prevN.y, -1, 1);
+        int dz = MathHelper.clamp_int(n.z - prevN.z, -1, 1);
+        int x = n.x, y = n.y, z = n.z;
+        int nx = n.x + dx, ny = n.y + dy, nz = n.z + dz;
+
+        Set<Node> nexts = new HashSet<>();
+        int determinant = Math.abs(dx) + Math.abs(dy) + Math.abs(dz);
+        if (determinant == 0) {
+            for (int i = -1; i <= 1; i++)
+                for (int j = -1; j <= 1; j++)
+                    for (int k = -1; k <= 1; k++) {
+                        if (i == 0 && j == 0 && k == 0) continue;
+                        nexts.add(openNode(x+i, y+j, z+k));
+                    }
+        } else if (determinant == 1) {
+            nexts.add(openNode(nx,ny,nz));
+            for (int i = -1; i<=1; i++) {
+                for (int j = - 1; j<=1; j++) {
+                    if (i == 0 && j == 0) continue;
+                    if (dx != 0 && isBlocked(x, y + i, z + j)) nexts.add(openNode(nx,y+i,z+j));
+                    if (dy != 0 && isBlocked(x + i, y, z + j)) nexts.add(openNode(x+i,ny,z+j));
+                    if (dz != 0 && isBlocked(x + i, y + j, z)) nexts.add(openNode(x+i,y+j,nz));
+                }
+            }
+        } else if (determinant == 2) {
+            if (dz != 0) nexts.add(openNode(x,y,nz));
+            if (dy != 0) nexts.add(openNode(x,ny,z));
+            if (dx != 0) nexts.add(openNode(nx,y,z));
+            nexts.add(openNode(nx,ny,nz));
+            if (dx == 0) {
+                if (isBlocked(x, y, z-dz)) {
+                    nexts.add(openNode(x, ny, z-dz));
+                    if (isBlocked(x+1, y, z-dz)) nexts.add(openNode(x+1, ny, z-dz));
+                    if (isBlocked(x-1, y, z-dz)) nexts.add(openNode(x-1, ny, z-dz));
+                }
+                if (isBlocked(x, y-dy, z)) {
+                    nexts.add(openNode(x, y-dy, nz));
+                    if (isBlocked(x+1, y-dy, z))nexts.add(openNode(x+1, y-dy, nz));
+                    if (isBlocked(x-1, y-dy, z))nexts.add(openNode(x+1, y-dy, nz));
+                }
+            } else if (dy == 0) {
+                if (isBlocked(x, y, z-dz)) {
+                    nexts.add(openNode(x, ny, z-dz));
+                    if (isBlocked(x, y+1, z-dz)) nexts.add(openNode(nx, y+1, z-dz));
+                    if (isBlocked(x, y-1, z-dz)) nexts.add(openNode(nx, y-1, z-dz));
+                }
+                if (isBlocked(x-dx, y, z)) {
+                    nexts.add(openNode(x-dx, y, nz));
+                    if (isBlocked(x-dx, y+1, z))nexts.add(openNode(x-dx, y+1, nz));
+                    if (isBlocked(x-dx, y-1, z))nexts.add(openNode(x-dx, y-1, nz));
+                }
+            } else if (dz == 0) {
+                if (isBlocked(x, y-dy, z)) {
+                    nexts.add(openNode(nx, y-dy, z));
+                    if (isBlocked(x, y-dy, z+1))nexts.add(openNode(nx, y-dy, z+1));
+                    if (isBlocked(x, y-dy, z-1))nexts.add(openNode(nx, y-dy, z-1));
+                }
+                if (isBlocked(x-dx, y, z)) {
+                    nexts.add(openNode(x-dx, ny, z));
+                    if (isBlocked(x-dx, y, z+1))nexts.add(openNode(x-dx, ny, z+1));
+                    if (isBlocked(x-dx, y, z-1))nexts.add(openNode(x-dx, ny, z-1));
+                }
+            }
+        } else if (determinant == 3) {
+            nexts.add(openNode(x,y,nz));
+            nexts.add(openNode(x,ny,z));
+            nexts.add(openNode(nx,y,z));
+            nexts.add(openNode(nx,y,nz));
+            nexts.add(openNode(x,ny,nz));
+            nexts.add(openNode(nx,ny,z));
+            nexts.add(openNode(nx,ny,nz));
+
+            if (isBlocked(x,y,z-dz)) {
+                nexts.add(openNode(x,ny,z-dz));
+                nexts.add(openNode(nx,ny,z-dz));
+                nexts.add(openNode(nx,y,z-dz));
+            }
+            if (isBlocked(x-dx,y,z)) {
+                nexts.add(openNode(x-dx,ny,nz));
+                nexts.add(openNode(x-dx,ny,z));
+                nexts.add(openNode(x-dx,y,nz));
+            }
+            if (isBlocked(x,y-dy,z)) {
+                nexts.add(openNode(x,y-dy,nz));
+                nexts.add(openNode(nx,y-dy,x));
+                nexts.add(openNode(nx,y-dy,nz));
+            }
+        }
+        return nexts;
+    }
+
+    public Node expand(int x, int y, int z, int dx, int dy, int dz, Node parent) {
+        while(true) {
+            int nx =  x + dx, ny = y + dy, nz = z + dz;
+            if (isBlocked(nx, ny, nz)) return null;
+
+            if (nx > destinationBB.minX && nx < destinationBB.maxX && ny > destinationBB.minY && ny < destinationBB.maxY && nz > destinationBB.minZ && nz < destinationBB.maxZ) return addNode(parent, openNode(nx,ny,nz), true);
+
+            int determinant = Math.abs(dx) + Math.abs(dy) + Math.abs(dz);
+            if (determinant == 1) {
+                for (int i = -1; i<=1; i++) {
+                    for (int j = - 1; j<=1; j++) {
+                        if (i == 0 && j == 0) continue;
+                        if (dx != 0 && isBlocked(nx, ny + i, nz + j) && !isBlocked(nx+dx, ny + i, nz + j)) return addNode(parent, openNode(nx,ny,nz), true);
+                        if (dy != 0 && isBlocked(nx + i, ny, nz + j) && !isBlocked(nx + i, ny+dy, nz + j)) return addNode(parent, openNode(nx,ny,nz), true);
+                        if (dz != 0 && isBlocked(nx + i, ny + j , nz) && !isBlocked(nx + i, ny + j , nz+dz)) return addNode(parent, openNode(nx,ny,nz), true);
+                    }
+                }
+            } else if (determinant == 2) {
+                if ((dx != 0 && isBlocked(nx , y , z ) && !isBlocked(nx + dx, y , z))
+                        || (dy != 0 && isBlocked(x , ny  , z) && !isBlocked(x , ny+dy  , z))
+                        || (dz != 0 && isBlocked(x  , y , nz  ) && !isBlocked(x  , y , nz+dz))) return addNode(parent, openNode(nx,ny,nz), true);
+                Node co_parent = openNode(nx,ny,nz);
+                addNode(parent, co_parent,false);
+                if (dx != 0) expand(nx, ny, nz, dx, 0,0, co_parent);
+                if (dy != 0) expand(nx, ny, nz, 0, dy,0, co_parent);
+                if (dz != 0) expand(nx, ny, nz, 0, 0,dz, co_parent);
+            } else if (determinant == 3) {
+                if (isBlocked(x, ny, nz ) || isBlocked(nx, y , nz) || isBlocked(nx, ny, z)) return addNode(parent, openNode(nx,ny,nz), true);
+                Node co_parent = openNode(nx,ny,nz);
+                addNode(parent, co_parent,false);
+                expand(nx, ny, nz, dx, 0, 0, co_parent);
+                        expand(nx, ny, nz, dx, dy, 0, co_parent);
+                        expand(nx, ny, nz, dx, 0, dz, co_parent);
+                        expand(nx, ny, nz, 0, dy, 0, co_parent);
+                        expand(nx, ny, nz, 0, dy, dz, co_parent);
+                        expand(nx, ny, nz, 0, 0, dz, co_parent);
+            }
+            x = nx; y = ny; z = nz;
+        }
+    }
+
+    private static final float playerWidth = 0.3f;
+    public boolean isBlocked(int x,int y, int z) {
+        if (x < minx || z < minz || x >= maxx || z >= maxz || y < miny || y >= maxy) return true;
+        int dx = x - minx, dy = y - miny, dz = z - minz;
+        int bitIdx = dx * leny * lenz + dy * lenz + dz;
+        int location = bitIdx / 4;
+        int bitStart = (2 * (bitIdx % 4));
+        long theBit = arr[location];
+        if (((theBit >> bitStart) & 0x2) > 0) return ((theBit >> bitStart) & 1) > 0;
+            float wX = x / 2.0f, wY = y / 2.0f, wZ = z / 2.0f;
+
+
+            AxisAlignedBB bb = AxisAlignedBB.fromBounds(wX - playerWidth, wY, wZ - playerWidth, wX + playerWidth, wY + 1.9f, wZ + playerWidth);
+
+            int i = MathHelper.floor_double(bb.minX);
+            int j = MathHelper.floor_double(bb.maxX + 1.0D);
+            int k = MathHelper.floor_double(bb.minY);
+            int l = MathHelper.floor_double(bb.maxY + 1.0D);
+            int i1 = MathHelper.floor_double(bb.minZ);
+            int j1 = MathHelper.floor_double(bb.maxZ + 1.0D);
+            BlockPos.MutableBlockPos blockPos = new BlockPos.MutableBlockPos();
+
+            List<AxisAlignedBB> list = new ArrayList<>();
+            for (int k1 = i; k1 < j; ++k1) {
+                for (int l1 = i1; l1 < j1; ++l1) {
+                    for (int i2 = k - 1; i2 < l; ++i2) {
+                        blockPos.set(k1, i2, l1);
+                        IBlockState iblockstate1 = world.getBlockState(blockPos);
+                        Block b = iblockstate1.getBlock();
+                        if (!b.getMaterial().blocksMovement())continue;
+                        if (b.isFullCube() && i2 == k-1) continue;
+                        if (b.isFullCube()) {
+                            theBit |= (3L << bitStart);
+                            arr[location] = theBit;
+                            return true;
+                        }
+                        try {
+                            b.addCollisionBoxesToList(world, blockPos, iblockstate1, bb, list, null);
+                        } catch (Exception e) {
+                            return true;
+                        }
+                        if (list.size() > 0) {
+                            theBit |= (3L << bitStart);
+                            arr[location] = theBit;
+                            return true;
+                        }
+                    }
+                }
+            }
+        theBit |= 2L << bitStart;
+        arr[location] = theBit;
+        return false;
+    }
+
+
+    @RequiredArgsConstructor
+    @Data
+    public static final class Node {
+        private final int x, y, z;
+
+        private float f, g = Float.MAX_VALUE, h = -1;
+        private boolean closed;
+
+        @EqualsAndHashCode.Exclude
+        private Node parent;
+
+        public static int makeHash(int x, int y, int z)
+        {
+            return y & 255 | (x & 32767) << 8 | (z & 32767) << 24 | (x < 0 ? Integer.MIN_VALUE : 0) | (z < 0 ? 32768 : 0);
+        }
+
+        public Node close() {
+            this.closed = true;
+            return this;
+        }
+
+    }
+}