Cornerfixes (#10520)

Adjust pathfinding to advance rather than creating a corner when it already exists

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

@@ -739,24 +739,18 @@ protected final void exploreInDirection(final MNode node, int dX, int dY, int dZ
               node.y + newY - nextY,
               node.z)))
             {
-                dX = 0;
-                dY = newY - nextY;
-                dZ = 0;
-
-                nextX = node.x + dX;
-                nextY = node.y + dY;
-                nextZ = node.z + dZ;
+                nextX = node.x;
+                nextY = node.y + (newY - nextY);
+                nextZ = node.z;
                 corner = true;
             }
             // If we're going down, take the air-corner before going to the lower node
             else if (!node.isCornerNode() && newY - node.y < 0 && (dX != 0 || dZ != 0) &&
                        (node.parent == null || (node.x != node.parent.x || node.y - 1 != node.parent.y
                                                   || node.z != node.parent.z)))
             {
-                dY = 0;
-
                 nextX = node.x + dX;
-                nextY = node.y + dY;
+                nextY = node.y;
                 nextZ = node.z + dZ;
 
                 corner = true;
@@ -783,7 +777,19 @@ else if (!node.isCornerNode() && newY - node.y < 0 && (dX != 0 || dZ != 0) &&
                 return;
             }
 
-            corner = true;
+            if (corner && nextNode.parent != null && (nextNode.parent.x != nextX || nextNode.parent.z != nextZ))
+            {
+                // Corner node from different direction already created, skip to using the actual next pos
+                nextX = node.x + dX;
+                nextY = newY;
+                nextZ = node.z + dZ;
+                nextNode = nodes.get(MNode.computeNodeKey(nextX, nextY, nextZ));
+                corner = false;
+            }
+            else
+            {
+                corner = true;
+            }
         }
 
         // Current node is already visited, only update nearby costs do not create new nodes
@@ -999,7 +1005,7 @@ else if (pathingOptions.dropCost != 0)
         {
             cost += pathingOptions.traverseToggleAbleCost;
         }
-        else if (!onPath && !ShapeUtil.hasCollision(cachedBlockLookup, tempWorldPos.set(x, y, z), state))
+        else if (!onPath && ShapeUtil.hasCollision(cachedBlockLookup, tempWorldPos.set(x, y, z), state))
         {
             cost += pathingOptions.walkInShapesCost;
         }