682 lines
28 KiB
Java
682 lines
28 KiB
Java
package com.hypixel.hytale.server.npc.navigation;
|
|
|
|
import com.hypixel.hytale.component.ComponentAccessor;
|
|
import com.hypixel.hytale.component.Ref;
|
|
import com.hypixel.hytale.function.function.BiToFloatFunction;
|
|
import com.hypixel.hytale.function.function.ToFloatFunction;
|
|
import com.hypixel.hytale.function.predicate.BiFloatPredicate;
|
|
import com.hypixel.hytale.logger.HytaleLogger;
|
|
import com.hypixel.hytale.math.util.MathUtil;
|
|
import com.hypixel.hytale.math.vector.Vector3d;
|
|
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
|
|
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
|
|
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
|
|
import it.unimi.dsi.fastutil.longs.Long2ObjectMap;
|
|
import it.unimi.dsi.fastutil.longs.Long2ObjectMaps;
|
|
import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap;
|
|
import it.unimi.dsi.fastutil.longs.Long2ObjectMap.Entry;
|
|
import it.unimi.dsi.fastutil.objects.ObjectArrayList;
|
|
import it.unimi.dsi.fastutil.objects.ObjectIterator;
|
|
import java.util.List;
|
|
import javax.annotation.Nonnull;
|
|
import javax.annotation.Nullable;
|
|
|
|
public class AStarBase {
|
|
public static final double FULL_STEP_THRESHOLD = 0.9999999;
|
|
public static final double REQUIRED_TARGET_DISTANCE = 9.999999994736442E-8;
|
|
public static final double HALF_STEP_THRESHOLD = 0.49999995;
|
|
public static final double ON_GRID_THRESHOLD = 0.01;
|
|
protected static final int INDEX_FRACTIONAL_BITS = 1;
|
|
protected static final int POSITION_BITS = 11;
|
|
protected static final int POSITION_OFFSET = 1024;
|
|
protected static final int POSITION_MASK = 2047;
|
|
protected int maxPathLength = 200;
|
|
protected int openNodesLimit = 80;
|
|
protected int totalNodesLimit = 400;
|
|
protected boolean canMoveDiagonal = true;
|
|
protected boolean optimizedBuildPath = true;
|
|
protected boolean isAvoidingBlockDamage;
|
|
protected boolean isRelaxedMoveConstraints;
|
|
protected final Vector3d startPosition = new Vector3d();
|
|
protected AStarEvaluator evaluator;
|
|
protected double positionToIndexOffsetX;
|
|
protected double positionToIndexOffsetY;
|
|
protected double positionToIndexOffsetZ;
|
|
protected long indexToPositionOffsetX;
|
|
protected long indexToPositionOffsetY;
|
|
protected long indexToPositionOffsetZ;
|
|
protected long startPositionIndex;
|
|
protected boolean is2D;
|
|
protected boolean projectedX;
|
|
protected boolean projectedY;
|
|
protected boolean projectedZ;
|
|
protected final Vector3d searchDirectionsWorldNormal = new Vector3d();
|
|
protected boolean searchDirectionIsDiagonalMoves;
|
|
protected boolean searchDirectionIs2D;
|
|
protected Vector3d[] searchDirections;
|
|
protected double[] searchDirectionDistances;
|
|
protected int[] inverseSearchDirections;
|
|
protected int normalsPerDirection;
|
|
protected int[] normalDirections;
|
|
protected AStarNodePool nodePool;
|
|
protected final List<AStarNode> openNodes = new ObjectArrayList();
|
|
protected final Long2ObjectMap<AStarNode> visitedBlocks = new Long2ObjectOpenHashMap();
|
|
protected int iterations;
|
|
@Nullable
|
|
protected AStarNode path;
|
|
protected AStarBase.Progress progress;
|
|
protected final Vector3d pathEnd = new Vector3d();
|
|
protected final Vector3d tempPositionVector = new Vector3d();
|
|
protected final Vector3d tempDirectionVector = new Vector3d();
|
|
|
|
public void setCanMoveDiagonal(boolean canMoveDiagonal) {
|
|
this.canMoveDiagonal = canMoveDiagonal;
|
|
}
|
|
|
|
public void setMaxPathLength(int maxPathLength) {
|
|
this.maxPathLength = maxPathLength;
|
|
}
|
|
|
|
public void setOpenNodesLimit(int openNodesLimit) {
|
|
this.openNodesLimit = openNodesLimit;
|
|
}
|
|
|
|
public void setTotalNodesLimit(int totalNodesLimit) {
|
|
this.totalNodesLimit = totalNodesLimit;
|
|
}
|
|
|
|
public void setStartPosition(@Nonnull Vector3d position) {
|
|
this.startPosition.assign(position);
|
|
}
|
|
|
|
@Nonnull
|
|
public Vector3d getStartPosition() {
|
|
return this.startPosition;
|
|
}
|
|
|
|
public void setOptimizedBuildPath(boolean optimizedBuildPath) {
|
|
this.optimizedBuildPath = optimizedBuildPath;
|
|
}
|
|
|
|
public AStarEvaluator getEvaluator() {
|
|
return this.evaluator;
|
|
}
|
|
|
|
@Nonnull
|
|
public List<AStarNode> getOpenNodes() {
|
|
return this.openNodes;
|
|
}
|
|
|
|
public int getOpenCount() {
|
|
return this.openNodes.size();
|
|
}
|
|
|
|
@Nonnull
|
|
public Long2ObjectMap<AStarNode> getVisitedBlocks() {
|
|
return this.visitedBlocks;
|
|
}
|
|
|
|
public long getStartPositionIndex() {
|
|
return this.startPositionIndex;
|
|
}
|
|
|
|
@Nullable
|
|
public AStarNode getPath() {
|
|
return this.path;
|
|
}
|
|
|
|
@Nullable
|
|
public Vector3d getPosition() {
|
|
return this.path != null ? this.path.getPosition() : null;
|
|
}
|
|
|
|
public int getLength() {
|
|
return this.path != null ? this.path.getLength() : 0;
|
|
}
|
|
|
|
public int getIterations() {
|
|
return this.iterations;
|
|
}
|
|
|
|
@Nullable
|
|
public Vector3d getEndPosition() {
|
|
return this.path != null ? this.pathEnd : null;
|
|
}
|
|
|
|
public void clearPath() {
|
|
this.path = null;
|
|
if (!this.visitedBlocks.isEmpty()) {
|
|
Long2ObjectMaps.fastForEach(this.visitedBlocks, nodeEntry -> this.nodePool.deallocate((AStarNode)nodeEntry.getValue()));
|
|
this.visitedBlocks.clear();
|
|
}
|
|
|
|
this.openNodes.clear();
|
|
this.setProgress(AStarBase.Progress.UNSTARTED);
|
|
}
|
|
|
|
public AStarBase.Progress initComputePath(
|
|
@Nonnull Ref<EntityStore> ref,
|
|
@Nonnull Vector3d start,
|
|
AStarEvaluator evaluator,
|
|
@Nonnull MotionController motionController,
|
|
@Nonnull ProbeMoveData probeMoveData,
|
|
@Nonnull AStarNodePoolProvider nodePoolProvider,
|
|
@Nonnull ComponentAccessor<EntityStore> componentAccessor
|
|
) {
|
|
this.clearPath();
|
|
this.iterations = 0;
|
|
this.evaluator = evaluator;
|
|
this.startPosition.assign(start);
|
|
this.isAvoidingBlockDamage = probeMoveData.isAvoidingBlockDamage;
|
|
this.isRelaxedMoveConstraints = probeMoveData.isRelaxedMoveConstraints;
|
|
long startBlockX = MathUtil.fastFloor(this.startPosition.x);
|
|
long startBlockY = MathUtil.fastFloor(this.startPosition.y);
|
|
long startBlockZ = MathUtil.fastFloor(this.startPosition.z);
|
|
long twoX = 2L * startBlockX;
|
|
long twoY = 2L * startBlockY;
|
|
long twoZ = 2L * startBlockZ;
|
|
double positionToIndexOffset = -1023.25;
|
|
this.positionToIndexOffsetX = twoX + positionToIndexOffset;
|
|
this.positionToIndexOffsetY = twoY + positionToIndexOffset;
|
|
this.positionToIndexOffsetZ = twoZ + positionToIndexOffset;
|
|
this.indexToPositionOffsetX = twoX + 1L - 1024L;
|
|
this.indexToPositionOffsetY = twoY + 1L - 1024L;
|
|
this.indexToPositionOffsetZ = twoZ + 1L - 1024L;
|
|
this.startPositionIndex = this.positionToIndex(this.startPosition);
|
|
Vector3d componentSelector = motionController.getComponentSelector();
|
|
this.is2D = motionController.is2D();
|
|
this.projectedX = this.is2D && componentSelector.x == 0.0;
|
|
this.projectedY = this.is2D && componentSelector.y == 0.0;
|
|
this.projectedZ = this.is2D && componentSelector.z == 0.0;
|
|
if (this.searchDirections == null
|
|
|| this.searchDirectionIs2D != this.is2D
|
|
|| this.searchDirectionIsDiagonalMoves != this.canMoveDiagonal
|
|
|| !this.searchDirectionsWorldNormal.equals(motionController.getWorldNormal())) {
|
|
this.searchDirectionIsDiagonalMoves = this.canMoveDiagonal;
|
|
this.searchDirectionIs2D = this.is2D;
|
|
this.searchDirectionsWorldNormal.assign(motionController.getWorldNormal());
|
|
int searchDirectionCount = this.is2D ? (this.canMoveDiagonal ? 8 : 4) : (this.canMoveDiagonal ? 26 : 6);
|
|
this.searchDirections = new Vector3d[searchDirectionCount];
|
|
this.searchDirectionDistances = new double[searchDirectionCount];
|
|
int directionIndex = 0;
|
|
|
|
for (double x = -1.0; x <= 1.0; x++) {
|
|
if (!this.projectedX || x == 0.0) {
|
|
for (double y = -1.0; y <= 1.0; y++) {
|
|
if (!this.projectedY || y == 0.0) {
|
|
for (double z = -1.0; z <= 1.0; z++) {
|
|
if ((!this.projectedZ || z == 0.0) && (x != 0.0 || y != 0.0 || z != 0.0)) {
|
|
Vector3d direction = new Vector3d(x, y, z);
|
|
this.searchDirections[directionIndex] = direction;
|
|
this.searchDirectionDistances[directionIndex] = direction.length();
|
|
directionIndex++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
this.inverseSearchDirections = new int[searchDirectionCount];
|
|
|
|
for (int i = 0; i < this.inverseSearchDirections.length; i++) {
|
|
this.inverseSearchDirections[i] = -1;
|
|
}
|
|
|
|
for (int i = 0; i < this.searchDirections.length - 1; i++) {
|
|
if (this.inverseSearchDirections[i] == -1) {
|
|
this.tempDirectionVector.assign(this.searchDirections[i]).negate();
|
|
|
|
for (int j = i + 1; j < this.searchDirections.length; j++) {
|
|
if (this.searchDirections[j].equals(this.tempDirectionVector)) {
|
|
this.inverseSearchDirections[i] = j;
|
|
this.inverseSearchDirections[j] = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (this.inverseSearchDirections[i] == -1) {
|
|
throw new IllegalStateException("Can't find inverse search direction");
|
|
}
|
|
}
|
|
}
|
|
|
|
if (this.is2D) {
|
|
this.normalsPerDirection = 1;
|
|
this.normalDirections = new int[this.normalsPerDirection * searchDirectionCount];
|
|
directionIndex = 0;
|
|
|
|
for (int ix = 0; ix < this.searchDirections.length; ix++) {
|
|
Vector3d direction = this.searchDirections[ix];
|
|
int endIndex = directionIndex + this.normalsPerDirection;
|
|
Vector3d oneNormal = null;
|
|
|
|
for (int jx = 0; jx < this.searchDirections.length; jx++) {
|
|
Vector3d otherDirection = this.searchDirections[jx];
|
|
if (ix != jx && direction.dot(otherDirection) == 0.0 && (oneNormal == null || oneNormal.dot(otherDirection) == 0.0)) {
|
|
this.normalDirections[directionIndex++] = jx;
|
|
if (directionIndex == endIndex) {
|
|
break;
|
|
}
|
|
|
|
oneNormal = otherDirection;
|
|
}
|
|
}
|
|
|
|
if (directionIndex != endIndex) {
|
|
throw new IllegalStateException("Can't find correct number of normals");
|
|
}
|
|
}
|
|
}
|
|
|
|
this.nodePool = nodePoolProvider.getPool(searchDirectionCount);
|
|
}
|
|
|
|
probeMoveData.setSaveSegments(false);
|
|
this.tempPositionVector
|
|
.assign(this.projectedX ? start.x : startBlockX + 0.5, this.projectedY ? start.y : startBlockY + 0.5, this.projectedZ ? start.z : startBlockZ + 0.5);
|
|
Vector3d position = this.canAdvance(ref, this.startPosition, this.tempPositionVector, motionController, probeMoveData, componentAccessor);
|
|
if (position != null) {
|
|
this.addStartNode(this.startPosition, position, motionController);
|
|
probeMoveData.setSaveSegments(true);
|
|
return this.setProgress(AStarBase.Progress.COMPUTING);
|
|
} else {
|
|
this.tempPositionVector.x = this.projectedX ? start.x : MathUtil.fastFloor(2.0 * start.x) / 2.0;
|
|
this.tempPositionVector.y = this.projectedY ? start.y : MathUtil.fastFloor(2.0 * start.y) / 2.0;
|
|
this.tempPositionVector.z = this.projectedZ ? start.z : MathUtil.fastFloor(2.0 * start.z) / 2.0;
|
|
|
|
for (double xx = this.projectedX ? 0.0 : 0.5; xx >= 0.0; xx -= 0.5) {
|
|
for (double yx = this.projectedY ? 0.0 : 0.5; yx >= 0.0; yx -= 0.5) {
|
|
for (double zx = this.projectedZ ? 0.0 : 0.5; zx >= 0.0; zx -= 0.5) {
|
|
this.tempDirectionVector.assign(xx, yx, zx).add(this.tempPositionVector);
|
|
position = this.canAdvance(ref, this.startPosition, this.tempDirectionVector, motionController, probeMoveData, componentAccessor);
|
|
if (position != null) {
|
|
this.addStartNode(this.startPosition, position, motionController);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (this.openNodes.isEmpty()) {
|
|
double startX = this.tempPositionVector.x + (this.projectedX ? 0 : -1);
|
|
double endX = this.tempPositionVector.x + (this.projectedX ? 0 : 1);
|
|
double startY = this.tempPositionVector.y + (this.projectedY ? 0 : -1);
|
|
double endY = this.tempPositionVector.y + (this.projectedY ? 0 : 1);
|
|
double startZ = this.tempPositionVector.z + (this.projectedZ ? 0 : -1);
|
|
double endZ = this.tempPositionVector.z + (this.projectedZ ? 0 : 1);
|
|
|
|
for (double xx = startX; xx <= endX; xx += 0.5) {
|
|
for (double yx = startY; yx <= endY; yx += 0.5) {
|
|
for (double zxx = startZ; zxx <= endZ; zxx += 0.5) {
|
|
this.tempDirectionVector.assign(xx, yx, zxx);
|
|
position = this.canAdvance(ref, this.startPosition, this.tempDirectionVector, motionController, probeMoveData, componentAccessor);
|
|
if (position != null) {
|
|
this.addStartNode(this.startPosition, position, motionController);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
probeMoveData.setSaveSegments(true);
|
|
return this.setProgress(this.openNodes.isEmpty() ? AStarBase.Progress.ABORTED : AStarBase.Progress.COMPUTING);
|
|
}
|
|
}
|
|
|
|
public AStarBase.Progress computePath(
|
|
@Nonnull Ref<EntityStore> ref,
|
|
@Nonnull MotionController motionController,
|
|
@Nonnull ProbeMoveData probeMoveData,
|
|
int nodesToProcess,
|
|
@Nonnull ComponentAccessor<EntityStore> componentAccessor
|
|
) {
|
|
if (this.progress != AStarBase.Progress.COMPUTING) {
|
|
return this.progress;
|
|
} else {
|
|
probeMoveData.isAvoidingBlockDamage = this.isAvoidingBlockDamage;
|
|
probeMoveData.isRelaxedMoveConstraints = this.isRelaxedMoveConstraints;
|
|
|
|
while (!this.openNodes.isEmpty() && nodesToProcess-- > 0) {
|
|
int idx = this.openNodes.size() - 1;
|
|
AStarNode node = this.openNodes.get(idx);
|
|
node.close();
|
|
if (this.evaluator.isGoalReached(ref, this, node, motionController, componentAccessor)) {
|
|
this.buildPath(node);
|
|
return this.setProgress(AStarBase.Progress.ACCOMPLISHED);
|
|
}
|
|
|
|
this.openNodes.remove(idx);
|
|
this.iterations++;
|
|
if (node.getLength() < this.maxPathLength) {
|
|
Vector3d nodePosition = node.getPosition();
|
|
AStarNode[] successors = node.getSuccessors();
|
|
int searchDirectionCount = this.searchDirections.length;
|
|
|
|
label89:
|
|
for (int directionIndex = 0; directionIndex < searchDirectionCount; directionIndex++) {
|
|
if (successors[directionIndex] == null) {
|
|
double directionLength = this.searchDirectionDistances[directionIndex];
|
|
probeMoveData.setPosition(nodePosition).setDirection(this.searchDirections[directionIndex]);
|
|
double distance = motionController.probeMove(ref, probeMoveData, componentAccessor);
|
|
double halfThreshold = directionLength * 0.49999995;
|
|
if (!(distance < halfThreshold)) {
|
|
double halfDistance = directionLength * 0.5;
|
|
probeMoveData.computePosition(halfDistance, probeMoveData.targetPosition);
|
|
long halfPositionIndex = this.positionToIndex(probeMoveData.targetPosition);
|
|
if (halfPositionIndex != -1L) {
|
|
AStarNode otherNode = (AStarNode)this.visitedBlocks.get(halfPositionIndex);
|
|
if (otherNode != null) {
|
|
this.updateNode(node, directionIndex, otherNode, motionController);
|
|
} else {
|
|
if (this.is2D) {
|
|
int firstNormalIndex = directionIndex * this.normalsPerDirection;
|
|
|
|
for (int normalIndex = firstNormalIndex; normalIndex < firstNormalIndex + this.normalsPerDirection; normalIndex++) {
|
|
Vector3d normal = this.searchDirections[this.normalDirections[normalIndex]];
|
|
long diagonalVertexIndex = this.addOffsetToIndex(halfPositionIndex, (long)normal.x, (long)normal.y, (long)normal.z);
|
|
AStarNode diagonalNode = (AStarNode)this.visitedBlocks.get(diagonalVertexIndex);
|
|
if (diagonalNode != null) {
|
|
AStarNode otherDiagonalNode = diagonalNode.getSuccessor(this.inverseSearchDirections[normalIndex]);
|
|
if (otherDiagonalNode != null) {
|
|
continue label89;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
Vector3d destination = distance >= directionLength * 0.9999999 ? probeMoveData.probePosition : probeMoveData.targetPosition;
|
|
this.addOrUpdateNode(node, directionIndex, destination, motionController, componentAccessor);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (this.openNodesLimit > 0 && this.openNodes.size() >= this.openNodesLimit) {
|
|
return this.setProgress(AStarBase.Progress.TERMINATED_OPEN_NODE_LIMIT_EXCEEDED);
|
|
}
|
|
|
|
if (this.totalNodesLimit > 0 && this.visitedBlocks.size() >= this.totalNodesLimit) {
|
|
return this.setProgress(AStarBase.Progress.TERMINATED_TOTAL_NODE_LIMIT_EXCEEDED);
|
|
}
|
|
}
|
|
}
|
|
|
|
return this.setProgress(this.openNodes.isEmpty() ? AStarBase.Progress.TERMINATED : AStarBase.Progress.COMPUTING);
|
|
}
|
|
}
|
|
|
|
public AStarBase.Progress getProgress() {
|
|
return this.progress;
|
|
}
|
|
|
|
public boolean isComputing() {
|
|
return this.progress == AStarBase.Progress.COMPUTING;
|
|
}
|
|
|
|
public float buildLongestPath() {
|
|
AStarNode node = this.buildBestPath(AStarNode::getTravelCost, (oldV, v) -> v > oldV, 0.0F);
|
|
return node == null ? 0.0F : node.getTravelCost();
|
|
}
|
|
|
|
public float buildFurthestPath() {
|
|
AStarNode node = this.buildBestPath(n -> (float)n.getPosition().distanceSquaredTo(this.startPosition), (oldV, v) -> v > oldV, 0.0F);
|
|
return node == null ? 0.0F : node.getTravelCost();
|
|
}
|
|
|
|
@Nullable
|
|
public AStarNode buildBestPath(@Nonnull ToFloatFunction<AStarNode> weight, @Nonnull BiFloatPredicate predicate, float initialValue) {
|
|
if (this.path != null) {
|
|
return null;
|
|
} else {
|
|
AStarNode node = this.findBestVisitedNode(weight, predicate, initialValue);
|
|
if (node == null) {
|
|
return null;
|
|
} else {
|
|
this.buildPath(node);
|
|
return node;
|
|
}
|
|
}
|
|
}
|
|
|
|
@Nullable
|
|
public AStarNode findBestVisitedNode(@Nonnull ToFloatFunction<AStarNode> weight, @Nonnull BiFloatPredicate predicate, float initialValue) {
|
|
if (this.visitedBlocks.isEmpty()) {
|
|
return null;
|
|
} else {
|
|
ObjectIterator<Entry<AStarNode>> iterator = Long2ObjectMaps.fastIterator(this.visitedBlocks);
|
|
float value = initialValue;
|
|
AStarNode resultNode = null;
|
|
|
|
while (iterator.hasNext()) {
|
|
AStarNode node = (AStarNode)((Entry)iterator.next()).getValue();
|
|
float v = weight.applyAsFloat(node);
|
|
if (predicate.test(value, v)) {
|
|
value = v;
|
|
resultNode = node;
|
|
}
|
|
}
|
|
|
|
return resultNode;
|
|
}
|
|
}
|
|
|
|
@Nullable
|
|
public <T> AStarNode buildBestPath(@Nonnull BiToFloatFunction<AStarNode, T> weight, @Nonnull BiFloatPredicate predicate, float initialValue, T obj) {
|
|
if (this.path != null) {
|
|
return null;
|
|
} else {
|
|
AStarNode node = this.findBestVisitedNode(weight, predicate, initialValue, obj);
|
|
if (node == null) {
|
|
return null;
|
|
} else {
|
|
this.buildPath(node);
|
|
return node;
|
|
}
|
|
}
|
|
}
|
|
|
|
@Nullable
|
|
public <T> AStarNode findBestVisitedNode(@Nonnull BiToFloatFunction<AStarNode, T> weight, @Nonnull BiFloatPredicate predicate, float initialValue, T obj) {
|
|
if (this.visitedBlocks.isEmpty()) {
|
|
return null;
|
|
} else {
|
|
ObjectIterator<Entry<AStarNode>> iterator = Long2ObjectMaps.fastIterator(this.visitedBlocks);
|
|
float value = initialValue;
|
|
AStarNode resultNode = null;
|
|
|
|
while (iterator.hasNext()) {
|
|
AStarNode node = (AStarNode)((Entry)iterator.next()).getValue();
|
|
float v = weight.applyAsFloat(node, obj);
|
|
if (predicate.test(value, v)) {
|
|
value = v;
|
|
resultNode = node;
|
|
}
|
|
}
|
|
|
|
return resultNode;
|
|
}
|
|
}
|
|
|
|
@Nonnull
|
|
public AStarDebugBase createDebugHelper(@Nonnull HytaleLogger logger) {
|
|
return new AStarDebugBase(this, logger);
|
|
}
|
|
|
|
public static long indexFromXYZ(long dx, long dy, long dz) {
|
|
return dx >= 0L && dx <= 2047L && dy >= 0L && dy <= 2047L && dz >= 0L && dz <= 2047L ? (dx << 22) + (dy << 11) + dz : -1L;
|
|
}
|
|
|
|
public static int zFromIndex(long index) {
|
|
return (int)(index & 2047L);
|
|
}
|
|
|
|
public static int yFromIndex(long index) {
|
|
return zFromIndex(index >> 11);
|
|
}
|
|
|
|
public static int xFromIndex(long index) {
|
|
return zFromIndex(index >> 22);
|
|
}
|
|
|
|
@Nonnull
|
|
public static String positionIndexToString(long index) {
|
|
double x = (xFromIndex(index) - 1024) * 0.5 + 0.5;
|
|
double y = (yFromIndex(index) - 1024) * 0.5 + 0.5;
|
|
double z = (zFromIndex(index) - 1024) * 0.5 + 0.5;
|
|
return "[" + x + "/" + y + "/" + z + "]";
|
|
}
|
|
|
|
protected AStarBase.Progress setProgress(AStarBase.Progress progress) {
|
|
this.progress = progress;
|
|
return this.progress;
|
|
}
|
|
|
|
@Nullable
|
|
protected Vector3d canAdvance(
|
|
@Nonnull Ref<EntityStore> ref,
|
|
@Nonnull Vector3d startPosition,
|
|
@Nonnull Vector3d destination,
|
|
@Nonnull MotionController motionController,
|
|
@Nonnull ProbeMoveData probeMoveData,
|
|
@Nonnull ComponentAccessor<EntityStore> componentAccessor
|
|
) {
|
|
probeMoveData.setPosition(startPosition).setTargetPosition(destination);
|
|
return probeMoveData.canAdvance(ref, motionController, 0.9999999, componentAccessor) ? probeMoveData.probePosition : null;
|
|
}
|
|
|
|
protected void addStartNode(Vector3d startPosition, @Nonnull Vector3d position, @Nonnull MotionController motionController) {
|
|
long positionIndex = this.positionToIndex(position);
|
|
float cost = this.measureWalkCost(startPosition, position, motionController);
|
|
AStarNode node = this.nodePool.allocate().initAsStartNode(position, positionIndex, cost, this.evaluator.estimateToGoal(this, position, motionController));
|
|
this.addOpenNode(node, positionIndex);
|
|
}
|
|
|
|
protected void addOpenNode(
|
|
@Nonnull AStarNode parentNode, int directionIndex, @Nonnull Vector3d position, long positionIndex, float cost, MotionController motionController
|
|
) {
|
|
int inverseSearchDirection = this.inverseSearchDirections[directionIndex];
|
|
AStarNode node = this.nodePool
|
|
.allocate()
|
|
.initWithPredecessor(
|
|
parentNode, directionIndex, position, positionIndex, inverseSearchDirection, cost, this.evaluator.estimateToGoal(this, position, motionController)
|
|
);
|
|
this.addOpenNode(node, positionIndex);
|
|
}
|
|
|
|
protected void addOpenNode(@Nonnull AStarNode node, long index) {
|
|
int predecessor = this.openNodes.size() - 1;
|
|
float totalCost = node.getTotalCost();
|
|
|
|
while (predecessor >= 0 && this.openNodes.get(predecessor).getTotalCost() < totalCost) {
|
|
predecessor--;
|
|
}
|
|
|
|
this.openNodes.add(predecessor + 1, node);
|
|
this.visitedBlocks.put(index, node);
|
|
}
|
|
|
|
protected void updateNode(@Nonnull AStarNode node, int directionIndex, @Nonnull AStarNode targetNode, @Nonnull MotionController motionController) {
|
|
if (!targetNode.isInvalid()) {
|
|
float stepCost = this.measureWalkCost(node.getPosition(), targetNode.getPosition(), motionController);
|
|
this.updateNodeCost(node, directionIndex, targetNode, stepCost);
|
|
}
|
|
}
|
|
|
|
protected void addOrUpdateNode(
|
|
@Nonnull AStarNode node,
|
|
int directionIndex,
|
|
@Nonnull Vector3d position,
|
|
@Nonnull MotionController motionController,
|
|
@Nonnull ComponentAccessor<EntityStore> componentAccessor
|
|
) {
|
|
long positionIndex = this.positionToIndex(position);
|
|
if (positionIndex != -1L && positionIndex != node.positionIndex) {
|
|
AStarNode targetNode = (AStarNode)this.visitedBlocks.get(positionIndex);
|
|
if (targetNode == null) {
|
|
if (!motionController.isValidPosition(position, componentAccessor)) {
|
|
this.visitedBlocks.put(positionIndex, this.nodePool.allocate().initAsInvalid(position, this.positionToIndex(position)));
|
|
} else {
|
|
float travelCostToNode = node.getTravelCost() + this.measureWalkCost(node.getPosition(), position, motionController);
|
|
this.addOpenNode(node, directionIndex, position, positionIndex, travelCostToNode, motionController);
|
|
}
|
|
} else if (!targetNode.isInvalid()) {
|
|
float stepCost = this.measureWalkCost(node.getPosition(), position, motionController);
|
|
this.updateNodeCost(node, directionIndex, targetNode, stepCost);
|
|
}
|
|
}
|
|
}
|
|
|
|
protected void updateNodeCost(@Nonnull AStarNode node, int directionIndex, @Nonnull AStarNode targetNode, float stepCost) {
|
|
float travelCostToNode = node.getTravelCost() + stepCost;
|
|
float delta = travelCostToNode - targetNode.getTravelCost();
|
|
if (delta < 0.0F) {
|
|
targetNode.adjustOptimalPath(node, delta, directionIndex);
|
|
node.setSuccessor(directionIndex, targetNode, this.inverseSearchDirections[directionIndex], stepCost);
|
|
} else {
|
|
node.successors[directionIndex] = AStarNode.ENTRY_NODE_TAG;
|
|
targetNode.successors[this.inverseSearchDirections[directionIndex]] = AStarNode.ENTRY_NODE_TAG;
|
|
}
|
|
}
|
|
|
|
protected long positionToIndex(@Nonnull Vector3d position) {
|
|
long dx = MathUtil.fastFloor(position.x * 2.0 - this.positionToIndexOffsetX);
|
|
long dy = MathUtil.fastFloor(position.y * 2.0 - this.positionToIndexOffsetY);
|
|
long dz = MathUtil.fastFloor(position.z * 2.0 - this.positionToIndexOffsetZ);
|
|
return indexFromXYZ(dx, dy, dz);
|
|
}
|
|
|
|
protected float measureWalkCost(Vector3d fromPosition, Vector3d toPosition, @Nonnull MotionController motionController) {
|
|
return (float)motionController.waypointDistance(fromPosition, toPosition);
|
|
}
|
|
|
|
protected void buildPath(@Nullable AStarNode endNode) {
|
|
if (endNode == null) {
|
|
this.path = null;
|
|
} else {
|
|
this.pathEnd.assign(endNode.getPosition());
|
|
if (this.optimizedBuildPath) {
|
|
AStarNode node = endNode;
|
|
int length = 1;
|
|
int lastDirection = -1;
|
|
|
|
for (AStarNode lastCorner = null; node != null; node = node.getPredecessor()) {
|
|
node.setNextNode(lastCorner, length);
|
|
if (node.getPredecessorDirection() != lastDirection) {
|
|
lastDirection = node.getPredecessorDirection();
|
|
lastCorner = node;
|
|
length++;
|
|
}
|
|
|
|
this.path = node;
|
|
}
|
|
} else {
|
|
int length = 0;
|
|
|
|
for (AStarNode nextNode = null; endNode != null; endNode = endNode.getPredecessor()) {
|
|
endNode.setNextNode(nextNode, ++length);
|
|
this.path = endNode;
|
|
nextNode = endNode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
protected long addOffsetToIndex(long index, long xSteps, long ySteps, long zSteps) {
|
|
long x = xFromIndex(index) + xSteps;
|
|
long y = yFromIndex(index) + ySteps;
|
|
long z = zFromIndex(index) + zSteps;
|
|
return indexFromXYZ(x, y, z);
|
|
}
|
|
|
|
public static enum Progress {
|
|
UNSTARTED,
|
|
ABORTED,
|
|
COMPUTING,
|
|
ACCOMPLISHED,
|
|
TERMINATED,
|
|
TERMINATED_OPEN_NODE_LIMIT_EXCEEDED,
|
|
TERMINATED_TOTAL_NODE_LIMIT_EXCEEDED;
|
|
}
|
|
}
|