Add an optional direction change penalty to the A* calculation

src/main/java/com/zenith/command/impl/PathfinderCommand.java

@@ -683,6 +683,13 @@ public LiteralArgumentBuilder<CommandContext> register() {
                     .addField("Jump Penalty", CONFIG.client.extra.pathfinder.jumpPenalty)
                     .primaryColor();
             })))
+            .then(literal("directionChangePenalty").then(argument("cost", doubleArg()).executes(c -> {
+                CONFIG.client.extra.pathfinder.directionChangePenalty = getDouble(c, "cost");
+                c.getSource().getEmbed()
+                    .title("Pathfinder")
+                    .addField("Direction Change Penalty", CONFIG.client.extra.pathfinder.directionChangePenalty)
+                    .primaryColor();
+            })))
             .then(literal("allowSprint").then(argument("toggle", toggle()).executes(c -> {
                 CONFIG.client.extra.pathfinder.allowSprint = getToggle(c, "toggle");
                 c.getSource().getEmbed()
@@ -995,6 +1002,7 @@ private Map<String, String> getSettingsMap() {
         settingsMap.put("blockBreakAdditionalCost", String.valueOf(CONFIG.client.extra.pathfinder.blockBreakAdditionalCost));
         settingsMap.put("blockPlacementPenalty", String.valueOf(CONFIG.client.extra.pathfinder.blockPlacementPenalty));
         settingsMap.put("jumpPenalty", String.valueOf(CONFIG.client.extra.pathfinder.jumpPenalty));
+        settingsMap.put("directionChangePenalty", String.valueOf(CONFIG.client.extra.pathfinder.directionChangePenalty));
         settingsMap.put("lavaWalkCost", String.valueOf(CONFIG.client.extra.pathfinder.lavaWalkCost));
         settingsMap.put("allowSprint", toggleStr(CONFIG.client.extra.pathfinder.allowSprint));
         settingsMap.put("allowPlace", toggleStr(CONFIG.client.extra.pathfinder.allowPlace));

src/main/java/com/zenith/feature/pathfinder/calc/AStarPathFinder.java

@@ -39,6 +39,8 @@ protected Optional<IPath> calculate0(long primaryTimeout, long failureTimeout) {
             bestSoFar[i] = startNode;
         }
         MutableMoveResult res = new MutableMoveResult();
+        final double directionChangePenalty = calcContext.directionChangePenalty;
+        final boolean applyDirectionChangePenalty = directionChangePenalty > 0;
 //        BetterWorldBorder worldBorder = new BetterWorldBorder(calcContext.world.getWorldBorder());
         long startTime = System.currentTimeMillis();
 //        boolean slowPath = Baritone.settings().slowPath.value;
@@ -103,6 +105,18 @@ protected Optional<IPath> calculate0(long primaryTimeout, long failureTimeout) {
                 if (actionCost <= 0 || Double.isNaN(actionCost)) {
                     throw new IllegalStateException(moves + " calculated implausible cost " + actionCost);
                 }
+                // Apply direction change penalty when the horizontal movement direction differs from
+                // the direction the bot used to reach the current node. This penalizes turns (e.g. 90°)
+                // which cause the bot to lose momentum/speed in-game.
+                if (applyDirectionChangePenalty && currentNode.previous != null) {
+                    int incomingDx = currentNode.x - currentNode.previous.x;
+                    int incomingDz = currentNode.z - currentNode.previous.z;
+                    int outgoingDx = res.x - currentNode.x;
+                    int outgoingDz = res.z - currentNode.z;
+                    if (incomingDx != outgoingDx || incomingDz != outgoingDz) {
+                        actionCost += directionChangePenalty;
+                    }
+                }
                 // check destination after verifying it's not COST_INF -- some movements return a static IMPOSSIBLE object with COST_INF and destination being 0,0,0 to avoid allocating a new result for every failed calculation
 //                if (moves.dynamicXZ && !worldBorder.entirelyContains(res.x, res.z)) { // see issue #218
 //                    continue;

src/main/java/com/zenith/feature/pathfinder/movement/CalculationContext.java

@@ -49,6 +49,7 @@ public class CalculationContext {
     public final double breakBlockAdditionalCost = CONFIG.client.extra.pathfinder.blockBreakAdditionalCost;
     public final double backtrackCostFavoringCoefficient = 0.5;
     public final double jumpPenalty = CONFIG.client.extra.pathfinder.jumpPenalty;
+    public final double directionChangePenalty = CONFIG.client.extra.pathfinder.directionChangePenalty;
     public final double walkOnWaterOnePenalty = 3;
 
     public final PrecomputedData precomputedData = PrecomputedData.INSTANCE;

src/main/java/com/zenith/util/config/Config.java

@@ -206,6 +206,7 @@ public static final class Pathfinder {
                 public double blockPlacementPenalty = 20.0;
                 public double blockBreakAdditionalCost = 2;
                 public double jumpPenalty = 2.0;
+                public double directionChangePenalty = 0.0;
                 public double lavaWalkCost = 200;
                 public int maxFallHeightNoWater = 3;
                 public boolean allowLongFall = false;