2020-04-24 03:25:58 +02:00
|
|
|
package net.minestom.server.entity.pathfinding;
|
2020-03-29 20:58:30 +02:00
|
|
|
|
2020-04-24 03:25:58 +02:00
|
|
|
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;
|
2020-03-29 20:58:30 +02:00
|
|
|
|
|
|
|
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;
|
2020-04-09 14:25:42 +02:00
|
|
|
//System.out.println("TOOK " + duration + " ms not found!");
|
2020-03-29 20:58:30 +02:00
|
|
|
|
|
|
|
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;
|
2020-04-09 14:25:42 +02:00
|
|
|
//System.out.println("TOOK " + duration + " ms!");
|
2020-03-29 20:58:30 +02:00
|
|
|
|
|
|
|
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());
|
2020-04-23 01:26:45 +02:00
|
|
|
return Block.fromId(blockId).isSolid();
|
2020-03-29 20:58:30 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|