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

299 lines
9.3 KiB
Java

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