prepend feet to the path if start is adjacent to feet

src/main/java/baritone/behavior/PathingBehavior.java

@@ -265,7 +265,7 @@ public boolean secretInternalSetGoalAndPath(PathingCommand command) {
         if (goal == null) {
             return false;
         }
-        if (goal.isInGoal(ctx.playerFeet()) || goal.isInGoal(expectedSegmentStart)) {
+        if (goal.isInGoal(ctx.playerFeet())) {
             return false;
         }
         synchronized (pathPlanLock) {
@@ -553,7 +553,7 @@ private void findPathInNewThread(final BlockPos start, final boolean talkAboutIt
         });
     }
 
-    private static AbstractNodeCostSearch createPathfinder(BlockPos start, Goal goal, IPath previous, CalculationContext context) {
+    private AbstractNodeCostSearch createPathfinder(BlockPos start, Goal goal, IPath previous, CalculationContext context) {
         Goal transformed = goal;
         if (Baritone.settings().simplifyUnloadedYCoord.value && goal instanceof IGoalRenderPos) {
             BlockPos pos = ((IGoalRenderPos) goal).getGoalPos();
@@ -562,7 +562,14 @@ private static AbstractNodeCostSearch createPathfinder(BlockPos start, Goal goal
             }
         }
         Favoring favoring = new Favoring(context.getBaritone().getPlayerContext(), previous, context);
-        return new AStarPathFinder(start.getX(), start.getY(), start.getZ(), transformed, favoring, context);
+        BetterBlockPos feet = ctx.playerFeet();
+        var realStart = new BetterBlockPos(start);
+        var sub = feet.subtract(realStart);
+        if (feet.getY() == realStart.getY() && Math.abs(sub.getX()) <= 1 && Math.abs(sub.getZ()) <= 1) {
+            realStart = feet;
+        }
+        return new AStarPathFinder(realStart, start.getX(), start.getY(), start.getZ(), transformed, favoring, context);
+
     }
 
     @Override

src/main/java/baritone/pathing/calc/AStarPathFinder.java

@@ -41,8 +41,8 @@ public final class AStarPathFinder extends AbstractNodeCostSearch {
     private final Favoring favoring;
     private final CalculationContext calcContext;
 
-    public AStarPathFinder(int startX, int startY, int startZ, Goal goal, Favoring favoring, CalculationContext context) {
-        super(startX, startY, startZ, goal, context);
+    public AStarPathFinder(BetterBlockPos realStart, int startX, int startY, int startZ, Goal goal, Favoring favoring, CalculationContext context) {
+        super(realStart, startX, startY, startZ, goal, context);
         this.favoring = favoring;
         this.calcContext = context;
     }
@@ -96,7 +96,7 @@ protected Optional<IPath> calculate0(long primaryTimeout, long failureTimeout) {
             numNodes++;
             if (goal.isInGoal(currentNode.x, currentNode.y, currentNode.z)) {
                 logDebug("Took " + (System.currentTimeMillis() - startTime) + "ms, " + numMovementsConsidered + " movements considered");
-                return Optional.of(new Path(startNode, currentNode, numNodes, goal, calcContext));
+                return Optional.of(new Path(realStart, startNode, currentNode, numNodes, goal, calcContext));
             }
             for (Moves moves : allMoves) {
                 int newX = currentNode.x + moves.xOffset;

src/main/java/baritone/pathing/calc/AbstractNodeCostSearch.java

@@ -36,6 +36,7 @@
  */
 public abstract class AbstractNodeCostSearch implements IPathFinder, Helper {
 
+    protected final BetterBlockPos realStart;
     protected final int startX;
     protected final int startY;
     protected final int startZ;
@@ -81,7 +82,8 @@ public abstract class AbstractNodeCostSearch implements IPathFinder, Helper {
      */
     protected static final double MIN_IMPROVEMENT = 0.01;
 
-    AbstractNodeCostSearch(int startX, int startY, int startZ, Goal goal, CalculationContext context) {
+    AbstractNodeCostSearch(BetterBlockPos realStart, int startX, int startY, int startZ, Goal goal, CalculationContext context) {
+        this.realStart = realStart;
         this.startX = startX;
         this.startY = startY;
         this.startZ = startZ;
@@ -177,7 +179,7 @@ protected PathNode getNodeAtPosition(int x, int y, int z, long hashCode) {
 
     @Override
     public Optional<IPath> pathToMostRecentNodeConsidered() {
-        return Optional.ofNullable(mostRecentConsidered).map(node -> new Path(startNode, node, 0, goal, context));
+        return Optional.ofNullable(mostRecentConsidered).map(node -> new Path(realStart, startNode, node, 0, goal, context));
     }
 
     @Override
@@ -208,7 +210,7 @@ protected Optional<IPath> bestSoFar(boolean logInfo, int numNodes) {
                     System.out.println("Path goes for " + Math.sqrt(dist) + " blocks");
                     logDebug("A* cost coefficient " + COEFFICIENTS[i]);
                 }
-                return Optional.of(new Path(startNode, bestSoFar[i], numNodes, goal, context));
+                return Optional.of(new Path(realStart, startNode, bestSoFar[i], numNodes, goal, context));
             }
         }
         // instead of returning bestSoFar[0], be less misleading

src/main/java/baritone/pathing/calc/Path.java

@@ -68,8 +68,9 @@ class Path extends PathBase {
 
     private volatile boolean verified;
 
-    Path(PathNode start, PathNode end, int numNodes, Goal goal, CalculationContext context) {
-        this.start = new BetterBlockPos(start.x, start.y, start.z);
+    Path(BetterBlockPos realStart, PathNode start, PathNode end, int numNodes, Goal goal, CalculationContext context) {
+        this.start = realStart;
+        var startNodePos = new BetterBlockPos(start.x, start.y, start.z);
         this.end = new BetterBlockPos(end.x, end.y, end.z);
         this.numNodes = numNodes;
         this.movements = new ArrayList<>();
@@ -85,6 +86,13 @@ class Path extends PathBase {
             tempPath.addFirst(new BetterBlockPos(current.x, current.y, current.z));
             current = current.previous;
         }
+        if (!realStart.equals(startNodePos)) {
+            PathNode fakeNode = new PathNode(realStart.x, realStart.y, realStart.z, goal);
+            fakeNode.cost = 0;
+            tempNodes.addFirst(fakeNode);
+            tempPath.addFirst(realStart);
+        }
+
         // Can't directly convert from the PathNode pseudo linked list to an array because we don't know how long it is
         // inserting into a LinkedList<E> keeps track of length, then when we addall (which calls .toArray) it's able
         // to performantly do that conversion since it knows the length.