diff options
Diffstat (limited to 'src/main/java/kr/syeyoung/dungeonsguide/pathfinding')
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; + } +} |