mirror of
https://github.com/Minestom/Minestom.git
synced 2024-12-26 02:57:37 +01:00
Finally a homemade pathfinder
This commit is contained in:
Felix Cravic
parent
6f5ff41c57
commit
5980943344
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
|
||||
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));
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -722,6 +722,10 @@ public enum Block {
|
||||
return blockId;
|
||||
}
|
||||
|
||||
public boolean isAir() {
|
||||
return this == AIR;
|
||||
}
|
||||
|
||||
public boolean isSolid() {
|
||||
if (blockId == 0) {
|
||||
return false;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user