/* * 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 . */ 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.init.Blocks; 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 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 route = new LinkedList<>(); @Getter private PriorityQueue 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, float within, long timeout) { route.clear(); nodeMap.clearMap(); { from = new Vec3(((int)(from.xCoord * 2)) / 2.0, ((int)(from.yCoord * 2)) / 2.0, ((int)(from.zCoord* 2)) / 2.0); to = new Vec3(((int)(to.xCoord * 2)) / 2.0, ((int)(to.yCoord * 2)) / 2.0, ((int)(to.zCoord* 2)) / 2.0); } 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 - 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)); 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; while(!open.isEmpty()) { if (forceEnd < System.currentTimeMillis() && timeout != -1) 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)) { Node jumpPT = expand(n.x, n.y, n.z, neighbor.x - n.x, neighbor.y - n.y, neighbor.z - n.z); if (jumpPT == null || jumpPT.closed) continue; addNode(n, jumpPT, true); } } 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 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 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) { 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 openNode(nx,ny,nz); 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 openNode(nx,ny,nz); if (dy != 0 && isBlocked(nx + i, ny, nz + j) && !isBlocked(nx + i, ny+dy, nz + j)) return openNode(nx,ny,nz); if (dz != 0 && isBlocked(nx + i, ny + j , nz) && !isBlocked(nx + i, ny + j , nz+dz)) return openNode(nx,ny,nz); } } } 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 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); } else if (determinant == 3) { if (isBlocked(x, ny, nz ) || isBlocked(nx, y , nz) || isBlocked(nx, ny, z)) return openNode(nx,ny,nz); if (expand(nx, ny, nz, dx, 0, 0) != null || expand(nx, ny, nz, dx, dy, 0) != null || expand(nx, ny, nz, dx, 0, dz) != null || expand(nx, ny, nz, 0, dy, 0) != null || expand(nx, ny, nz, 0, dy, dz) != null || expand(nx, ny, nz, 0, 0, dz) != null) return openNode(nx,ny,nz); } 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 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 (iblockstate1.equals( NodeProcessorDungeonRoom.preBuilt)) 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; } } }