From 8516eab043ed910600263789fffebfc790114868 Mon Sep 17 00:00:00 2001
From: Leijurv <leijurv@gmail.com>
Date: Tue, 7 Aug 2018 07:52:49 -0700
Subject: [PATCH] MovementDiagonal

---
 .../bot/pathing/calc/AStarPathFinder.java     |  8 ++-
 .../movement/movements/MovementDiagonal.java  | 57 +++++++++++++++++++
 2 files changed, 63 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java

diff --git a/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java b/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java
index aab1983f..8b1bf35a 100644
--- a/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java
+++ b/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java
@@ -7,8 +7,8 @@ import baritone.bot.pathing.movement.ActionCosts;
 import baritone.bot.pathing.movement.Movement;
 import baritone.bot.pathing.movement.MovementHelper;
 import baritone.bot.pathing.movement.movements.MovementAscend;
+import baritone.bot.pathing.movement.movements.MovementDiagonal;
 import baritone.bot.pathing.movement.movements.MovementDownward;
-import baritone.bot.pathing.movement.movements.MovementFall;
 import baritone.bot.pathing.movement.movements.MovementTraverse;
 import baritone.bot.pathing.path.IPath;
 import baritone.bot.utils.ToolSet;
@@ -155,7 +155,7 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
         int x = pos.getX();
         int y = pos.getY();
         int z = pos.getZ();
-        Movement[] movements = new Movement[13];
+        Movement[] movements = new Movement[17];
         movements[0] = new MovementTraverse(pos, new BlockPos(x + 1, y, z));
         movements[1] = new MovementTraverse(pos, new BlockPos(x - 1, y, z));
         movements[2] = new MovementTraverse(pos, new BlockPos(x, y, z + 1));
@@ -169,6 +169,10 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
         movements[10] = MovementHelper.generateMovementFallOrDescend(pos, EnumFacing.EAST);
         movements[11] = MovementHelper.generateMovementFallOrDescend(pos, EnumFacing.WEST);
         movements[12] = new MovementDownward(pos);
+        movements[13] = new MovementDiagonal(pos, EnumFacing.NORTH, EnumFacing.WEST);
+        movements[14] = new MovementDiagonal(pos, EnumFacing.NORTH, EnumFacing.EAST);
+        movements[15] = new MovementDiagonal(pos, EnumFacing.SOUTH, EnumFacing.WEST);
+        movements[16] = new MovementDiagonal(pos, EnumFacing.SOUTH, EnumFacing.EAST);
         /*Action[] actions = new Action[26];
         actions[0] = new ActionPillar(pos);
         actions[1] = new ActionBridge(pos, new BlockPos(x + 1, y, z));
diff --git a/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java b/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java
new file mode 100644
index 00000000..9fdea2b3
--- /dev/null
+++ b/src/main/java/baritone/bot/pathing/movement/movements/MovementDiagonal.java
@@ -0,0 +1,57 @@
+package baritone.bot.pathing.movement.movements;
+
+import baritone.bot.pathing.movement.Movement;
+import baritone.bot.pathing.movement.MovementHelper;
+import baritone.bot.pathing.movement.MovementState;
+import baritone.bot.utils.BlockStateInterface;
+import baritone.bot.utils.ToolSet;
+import net.minecraft.util.EnumFacing;
+import net.minecraft.util.math.BlockPos;
+
+public class MovementDiagonal extends Movement {
+    public MovementDiagonal(BlockPos start, EnumFacing dir1, EnumFacing dir2) {
+        this(start, start.offset(dir1), start.offset(dir2), dir2);
+        //super(start, start.offset(dir1).offset(dir2), new BlockPos[]{start.offset(dir1), start.offset(dir1).up(), start.offset(dir2), start.offset(dir2).up(), start.offset(dir1).offset(dir2), start.offset(dir1).offset(dir2).up()}, new BlockPos[]{start.offset(dir1).offset(dir2).down()});
+    }
+
+    public MovementDiagonal(BlockPos start, BlockPos dir1, BlockPos dir2, EnumFacing drr2) {
+        this(start, dir1.offset(drr2), dir1, dir2);
+    }
+
+    public MovementDiagonal(BlockPos start, BlockPos end, BlockPos dir1, BlockPos dir2) {
+        super(start, end, new BlockPos[]{dir1, dir1.up(), dir2, dir2.up(), end, end.up()}, new BlockPos[]{end.down()});
+    }
+
+    @Override
+    public MovementState updateState(MovementState state) {
+        super.updateState(state);
+        switch (state.getStatus()) {
+            case PREPPING:
+            case UNREACHABLE:
+            case FAILED:
+                return state;
+            case WAITING:
+            case RUNNING:
+                break;
+            default:
+                return state;
+        }
+        if (playerFeet().equals(dest)) {
+            state.setStatus(MovementState.MovementStatus.SUCCESS);
+            return state;
+        }
+        moveTowards(positionsToBreak[0]);
+        return state;
+    }
+
+    @Override
+    protected double calculateCost(ToolSet ts) {
+        if (getTotalHardnessOfBlocksToBreak(ts) != 0) {
+            return COST_INF;
+        }
+        if (!MovementHelper.canWalkOn(positionsToPlace[0])) {
+            return COST_INF;
+        }
+        return Math.sqrt(2) * (BlockStateInterface.isWater(src) || BlockStateInterface.isWater(dest) ? WALK_ONE_IN_WATER_COST : WALK_ONE_BLOCK_COST);
+    }
+}