From 5980943344840e02cf0da65a3154d2ebc7b8a557 Mon Sep 17 00:00:00 2001 From: Felix Cravic Date: Sat, 25 Apr 2020 23:06:16 +0200 Subject: [PATCH] Finally a homemade pathfinder --- .../themode/demo/commands/SimpleCommand.java | 8 + .../server/entity/EntityCreature.java | 6 +- .../entity/pathfinding/AStarPathfinder.java | 184 +++++++++++ .../entity/pathfinding/EntityPathFinder.java | 18 +- .../server/entity/pathfinding/JPS.java | 298 ------------------ .../minestom/server/instance/block/Block.java | 4 + 6 files changed, 204 insertions(+), 314 deletions(-) create mode 100644 src/main/java/net/minestom/server/entity/pathfinding/AStarPathfinder.java delete mode 100644 src/main/java/net/minestom/server/entity/pathfinding/JPS.java diff --git a/src/main/java/fr/themode/demo/commands/SimpleCommand.java b/src/main/java/fr/themode/demo/commands/SimpleCommand.java index cb42adc58..dbbda7f9c 100644 --- a/src/main/java/fr/themode/demo/commands/SimpleCommand.java +++ b/src/main/java/fr/themode/demo/commands/SimpleCommand.java @@ -1,7 +1,9 @@ package fr.themode.demo.commands; import net.minestom.server.command.CommandProcessor; +import net.minestom.server.entity.EntityCreature; import net.minestom.server.entity.Player; +import net.minestom.server.instance.Instance; public class SimpleCommand implements CommandProcessor { @Override @@ -13,6 +15,12 @@ public class SimpleCommand implements CommandProcessor { public boolean process(Player player, String command, String[] args) { player.sendMessage("You tried the sample command!"); + Instance instance = player.getInstance(); + + for (EntityCreature creature : instance.getCreatures()) { + creature.setPathTo(player.getPosition()); + } + return true; } } diff --git a/src/main/java/net/minestom/server/entity/EntityCreature.java b/src/main/java/net/minestom/server/entity/EntityCreature.java index 860ea06fc..e88948a8f 100644 --- a/src/main/java/net/minestom/server/entity/EntityCreature.java +++ b/src/main/java/net/minestom/server/entity/EntityCreature.java @@ -131,13 +131,13 @@ public abstract class EntityCreature extends LivingEntity { public void jump(float height) { // FIXME magic value - Vector velocity = new Vector(0, height * 7, 0); + Vector velocity = new Vector(0, height * 10, 0); setVelocity(velocity, 200); } - public void moveTo(Position position) { + public void setPathTo(Position position) { pathFinder.getPath(position, blockPositions -> { - if (blockPositions.isEmpty()) { + if (blockPositions == null || blockPositions.isEmpty()) { // Didn't find path System.out.println("PATH NOT FOUND"); return; diff --git a/src/main/java/net/minestom/server/entity/pathfinding/AStarPathfinder.java b/src/main/java/net/minestom/server/entity/pathfinding/AStarPathfinder.java new file mode 100644 index 000000000..f271bec8d --- /dev/null +++ b/src/main/java/net/minestom/server/entity/pathfinding/AStarPathfinder.java @@ -0,0 +1,184 @@ +package net.minestom.server.entity.pathfinding; + +import net.minestom.server.instance.Instance; +import net.minestom.server.instance.block.Block; +import net.minestom.server.utils.BlockPosition; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedList; +import java.util.List; + +public class AStarPathfinder { + + // TODO ladder, jump, etc... + + private boolean climbLadder; + + public static LinkedList getPath(Instance instance, + BlockPosition start, BlockPosition end, + int maxCheck) { + long time = System.nanoTime(); + List open = new ArrayList<>(); + List closed = new ArrayList<>(); + + Node startNode = Node.fromBlockPosition(start); + Node endNode = Node.fromBlockPosition(end); + + open.add(startNode); + + int checkCount = 0; + + while (!open.isEmpty()) { + checkCount++; + if (checkCount >= maxCheck) + break; + + Node current = getCurrentNode(open); + open.remove(current); + closed.add(current); + + if (isTargetNode(end, current)) { + System.out.println("FOUND, RETURN: " + (System.nanoTime() - time)); + return buildPath(current); + } + + for (Node neighbor : getNeighbors(instance, current)) { + if (isInList(closed, neighbor)) + continue; + + boolean isInOpen = isInList(open, neighbor); + if (isShorter(neighbor, current) || !isInOpen) { + + neighbor.parent = current; + neighbor.g = getTentativeGScore(neighbor, current); + neighbor.f = neighbor.g + getDistance(neighbor, endNode); + if (!isInOpen) { + open.add(neighbor); + } + } + } + + } + + return null; + } + + private static List getNeighbors(Instance instance, Node current) { + List result = new ArrayList<>(); + BlockPosition currentBlockPosition = current.getBlockPosition(); + + for (int x = -1; x < 2; x++) { + for (int y = -1; y < 2; y++) { + for (int z = -1; z < 2; z++) { + if (x == 0 && y == 0 && z == 0) + continue; + BlockPosition neighbor = currentBlockPosition.clone().add(x, y, z); + if (canAccessBlock(instance, currentBlockPosition, neighbor)) { + Node node = Node.fromBlockPosition(neighbor); + result.add(node); + } + } + } + } + + return result; + } + + private static Node getCurrentNode(List open) { + Node closest = null; + + for (Node node : open) { + if (closest == null || node.f < closest.f) { + closest = node; + } + } + + return closest; + } + + private static boolean isTargetNode(BlockPosition target, Node node) { + return target.getX() == node.getX() && + target.getY() == node.getY() && + target.getZ() == node.getZ(); + } + + private static boolean canAccessBlock(Instance instance, BlockPosition current, BlockPosition target) { + if (instance.getChunkAt(target) == null) + return false; + + Block targetBlock = Block.fromId(instance.getBlockId(target)); + Block belowBlock = Block.fromId(instance.getBlockId(target.clone().add(0, -1, 0))); + + boolean result = targetBlock.isAir() && belowBlock.isSolid(); + return result; + } + + private static boolean isInList(List list, Node node) { + for (Node close : list) { + if (close.getX() == node.getX() && + close.getY() == node.getY() && + close.getZ() == node.getZ()) + return true; + } + return false; + } + + private static int getDistance(Node node1, Node node2) { + return node1.blockPosition.getDistance(node2.blockPosition); + } + + private static int getTentativeGScore(Node neighbor, Node current) { + return neighbor.g + getDistance(neighbor, current); + } + + private static boolean isShorter(Node neighbor, Node current) { + return getTentativeGScore(neighbor, current) < neighbor.g; + } + + private static LinkedList buildPath(Node finalNode) { + LinkedList result = new LinkedList<>(); + Node cache = finalNode; + while (cache != null) { + result.add(cache.blockPosition); + cache = cache.parent; + } + Collections.reverse(result); + return result; + } + + private static class Node { + public int g, h, f; + public Node parent; + private int x, y, z; + private BlockPosition blockPosition; + + public Node(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + this.blockPosition = new BlockPosition(x, y, z); + } + + public static Node fromBlockPosition(BlockPosition blockPosition) { + Node node = new Node(blockPosition.getX(), blockPosition.getY(), blockPosition.getZ()); + return node; + } + + public int getX() { + return x; + } + + public int getY() { + return y; + } + + public int getZ() { + return z; + } + + public BlockPosition getBlockPosition() { + return blockPosition; + } + } +} diff --git a/src/main/java/net/minestom/server/entity/pathfinding/EntityPathFinder.java b/src/main/java/net/minestom/server/entity/pathfinding/EntityPathFinder.java index 38148ab3b..4e930be1b 100644 --- a/src/main/java/net/minestom/server/entity/pathfinding/EntityPathFinder.java +++ b/src/main/java/net/minestom/server/entity/pathfinding/EntityPathFinder.java @@ -2,6 +2,7 @@ package net.minestom.server.entity.pathfinding; import net.minestom.server.MinecraftServer; import net.minestom.server.entity.Entity; +import net.minestom.server.instance.Instance; import net.minestom.server.utils.BlockPosition; import net.minestom.server.utils.Position; import net.minestom.server.utils.thread.MinestomThread; @@ -23,20 +24,11 @@ public class EntityPathFinder { public void getPath(Position target, Consumer> consumer) { pathfindingPool.execute(() -> { - LinkedList blockPositions = new LinkedList<>(); + Instance instance = entity.getInstance(); + BlockPosition start = entity.getPosition().toBlockPosition(); + BlockPosition end = target.toBlockPosition(); - JPS jps = new JPS(entity.getInstance(), entity.getPosition(), target); - - boolean first = true; - for (Position position : jps.getPath()) { - if (first) { - first = false; - continue; - } - blockPositions.add(position.toBlockPosition()); - } - - consumer.accept(blockPositions); + consumer.accept(AStarPathfinder.getPath(instance, start, end, 100)); }); } diff --git a/src/main/java/net/minestom/server/entity/pathfinding/JPS.java b/src/main/java/net/minestom/server/entity/pathfinding/JPS.java deleted file mode 100644 index 2285803bd..000000000 --- a/src/main/java/net/minestom/server/entity/pathfinding/JPS.java +++ /dev/null @@ -1,298 +0,0 @@ -package net.minestom.server.entity.pathfinding; - -import net.minestom.server.instance.Instance; -import net.minestom.server.instance.block.Block; -import net.minestom.server.utils.BlockPosition; -import net.minestom.server.utils.Position; - -import java.util.ArrayList; - -public class JPS { - - private Instance instance; - private Position startPosition; - private Position endPosition; - - private Node startNode; - private Node endNode; - - private boolean pathFound = false; - private ArrayList checkedNodes = new ArrayList<>(); - private ArrayList uncheckedNodes = new ArrayList<>(); - - private int maxNodeTests; - private boolean canClimbLadders; - private double maxFallDistance; - - // --- - // CONSTRUCTORS - // --- - - public JPS(Instance instance, Position start, Position end, int maxNodeTests, boolean canClimbLadders, double maxFallDistance) { - this.instance = instance; - this.startPosition = start; - this.endPosition = end; - - startNode = new Node(startPosition, 0, null); - endNode = new Node(endPosition, 0, null); - - this.maxNodeTests = maxNodeTests; - this.canClimbLadders = canClimbLadders; - this.maxFallDistance = maxFallDistance; - } - - public JPS(Instance instance, Position start, Position end) { - this(instance, start, end, 1000, false, 1); - } - - // --- - // PATHFINDING - // --- - - public Position[] getPath() { - // check if player could stand at start and endpoint, if not return empty path - if (!(canStandAt(startPosition) && canStandAt(endPosition))) - return new Position[0]; - - // time for benchmark - long nsStart = System.nanoTime(); - - uncheckedNodes.add(startNode); - - // cycle through untested nodes until a exit condition is fulfilled - while (checkedNodes.size() < maxNodeTests && pathFound == false && uncheckedNodes.size() > 0) { - Node n = uncheckedNodes.get(0); - for (Node nt : uncheckedNodes) - if (nt.getEstimatedFinalExpense() < n.getEstimatedFinalExpense()) - n = nt; - - if (n.estimatedExpenseLeft < 1) { - pathFound = true; - endNode = n; - - // print information about last node - //Bukkit.broadcastMessage(uncheckedNodes.size() + "uc " + checkedNodes.size() + "c " + round(n.expense) + "cne " + round(n.getEstimatedFinalExpense()) + "cnee "); - - break; - } - - n.getReachablePositions(); - uncheckedNodes.remove(n); - checkedNodes.add(n); - } - - // returning if no path has been found - if (!pathFound) { - float duration = (System.nanoTime() - nsStart) / 1000000f; - //System.out.println("TOOK " + duration + " ms not found!"); - - return new Position[0]; - } - - // get length of path to create array, 1 because of start - int length = 1; - Node n = endNode; - while (n.origin != null) { - n = n.origin; - length++; - } - - Position[] Positions = new Position[length]; - - //fill Array - n = endNode; - for (int i = length - 1; i > 0; i--) { - Positions[i] = n.getPosition(); - n = n.origin; - } - - Positions[0] = startNode.getPosition(); - - // outputting benchmark result - float duration = (System.nanoTime() - nsStart) / 1000000f; - //System.out.println("TOOK " + duration + " ms!"); - - return Positions; - } - - private Node getNode(Position loc) { - Node test = new Node(loc, 0, null); - - for (Node n : checkedNodes) - if (n.x == test.x && n.y == test.y && n.z == test.z) - return n; - - return test; - } - - // --- - // NODE - // --- - - public boolean isObstructed(Position loc) { - //if(loc.getBlock().getType().isSolid()) - //return true; - short blockId = instance.getBlockId(loc.toBlockPosition()); - return Block.fromId(blockId).isSolid(); - } - - // --- - // CHECKS - // --- - - public boolean canStandAt(Position loc) { - return !(isObstructed(loc) || isObstructed(loc.clone().add(0, 1, 0)) || !isObstructed(loc.clone().add(0, -1, 0))); - } - - public double distanceTo(Position loc1, Position loc2) { - double deltaX = Math.abs(loc1.getX() - loc2.getX()); - double deltaY = Math.abs(loc1.getY() - loc2.getY()); - double deltaZ = Math.abs(loc1.getZ() - loc2.getZ()); - - // euclidean distance - double distance2d = Math.sqrt(deltaX * deltaX + deltaZ * deltaZ); - double distance3d = Math.sqrt(distance2d * distance2d + deltaY * deltaY); - - return distance3d; - - // manhattan distance - //return deltaX + deltaY + deltaZ; - } - - // --- - // UTIL - // --- - - public double round(double d) { - return ((int) (d * 100)) / 100d; - } - - public class Node { - public int x; - public int y; - public int z; - public Node origin; - public double expense; - private Position position; - private BlockPosition blockPosition; - private double estimatedExpenseLeft = -1; - - // --- - // CONSTRUCTORS - // --- - - public Node(Position loc, double expense, Node origin) { - position = loc; - blockPosition = loc.toBlockPosition(); - x = blockPosition.getX(); - y = blockPosition.getY(); - z = blockPosition.getZ(); - - this.origin = origin; - - this.expense = expense; - } - - // --- - // GETTERS - // --- - - public Position getPosition() { - return position; - } - - public double getEstimatedFinalExpense() { - if (estimatedExpenseLeft == -1) - estimatedExpenseLeft = distanceTo(position, endPosition); - - return expense + 1.1 * estimatedExpenseLeft; - } - - // --- - // PATHFINDING - // --- - - public void getReachablePositions() { - //trying to get all possibly walkable blocks - for (int x = -1; x <= 1; x++) - for (int z = -1; z <= 1; z++) - if (!(x == 0 && z == 0) && x * z == 0) { - Position loc = new Position(blockPosition.getX() + x, blockPosition.getY(), blockPosition.getZ() + z); - - // usual unchanged y - if (canStandAt(loc)) - reachNode(loc, expense + 1); - - // one block up - if (!isObstructed(loc.clone().add(-x, 2, -z))) // block above current tile, thats why subtracting x and z - { - Position nLoc = loc.clone().add(0, 1, 0); - if (canStandAt(nLoc)) - reachNode(nLoc, expense + 1.4142); - } - - // one block down or falling multiple blocks down - if (!isObstructed(loc.clone().add(0, 1, 0))) // block above possible new tile - { - Position nLoc = loc.clone().add(0, -1, 0); - if (canStandAt(nLoc)) // one block down - reachNode(nLoc, expense + 1.4142); - else if (!isObstructed(nLoc) && !isObstructed(nLoc.clone().add(0, 1, 0))) // fall - { - int drop = 1; - while (drop <= maxFallDistance && !isObstructed(loc.clone().add(0, -drop, 0))) { - Position locF = loc.clone().add(0, -drop, 0); - if (canStandAt(locF)) { - Node fallNode = addFallNode(loc, expense + 1); - fallNode.reachNode(locF, expense + drop * 2); - } - - drop++; - } - } - } - - //ladder - /*if(canClimbLadders) - if(loc.clone().add(-x, 0, -z).getBlock().getType() == Material.LADDER) - { - Position nLoc = loc.clone().add(-x, 0, -z); - int up = 1; - while(nLoc.clone().add(0, up, 0).getBlock().getType() == Material.LADDER) - up++; - - reachNode(nLoc.clone().add(0, up, 0), expense + up * 2); - }*/ - } - } - - public void reachNode(Position locThere, double expenseThere) { - Node nt = getNode(locThere); - - if (nt.origin == null && nt != startNode) // new node - { - nt.expense = expenseThere; - nt.origin = this; - - uncheckedNodes.add(nt); - - return; - } - - // no new node - if (nt.expense > expenseThere) // this way is faster to go there - { - nt.expense = expenseThere; - nt.origin = this; - } - } - - public Node addFallNode(Position loc, double expense) { - Node n = new Node(loc, expense, this); - - return n; - } - - } - -} diff --git a/src/main/java/net/minestom/server/instance/block/Block.java b/src/main/java/net/minestom/server/instance/block/Block.java index 292ffa3b6..b799ba432 100644 --- a/src/main/java/net/minestom/server/instance/block/Block.java +++ b/src/main/java/net/minestom/server/instance/block/Block.java @@ -722,6 +722,10 @@ public enum Block { return blockId; } + public boolean isAir() { + return this == AIR; + } + public boolean isSolid() { if (blockId == 0) { return false;