269 lines
12 KiB
Java
269 lines
12 KiB
Java
|
/*
|
||
|
|
* To change this license header, choose License Headers in Project Properties.
|
||
|
|
* To change this template file, choose Tools | Templates
|
||
|
|
* and open the template in the editor.
|
||
|
|
*/
|
||
|
|
package baritone.pathfinding;
|
||
|
|
|
||
|
|
import baritone.pathfinding.goals.Goal;
|
||
|
|
import baritone.pathfinding.actions.ActionDescendTwo;
|
||
|
|
import baritone.pathfinding.actions.ActionBridge;
|
||
|
|
import baritone.pathfinding.actions.ActionClimb;
|
||
|
|
import baritone.pathfinding.actions.Action;
|
||
|
|
import baritone.pathfinding.actions.ActionDescend;
|
||
|
|
import baritone.pathfinding.actions.ActionFall;
|
||
|
|
import baritone.pathfinding.actions.ActionPillar;
|
||
|
|
import java.util.HashMap;
|
||
|
|
import java.util.Random;
|
||
|
|
import java.util.logging.Level;
|
||
|
|
import java.util.logging.Logger;
|
||
|
|
import baritone.Baritone;
|
||
|
|
import baritone.pathfinding.actions.ActionDescendThree;
|
||
|
|
import baritone.pathfinding.actions.ActionWalkDiagonal;
|
||
|
|
import baritone.util.Out;
|
||
|
|
import baritone.util.ToolSet;
|
||
|
|
import net.minecraft.client.Minecraft;
|
||
|
|
import net.minecraft.util.math.BlockPos;
|
||
|
|
import net.minecraft.util.EnumFacing;
|
||
|
|
import net.minecraft.world.chunk.EmptyChunk;
|
||
|
|
|
||
|
|
/**
|
||
|
|
*
|
||
|
|
* @author leijurv
|
||
|
|
*/
|
||
|
|
public class PathFinder {
|
||
|
|
final BlockPos start;
|
||
|
|
final Goal goal;
|
||
|
|
final HashMap<BlockPos, Node> map;
|
||
|
|
public PathFinder(BlockPos start, Goal goal) {
|
||
|
|
this.start = start;
|
||
|
|
this.goal = goal;
|
||
|
|
this.map = new HashMap<BlockPos, Node>();
|
||
|
|
}
|
||
|
|
static final double[] COEFFICIENTS = {1.5, 2, 2.5, 3, 4, 5, 10};
|
||
|
|
public static PathFinder currentlyRunning = null;
|
||
|
|
Node[] bestSoFar;
|
||
|
|
Node startNode;
|
||
|
|
Node mostRecentConsidered;
|
||
|
|
public Path getTempSolution() {
|
||
|
|
if (startNode == null || bestSoFar[0] == null) {
|
||
|
|
return null;
|
||
|
|
}
|
||
|
|
return new Path(startNode, bestSoFar[0], goal, 0);
|
||
|
|
}
|
||
|
|
public Path getMostRecentNodeConsidered() {
|
||
|
|
return mostRecentConsidered == null ? null : new Path(startNode, mostRecentConsidered, goal, 0);
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
* Do the actual path calculation. The returned path might not actually go
|
||
|
|
* to goal, but it will get as close as I could get
|
||
|
|
*
|
||
|
|
* @return
|
||
|
|
*/
|
||
|
|
public Path calculatePath() {
|
||
|
|
//a lot of these vars are local. that's because if someone tries to call this from multiple threads, they won't interfere (much)
|
||
|
|
startNode = getNodeAtPosition(start);
|
||
|
|
startNode.cost = 0;
|
||
|
|
bestSoFar = new Node[COEFFICIENTS.length];//keep track of the best node by the metric of (estimatedCostToGoal + cost / COEFFICIENTS[i])
|
||
|
|
double[] bestHeuristicSoFar = new double[COEFFICIENTS.length];
|
||
|
|
for (int i = 0; i < bestHeuristicSoFar.length; i++) {
|
||
|
|
bestHeuristicSoFar[i] = Double.MAX_VALUE;
|
||
|
|
}
|
||
|
|
OpenSet openSet = new OpenSet();
|
||
|
|
startNode.isOpen = true;
|
||
|
|
openSet.insert(startNode);
|
||
|
|
currentlyRunning = this;
|
||
|
|
long startTime = System.currentTimeMillis();
|
||
|
|
long timeoutTime = startTime + (Baritone.slowPath ? 40000 : 4000);
|
||
|
|
long lastPrintout = 0;
|
||
|
|
int numNodes = 0;
|
||
|
|
ToolSet ts = new ToolSet();
|
||
|
|
int numEmptyChunk = 0;
|
||
|
|
while (openSet.first != null && numEmptyChunk < 50 && System.currentTimeMillis() < timeoutTime) {
|
||
|
|
if (Baritone.slowPath) {
|
||
|
|
try {
|
||
|
|
Thread.sleep(100);
|
||
|
|
} catch (InterruptedException ex) {
|
||
|
|
Logger.getLogger(PathFinder.class.getName()).log(Level.SEVERE, null, ex);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
Node currentNode = openSet.removeLowest();
|
||
|
|
mostRecentConsidered = currentNode;
|
||
|
|
currentNode.isOpen = false;
|
||
|
|
currentNode.nextOpen = null;
|
||
|
|
BlockPos currentNodePos = currentNode.pos;
|
||
|
|
numNodes++;
|
||
|
|
if (System.currentTimeMillis() > lastPrintout + 1000) {//print once a second
|
||
|
|
Out.log("searching... at " + currentNodePos + ", considered " + numNodes + " nodes so far");
|
||
|
|
lastPrintout = System.currentTimeMillis();
|
||
|
|
}
|
||
|
|
if (goal.isInGoal(currentNodePos)) {
|
||
|
|
currentlyRunning = null;
|
||
|
|
return new Path(startNode, currentNode, goal, numNodes);
|
||
|
|
}
|
||
|
|
//long constructStart = System.nanoTime();
|
||
|
|
Action[] possibleActions = getConnectedPositions(currentNodePos);//actions that we could take that start at myPos, in random order
|
||
|
|
shuffle(possibleActions);
|
||
|
|
//long constructEnd = System.nanoTime();
|
||
|
|
//System.out.println(constructEnd - constructStart);
|
||
|
|
for (Action actionToGetToNeighbor : possibleActions) {
|
||
|
|
//long costStart = System.nanoTime();
|
||
|
|
double actionCost = actionToGetToNeighbor.cost(ts);
|
||
|
|
//long costEnd = System.nanoTime();
|
||
|
|
//System.out.println(actionToGetToNeighbor.getClass() + "" + (costEnd - costStart));
|
||
|
|
if (actionCost >= Action.COST_INF) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
if (Minecraft.getMinecraft().world.getChunkFromBlockCoords(actionToGetToNeighbor.to) instanceof EmptyChunk) {
|
||
|
|
numEmptyChunk++;
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
Node neighbor = getNodeAtPosition(actionToGetToNeighbor.to);
|
||
|
|
double tentativeCost = currentNode.cost + actionCost;
|
||
|
|
if (tentativeCost < neighbor.cost) {
|
||
|
|
neighbor.previous = currentNode;
|
||
|
|
neighbor.previousAction = actionToGetToNeighbor;
|
||
|
|
neighbor.cost = tentativeCost;
|
||
|
|
if (!neighbor.isOpen) {
|
||
|
|
openSet.insert(neighbor);//dont double count, dont insert into open set if it's already there
|
||
|
|
neighbor.isOpen = true;
|
||
|
|
}
|
||
|
|
for (int i = 0; i < bestSoFar.length; i++) {
|
||
|
|
double heuristic = neighbor.estimatedCostToGoal + neighbor.cost / COEFFICIENTS[i];
|
||
|
|
if (heuristic < bestHeuristicSoFar[i]) {
|
||
|
|
bestHeuristicSoFar[i] = heuristic;
|
||
|
|
bestSoFar[i] = neighbor;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
double bestDist = 0;
|
||
|
|
for (int i = 0; i < bestSoFar.length; i++) {
|
||
|
|
if (bestSoFar[i] == null) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
double dist = distFromStart(bestSoFar[i]);
|
||
|
|
if (dist > bestDist) {
|
||
|
|
bestDist = dist;
|
||
|
|
}
|
||
|
|
if (dist > MIN_DIST_PATH) {
|
||
|
|
Out.gui("A* cost coefficient " + COEFFICIENTS[i], Out.Mode.Debug);
|
||
|
|
if (COEFFICIENTS[i] >= 3) {
|
||
|
|
Out.gui("Warning: cost coefficient is greater than three! Probably means that", Out.Mode.Debug);
|
||
|
|
Out.gui("the path I found is pretty terrible (like sneak-bridging for dozens of blocks)", Out.Mode.Debug);
|
||
|
|
Out.gui("But I'm going to do it anyway, because yolo", Out.Mode.Debug);
|
||
|
|
}
|
||
|
|
Out.gui("Path goes for " + dist + " blocks", Out.Mode.Debug);
|
||
|
|
currentlyRunning = null;
|
||
|
|
return new Path(startNode, bestSoFar[i], goal, numNodes);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
Out.gui("Even with a cost coefficient of " + COEFFICIENTS[COEFFICIENTS.length - 1] + ", I couldn't get more than " + bestDist + " blocks =(", Out.Mode.Debug);
|
||
|
|
Out.gui("No path found =(", Out.Mode.Standard);
|
||
|
|
currentlyRunning = null;
|
||
|
|
return null;
|
||
|
|
}
|
||
|
|
private double distFromStart(Node n) {
|
||
|
|
int xDiff = n.pos.getX() - start.getX();
|
||
|
|
int yDiff = n.pos.getY() - start.getY();
|
||
|
|
int zDiff = n.pos.getZ() - start.getZ();
|
||
|
|
return Math.sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff);
|
||
|
|
}
|
||
|
|
private final double MIN_DIST_PATH = 5;
|
||
|
|
private Node getNodeAtPosition(BlockPos pos) {
|
||
|
|
Node alr = map.get(pos);
|
||
|
|
if (alr == null) {
|
||
|
|
Node node = new Node(pos, goal);
|
||
|
|
map.put(pos, node);
|
||
|
|
return node;
|
||
|
|
}
|
||
|
|
return alr;
|
||
|
|
}
|
||
|
|
private static Action[] getConnectedPositions(BlockPos pos) {
|
||
|
|
int x = pos.getX();
|
||
|
|
int y = pos.getY();
|
||
|
|
int z = pos.getZ();
|
||
|
|
Action[] actions = new Action[26];
|
||
|
|
actions[0] = new ActionPillar(pos);
|
||
|
|
actions[1] = new ActionBridge(pos, new BlockPos(x + 1, y, z));
|
||
|
|
actions[2] = new ActionBridge(pos, new BlockPos(x - 1, y, z));
|
||
|
|
actions[3] = new ActionBridge(pos, new BlockPos(x, y, z + 1));
|
||
|
|
actions[4] = new ActionBridge(pos, new BlockPos(x, y, z - 1));
|
||
|
|
actions[5] = new ActionClimb(pos, new BlockPos(x + 1, y + 1, z));
|
||
|
|
actions[6] = new ActionClimb(pos, new BlockPos(x - 1, y + 1, z));
|
||
|
|
actions[7] = new ActionClimb(pos, new BlockPos(x, y + 1, z + 1));
|
||
|
|
actions[8] = new ActionClimb(pos, new BlockPos(x, y + 1, z - 1));
|
||
|
|
actions[9] = new ActionDescend(pos, new BlockPos(x, y - 1, z - 1));
|
||
|
|
actions[10] = new ActionDescend(pos, new BlockPos(x, y - 1, z + 1));
|
||
|
|
actions[11] = new ActionDescend(pos, new BlockPos(x - 1, y - 1, z));
|
||
|
|
actions[12] = new ActionDescend(pos, new BlockPos(x + 1, y - 1, z));
|
||
|
|
actions[13] = new ActionFall(pos);
|
||
|
|
actions[14] = new ActionDescendTwo(pos, new BlockPos(x, y - 2, z - 1));
|
||
|
|
actions[15] = new ActionDescendTwo(pos, new BlockPos(x, y - 2, z + 1));
|
||
|
|
actions[16] = new ActionDescendTwo(pos, new BlockPos(x - 1, y - 2, z));
|
||
|
|
actions[17] = new ActionDescendTwo(pos, new BlockPos(x + 1, y - 2, z));
|
||
|
|
actions[18] = new ActionDescendThree(pos, new BlockPos(x, y - 3, z - 1));
|
||
|
|
actions[19] = new ActionDescendThree(pos, new BlockPos(x, y - 3, z + 1));
|
||
|
|
actions[20] = new ActionDescendThree(pos, new BlockPos(x - 1, y - 3, z));
|
||
|
|
actions[21] = new ActionDescendThree(pos, new BlockPos(x + 1, y - 3, z));
|
||
|
|
actions[22] = new ActionWalkDiagonal(pos, EnumFacing.NORTH, EnumFacing.WEST);
|
||
|
|
actions[23] = new ActionWalkDiagonal(pos, EnumFacing.NORTH, EnumFacing.EAST);
|
||
|
|
actions[24] = new ActionWalkDiagonal(pos, EnumFacing.SOUTH, EnumFacing.WEST);
|
||
|
|
actions[25] = new ActionWalkDiagonal(pos, EnumFacing.SOUTH, EnumFacing.EAST);
|
||
|
|
return actions;
|
||
|
|
}
|
||
|
|
private final Random random = new Random();
|
||
|
|
private void shuffle(Action[] list) {
|
||
|
|
int len = list.length;
|
||
|
|
for (int i = 0; i < len; i++) {
|
||
|
|
int j = random.nextInt(len);
|
||
|
|
Action e = list[j];
|
||
|
|
list[j] = list[i];
|
||
|
|
list[i] = e;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* My own implementation of a singly linked list
|
||
|
|
*/
|
||
|
|
private static class OpenSet {
|
||
|
|
Node first = null;
|
||
|
|
public Node removeLowest() {
|
||
|
|
if (first == null) {
|
||
|
|
return null;
|
||
|
|
}
|
||
|
|
Node current = first.nextOpen;
|
||
|
|
if (current == null) {
|
||
|
|
Node n = first;
|
||
|
|
first = null;
|
||
|
|
return n;
|
||
|
|
}
|
||
|
|
Node previous = first;
|
||
|
|
double bestValue = first.estimatedCostToGoal + first.cost;
|
||
|
|
Node bestNode = first;
|
||
|
|
Node beforeBest = null;
|
||
|
|
while (current != null) {
|
||
|
|
double comp = current.estimatedCostToGoal + current.cost;
|
||
|
|
if (comp < bestValue) {
|
||
|
|
bestValue = comp;
|
||
|
|
bestNode = current;
|
||
|
|
beforeBest = previous;
|
||
|
|
}
|
||
|
|
previous = current;
|
||
|
|
current = current.nextOpen;
|
||
|
|
}
|
||
|
|
if (beforeBest == null) {
|
||
|
|
first = first.nextOpen;
|
||
|
|
return bestNode;
|
||
|
|
}
|
||
|
|
beforeBest.nextOpen = bestNode.nextOpen;
|
||
|
|
return bestNode;
|
||
|
|
}
|
||
|
|
public void insert(Node node) {
|
||
|
|
node.nextOpen = first;
|
||
|
|
first = node;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|