From d7e1110d462a57f9793ddf3cdf0d87fff9b4be7f Mon Sep 17 00:00:00 2001
From: fullwall <fullwall25@gmail.com>
Date: Wed, 16 Oct 2019 18:04:55 +0800
Subject: [PATCH] Normalise distance margin calculations across implementations
 and loosen up the y distance restriction

---
 .../citizensnpcs/npc/ai/AStarNavigationStrategy.java | 12 ++++++------
 .../citizensnpcs/npc/ai/MCNavigationStrategy.java    | 11 ++++++-----
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/main/src/main/java/net/citizensnpcs/npc/ai/AStarNavigationStrategy.java b/main/src/main/java/net/citizensnpcs/npc/ai/AStarNavigationStrategy.java
index d56244463..ccf542e5d 100644
--- a/main/src/main/java/net/citizensnpcs/npc/ai/AStarNavigationStrategy.java
+++ b/main/src/main/java/net/citizensnpcs/npc/ai/AStarNavigationStrategy.java
@@ -99,22 +99,22 @@ public class AStarNavigationStrategy extends AbstractPathStrategy {
                 destVector.setZ(vector.getZ() + targetFace.getModZ());
             }
         }*/
-        if (currLoc.toVector().distanceSquared(destVector) <= params.distanceMargin()) {
+        double dX = vector.getBlockX() - currLoc.getX();
+        double dZ = vector.getBlockZ() - currLoc.getZ();
+        double dY = vector.getY() - currLoc.getY();
+        double xzDistance = dX * dX + dZ * dZ;
+        if ((dY * dY) < 1 && xzDistance <= params.distanceMargin()) {
             plan.update(npc);
             if (plan.isComplete()) {
                 return true;
             }
             vector = plan.getCurrentVector();
         }
-        double dX = vector.getBlockX() - currLoc.getX();
-        double dZ = vector.getBlockZ() - currLoc.getZ();
-        double dY = vector.getY() - currLoc.getY();
-        double xzDistance = dX * dX + dZ * dZ;
-        double distance = xzDistance + dY * dY;
         if (params.debug()) {
             npc.getEntity().getWorld().playEffect(vector.toLocation(npc.getEntity().getWorld()), Effect.ENDER_SIGNAL,
                     0);
         }
+        double distance = xzDistance + dY * dY;
         if (distance > 0 && dY > NMS.getStepHeight(npc.getEntity()) && xzDistance <= 2.75) {
             NMS.setShouldJump(npc.getEntity());
         }
diff --git a/main/src/main/java/net/citizensnpcs/npc/ai/MCNavigationStrategy.java b/main/src/main/java/net/citizensnpcs/npc/ai/MCNavigationStrategy.java
index e943d7182..cd75364d4 100644
--- a/main/src/main/java/net/citizensnpcs/npc/ai/MCNavigationStrategy.java
+++ b/main/src/main/java/net/citizensnpcs/npc/ai/MCNavigationStrategy.java
@@ -38,10 +38,6 @@ public class MCNavigationStrategy extends AbstractPathStrategy {
         this.navigator = NMS.getTargetNavigator(npc.getEntity(), dest, params);
     }
 
-    private double distanceSquared() {
-        return handle.getLocation(HANDLE_LOCATION).distanceSquared(target);
-    }
-
     @Override
     public Iterable<Vector> getPath() {
         return navigator.getPath();
@@ -76,7 +72,12 @@ public class MCNavigationStrategy extends AbstractPathStrategy {
             return true;
         boolean wasFinished = navigator.update();
         parameters.run();
-        if (distanceSquared() < parameters.distanceMargin()) {
+        handle.getLocation(HANDLE_LOCATION);
+        double dX = target.getBlockX() + 0.5 - HANDLE_LOCATION.getX();
+        double dZ = target.getBlockZ() + 0.5 - HANDLE_LOCATION.getZ();
+        double dY = target.getY() - HANDLE_LOCATION.getY();
+        double xzDistance = dX * dX + dZ * dZ;
+        if ((dY * dY) < 1 && xzDistance <= parameters.distanceMargin()) {
             stop();
             return true;
         }