Finally a homemade pathfinder

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;
     }
 }

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;

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<BlockPosition> getPath(Instance instance,
+                                                    BlockPosition start, BlockPosition end,
+                                                    int maxCheck) {
+        long time = System.nanoTime();
+        List<Node> open = new ArrayList<>();
+        List<Node> 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<Node> getNeighbors(Instance instance, Node current) {
+        List<Node> 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<Node> 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<Node> 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<BlockPosition> buildPath(Node finalNode) {
+        LinkedList<BlockPosition> 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;
+        }
+    }
+}

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<LinkedList<BlockPosition>> consumer) {
         pathfindingPool.execute(() -> {
-            LinkedList<BlockPosition> 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);
+            consumer.accept(AStarPathfinder.getPath(instance, start, end, 100));
-
-            boolean first = true;
-            for (Position position : jps.getPath()) {
-                if (first) {
-                    first = false;
-                    continue;
-                }
-                blockPositions.add(position.toBlockPosition());
-            }
-
-            consumer.accept(blockPositions);
         });
     }
 

src/main/java/net/minestom/server/entity/pathfinding/JPS.java

@@ -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<Node> checkedNodes = new ArrayList<>();
-    private ArrayList<Node> 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;
-        }
-
-    }
-
-}

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;