Builder pathfinding (#9872)

Improves builder pathfinding, they no longer try to reach air corners of the schematic
Builders now got a dedicated pathjob for finding a suitable position to build on, which is the smallest distance from build position and building location

src/main/java/com/minecolonies/core/entity/ai/workers/AbstractEntityAIStructure.java

@@ -132,6 +132,11 @@ public enum ItemCheckResult
      */
     protected BlockPos blockToMine;
 
+    /**
+     * Block to go to when building
+     */
+    protected BlockPos gotoPos = null;
+
     /**
      * The id in the list of the last picked up item.
      */
@@ -361,15 +366,24 @@ protected IAIState structureStep()
         //Fill workFrom with the position from where the builder should build.
         //also ensure we are at that position.
         final BlockPos progress = getProgressPos() == null ? NULL_POS : getProgressPos().getA();
-        final BlockPos worldPos = structurePlacer.getB().getProgressPosInWorld(progress);
+        final BlockPos worldPos;
+        if (gotoPos != null)
+        {
+            worldPos = structurePlacer.getB().getProgressPosInWorld(gotoPos);
+        }
+        else
+        {
+            worldPos = job.getWorkOrder().getLocation();
+        }
+
         if (getProgressPos() != null)
         {
             structurePlacer.getB().setStage(getProgressPos().getB());
         }
 
-        if ((!progress.equals(NULL_POS) || blockToMine != null) && !limitReached && (blockToMine == null
-                                                                                       ? !walkToConstructionSite(worldPos)
-                                                                                       : !walkToConstructionSite(blockToMine)))
+        if ((worldPos != null || blockToMine != null) && !limitReached && (blockToMine == null
+                                                                             ? !walkToConstructionSite(worldPos)
+                                                                             : !walkToConstructionSite(blockToMine)))
         {
             return getState();
         }
@@ -470,6 +484,15 @@ else if (result.getBlockResult().getResult() == BlockPlacementResult.Result.LIMI
         }
         else
         {
+            if (structurePlacer.getB().getStage() != CLEAR_WATER)
+            {
+                gotoPos = result.getIteratorPos();
+            }
+            else
+            {
+                gotoPos = null;
+            }
+
             this.storeProgressPos(result.getIteratorPos(), structurePlacer.getB().getStage());
         }
 

src/main/java/com/minecolonies/core/entity/ai/workers/builder/EntityAIStructureBuilder.java

@@ -16,13 +16,16 @@
 import com.minecolonies.core.colony.workorders.WorkOrderBuilding;
 import com.minecolonies.core.entity.ai.workers.AbstractEntityAIStructureWithWorkOrder;
 import com.minecolonies.core.entity.ai.workers.util.BuildingStructureHandler;
+import com.minecolonies.core.entity.pathfinding.navigation.MinecoloniesAdvancedPathNavigate;
+import com.minecolonies.core.entity.pathfinding.pathjobs.PathJobMoveCloseToXNearY;
+import com.minecolonies.core.entity.pathfinding.pathresults.PathResult;
+import net.minecraft.core.BlockPos;
 import net.minecraft.network.chat.Component;
 import net.minecraft.network.chat.MutableComponent;
-import net.minecraft.world.level.block.Block;
-import net.minecraft.world.level.block.state.BlockState;
 import net.minecraft.world.entity.Entity;
 import net.minecraft.world.entity.monster.Monster;
-import net.minecraft.core.BlockPos;
+import net.minecraft.world.level.block.Block;
+import net.minecraft.world.level.block.state.BlockState;
 import org.jetbrains.annotations.NotNull;
 
 import static com.minecolonies.api.entity.ai.statemachine.states.AIWorkerState.*;
@@ -48,6 +51,11 @@ public class EntityAIStructureBuilder extends AbstractEntityAIStructureWithWorkO
      */
     private static final int LEVEL_TO_PURGE_MOBS = 4;
 
+    /**
+     * Current goto path
+     */
+    PathResult gotoPath = null;
+
     /**
      * Initialize the builder and add all his tasks.
      *
@@ -192,33 +200,36 @@ public boolean walkToConstructionSite(final BlockPos currentBlock)
     {
         if (workFrom == null)
         {
-            workFrom = findRandomPositionToWalkTo(5, currentBlock);
-            if (workFrom == null && pathBackupFactor > 10)
+            if (gotoPath == null || gotoPath.isCancelled())
             {
-                workFrom = worker.blockPosition();
+                gotoPath = ((MinecoloniesAdvancedPathNavigate) worker.getNavigation()).setPathJob(new PathJobMoveCloseToXNearY(world,
+                  currentBlock,
+                  job.getWorkOrder().getLocation(),
+                  5,
+                  worker), currentBlock, 1.0, false);
+            }
+            else if (gotoPath.isDone())
+            {
+                if (gotoPath.getPath() != null)
+                {
+                    workFrom = gotoPath.getPath().getTarget();
+                }
+                gotoPath = null;
             }
-            return false;
-        }
 
-        if (BlockPosUtil.getDistance2D(worker.blockPosition(), currentBlock) <= 5L + (pathBackupFactor * 5L))
-        {
-            return true;
+            return false;
         }
 
         if (walkToBlock(workFrom))
         {
             return false;
         }
 
-        if (BlockPosUtil.getDistance2D(worker.blockPosition(), currentBlock) > 5L + (pathBackupFactor * 5L))
+        if (BlockPosUtil.getDistance2D(worker.blockPosition(), currentBlock) > 5)
         {
+            double distToBuilding = BlockPosUtil.dist(workFrom, job.getWorkOrder().getLocation());
             workFrom = null;
-            return false;
-        }
-
-        if (pathBackupFactor > 1)
-        {
-            pathBackupFactor--;
+            return distToBuilding < 100;
         }
 
         return true;

src/main/java/com/minecolonies/core/entity/pathfinding/navigation/MinecoloniesAdvancedPathNavigate.java

@@ -188,7 +188,13 @@ public PathResult<AbstractPathJob> moveToRandomPosAroundX(final int range, final
           3,
           (int) ourEntity.getAttribute(Attributes.FOLLOW_RANGE).getValue(),
           range,
-          ourEntity, pos), pos, speedFactor, true);
+          ourEntity, pos), pos, speedFactor, false);
+
+        if (result == null)
+        {
+            return null;
+        }
+
         result.getJob().getPathingOptions().withToggleCost(1).withJumpCost(1).withDropCost(1);
         return result;
     }

src/main/java/com/minecolonies/core/entity/pathfinding/navigation/PathingStuckHandler.java

@@ -223,6 +223,11 @@ public void checkStuck(final AbstractAdvancedPathNavigate navigator)
                         // Delay next action when the entity is moving
                         delayToNextUnstuckAction = Math.max(delayToNextUnstuckAction, 100);
                     }
+                    else if (lastPathIndex < 2 && navigator.getPath().getNodeCount() > 2)
+                    {
+                        // Skip ahead on the node index, incase the starting position is bad
+                        navigator.getPath().setNextNodeIndex(2);
+                    }
 
                     if ((stuckLevel == 0 || navigator.getPath().getTarget().distSqr(prevDestination) < 25))
                     {

src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/AbstractPathJob.java

@@ -498,6 +498,11 @@ private boolean reevaluteHeuristic(final MNode node, final boolean reaches)
 
         costPerEstimation = costPerEstimation / count;
 
+        if (costPerEstimation <= 0.0)
+        {
+            return false;
+        }
+
         // Detect an overstimating heuristic(not guranteed, but can check the found path)
         if (costPerEstimation < 1 || (costPerEstimation > 1.2 && !reaches))
         {

src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/PathJobMoveCloseToXNearY.java

@@ -0,0 +1,83 @@
+package com.minecolonies.core.entity.pathfinding.pathjobs;
+
+import com.minecolonies.api.util.BlockPosUtil;
+import com.minecolonies.api.util.ShapeUtil;
+import com.minecolonies.core.entity.pathfinding.MNode;
+import com.minecolonies.core.entity.pathfinding.PathfindingUtils;
+import com.minecolonies.core.entity.pathfinding.SurfaceType;
+import com.minecolonies.core.entity.pathfinding.pathresults.PathResult;
+import net.minecraft.core.BlockPos;
+import net.minecraft.world.entity.Mob;
+import net.minecraft.world.level.Level;
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * Job that handles moving close to a position near another
+ */
+public class PathJobMoveCloseToXNearY extends AbstractPathJob
+{
+    /**
+     * Position to go close to
+     */
+    protected final BlockPos desiredPosition;
+
+    /**
+     * Position to stay nearby
+     */
+    protected final BlockPos nearbyPosition;
+
+    /**
+     * Required distance to reach
+     */
+    protected final int distToDesired;
+
+    public PathJobMoveCloseToXNearY(
+      final Level world,
+      final BlockPos desiredPosition,
+      final BlockPos nearbyPosition,
+      final int distToDesired,
+      final Mob entity)
+    {
+        super(world, PathfindingUtils.prepareStart(entity), desiredPosition, new PathResult<PathJobMoveCloseToXNearY>(), entity);
+
+        this.desiredPosition = desiredPosition;
+        this.nearbyPosition = nearbyPosition;
+        this.distToDesired = distToDesired;
+        extraNodes = 20;
+        maxNodes /= 2;
+    }
+
+    @Override
+    protected double computeHeuristic(final int x, final int y, final int z)
+    {
+        return BlockPosUtil.distManhattan(desiredPosition, x, y, z) * 2 + BlockPosUtil.distManhattan(nearbyPosition, x, y, z);
+    }
+
+    @Override
+    protected boolean isAtDestination(@NotNull final MNode n)
+    {
+        return BlockPosUtil.distManhattan(desiredPosition, n.x, n.y, n.z) < distToDesired
+                 && SurfaceType.getSurfaceType(world, cachedBlockLookup.getBlockState(n.x, n.y - 1, n.z), tempWorldPos.set(n.x, n.y - 1, n.z), getPathingOptions())
+                      == SurfaceType.WALKABLE;
+    }
+
+    @Override
+    protected double getEndNodeScore(@NotNull final MNode n)
+    {
+        return BlockPosUtil.distManhattan(desiredPosition, n.x, n.y, n.z) * 2 + BlockPosUtil.distManhattan(nearbyPosition, n.x, n.y, n.z);
+    }
+
+    @Override
+    protected boolean stopOnNodeLimit(final int totalNodesVisited, final MNode bestNode, final int nodesSinceEndNode)
+    {
+        if (nodesSinceEndNode > 200)
+        {
+            return true;
+        }
+        else
+        {
+            maxNodes += 200;
+            return false;
+        }
+    }
+}