Upstream
/
Minestom
mirror of https://github.com/Minestom/Minestom.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Finally a homemade pathfinder

This commit is contained in:
Felix Cravic 2020-04-25 23:06:16 +02:00
parent 6f5ff41c57
commit 5980943344
6 changed files with 204 additions and 314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/java/fr/themode/demo/commands/SimpleCommand.java
View File

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

6
src/main/java/net/minestom/server/entity/EntityCreature.java
View File

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

184
src/main/java/net/minestom/server/entity/pathfinding/AStarPathfinder.java Normal file
View File

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

18
src/main/java/net/minestom/server/entity/pathfinding/EntityPathFinder.java
View File

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

298
src/main/java/net/minestom/server/entity/pathfinding/JPS.java
View File

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

4
src/main/java/net/minestom/server/instance/block/Block.java
View File

@ -722,6 +722,10 @@ public enum Block {
return blockId;
}
public boolean isAir() {
return this == AIR;
}
public boolean isSolid() {
if (blockId == 0) {
return false;