Pathfinding 1.20.5 (Minestom/Minestom#2153)

src/main/java/net/minestom/server/collision/BlockCollision.java

@@ -90,7 +90,7 @@ private static PhysicsResult stepPhysics(@NotNull BoundingBox boundingBox,
                                              @NotNull Vec velocity, @NotNull Pos entityPosition,
                                              @NotNull Block.Getter getter, boolean singleCollision) {
         // Allocate once and update values
-        SweepResult finalResult = new SweepResult(1 - Vec.EPSILON, 0, 0, 0, null, null);
+        SweepResult finalResult = new SweepResult(1 - Vec.EPSILON, 0, 0, 0, null, 0, 0, 0);
 
         boolean foundCollisionX = false, foundCollisionY = false, foundCollisionZ = false;
 
@@ -114,19 +114,19 @@ private static PhysicsResult stepPhysics(@NotNull BoundingBox boundingBox,
             if (result.collisionX()) {
                 foundCollisionX = true;
                 collisionShapes[0] = finalResult.collidedShape;
-                collidedPoints[0] = finalResult.collidedPosition;
+                collidedPoints[0] = new Vec(finalResult.collidedPositionX, finalResult.collidedPositionY, finalResult.collidedPositionZ);
                 hasCollided = true;
                 if (singleCollision) break;
             } else if (result.collisionZ()) {
                 foundCollisionZ = true;
                 collisionShapes[2] = finalResult.collidedShape;
-                collidedPoints[2] = finalResult.collidedPosition;
+                collidedPoints[2] = new Vec(finalResult.collidedPositionX, finalResult.collidedPositionY, finalResult.collidedPositionZ);
                 hasCollided = true;
                 if (singleCollision) break;
             } else if (result.collisionY()) {
                 foundCollisionY = true;
                 collisionShapes[1] = finalResult.collidedShape;
-                collidedPoints[1] = finalResult.collidedPosition;
+                collidedPoints[1] = new Vec(finalResult.collidedPositionX, finalResult.collidedPositionY, finalResult.collidedPositionZ);
                 hasCollided = true;
                 if (singleCollision) break;
             }
@@ -158,7 +158,8 @@ private static PhysicsResult computePhysics(@NotNull BoundingBox boundingBox,
                                                 @NotNull SweepResult finalResult) {
         // If the movement is small we don't need to run the expensive ray casting.
         // Positions of move less than one can have hardcoded blocks to check for every direction
-        if (velocity.length() < 1) {
+        // Diagonals are a special case which will work with fast physics
+        if (velocity.length() <= 1 || isDiagonal(velocity)) {
             fastPhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult);
         } else {
             slowPhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult);
@@ -187,14 +188,19 @@ private static PhysicsResult computePhysics(@NotNull BoundingBox boundingBox,
                 Vec.ZERO, null, null, false, finalResult);
     }
 
+    private static boolean isDiagonal(Vec velocity) {
+        return Math.abs(velocity.x()) == 1 && Math.abs(velocity.z()) == 1;
+    }
+
     private static void slowPhysics(@NotNull BoundingBox boundingBox,
                                     @NotNull Vec velocity, Pos entityPosition,
                                     @NotNull Block.Getter getter,
                                     @NotNull Vec[] allFaces,
                                     @NotNull SweepResult finalResult) {
+        BlockIterator iterator = new BlockIterator();
         // When large moves are done we need to ray-cast to find all blocks that could intersect with the movement
         for (Vec point : allFaces) {
-            BlockIterator iterator = new BlockIterator(Vec.fromPoint(point.add(entityPosition)), velocity, 0, velocity.length());
+            iterator.reset(Vec.fromPoint(point.add(entityPosition)), velocity, 0, velocity.length(), false);
             int timer = -1;
 
             while (iterator.hasNext() && timer != 0) {
@@ -298,7 +304,7 @@ static boolean checkBoundingBox(int blockX, int blockY, int blockZ,
             // don't fall out of if statement, we could end up redundantly grabbing a block, and we only need to
             // collision check against the current shape since the below shape isn't tall
             if (belowShape.relativeEnd().y() > 1) {
-                // we should always check both shapes, so no short-circuit here, to handle cases where the bounding box
+                // we should always check both shapes, so no short-circuit here, to handle properties where the bounding box
                 // hits the current solid but misses the tall solid
                 return belowShape.intersectBoxSwept(entityPosition, entityVelocity, belowPos, boundingBox, finalResult) |
                         (currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, currentPos, boundingBox, finalResult));

src/main/java/net/minestom/server/collision/BoundingBox.java

@@ -19,9 +19,11 @@ public final class BoundingBox implements Shape {
     private static final BoundingBox sneakingBoundingBox = new BoundingBox(0.6, 1.5, 0.6);
     private static final BoundingBox smallBoundingBox = new BoundingBox(0.6, 0.6, 0.6);
 
-    final static BoundingBox ZERO = new BoundingBox(0, 0, 0);
+    static final BoundingBox ZERO = new BoundingBox(0, 0, 0);
 
-    private final double width, height, depth;
+    private final double width;
+    private final double height;
+    private final double depth;
     private final Point offset;
     private Point relativeEnd;
 
@@ -52,8 +54,10 @@ public boolean intersectBox(@NotNull Point positionRelative, @NotNull BoundingBo
     @Override
     @ApiStatus.Experimental
     public boolean intersectBoxSwept(@NotNull Point rayStart, @NotNull Point rayDirection, @NotNull Point shapePos, @NotNull BoundingBox moving, @NotNull SweepResult finalResult) {
-        if (RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, this, shapePos, finalResult) ) {
-            finalResult.collidedPosition = rayStart.add(rayDirection.mul(finalResult.res));
+        if (RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, this, shapePos, finalResult)) {
+            finalResult.collidedPositionX = rayStart.x() + rayDirection.x() * finalResult.res;
+            finalResult.collidedPositionY = rayStart.y() + rayDirection.y() * finalResult.res;
+            finalResult.collidedPositionZ = rayStart.z() + rayDirection.z() * finalResult.res;
             finalResult.collidedShape = this;
             return true;
         }
@@ -167,58 +171,119 @@ public enum AxisMask {
         NONE
     }
 
-    public Iterator<Point> getBlocks(Point point) {
+    public PointIterator getBlocks(Point point) {
         return new PointIterator(this, point, AxisMask.NONE, 0);
     }
 
-    public Iterator<Point> getBlocks(Point point, AxisMask axisMask, double axis) {
+    public PointIterator getBlocks(Point point, AxisMask axisMask, double axis) {
         return new PointIterator(this, point, axisMask, axis);
     }
 
-    static class PointIterator implements Iterator<Point> {
-        private final double sx, sy, sz;
-        double x, y, z;
-        private double minX, minY, minZ, maxX, maxY, maxZ;
+    public static class MutablePoint {
+        double x;
+        double y;
+        double z;
+
+        public void set(double x, double y, double z) {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+        }
+
+        public double x() {
+            return x;
+        }
+
+        public double y() {
+            return y;
+        }
+
+        public double z() {
+            return z;
+        }
+
+        public int blockX() {
+            return (int) Math.floor(x);
+        }
+
+        public int blockY() {
+            return (int) Math.floor(y);
+        }
+
+        public int blockZ() {
+            return (int) Math.floor(z);
+        }
+    }
+
+    public static class PointIterator implements Iterator<MutablePoint> {
+        private double sx;
+        private double sy;
+        private double sz;
+        double x;
+        double y;
+        double z;
+        private double minX;
+        private double minY;
+        private double minZ;
+        private double maxX;
+        private double maxY;
+        private double maxZ;
+        private final MutablePoint point = new MutablePoint();
+
+        public PointIterator() {
+        }
 
         public PointIterator(BoundingBox boundingBox, Point p, AxisMask axisMask, double axis) {
-            minX = (int) Math.floor(boundingBox.minX() + p.x());
-            minY = (int) Math.floor(boundingBox.minY() + p.y());
-            minZ = (int) Math.floor(boundingBox.minZ() + p.z());
-            maxX = (int) Math.floor(boundingBox.maxX() + p.x());
-            maxY = (int) Math.floor(boundingBox.maxY() + p.y());
-            maxZ = (int) Math.floor(boundingBox.maxZ() + p.z());
+            reset(boundingBox, p, axisMask, axis);
+        }
+
+        public void reset(BoundingBox boundingBox, double pointX, double pointY, double pointZ, AxisMask axisMask, int axis) {
+            minX = (int) Math.floor(boundingBox.minX() + pointX);
+            minY = (int) Math.floor(boundingBox.minY() + pointY);
+            minZ = (int) Math.floor(boundingBox.minZ() + pointZ);
+            maxX = (int) Math.floor(boundingBox.maxX() + pointX);
+            maxY = (int) Math.floor(boundingBox.maxY() + pointY);
+            maxZ = (int) Math.floor(boundingBox.maxZ() + pointZ);
 
             x = minX;
             y = minY;
             z = minZ;
 
-            sx = boundingBox.minX() + p.x() - minX;
-            sy = boundingBox.minY() + p.y() - minY;
-            sz = boundingBox.minZ() + p.z() - minZ;
+            sx = boundingBox.minX() + pointX - minX;
+            sy = boundingBox.minY() + pointY - minY;
+            sz = boundingBox.minZ() + pointZ - minZ;
 
             if (axisMask == AxisMask.X) {
-                x = axis + p.x();
+                x = axis + pointX;
                 minX = x;
                 maxX = x;
             } else if (axisMask == AxisMask.Y) {
-                y = axis + p.y();
+                y = axis + pointY;
                 minY = y;
                 maxY = y;
             } else if (axisMask == AxisMask.Z) {
-                z = axis + p.z();
+                z = axis + pointZ;
                 minZ = z;
                 maxZ = z;
             }
         }
 
+        public void reset(BoundingBox boundingBox, Point p, AxisMask axisMask, double axis) {
+            reset(boundingBox, p.x(), p.y(), p.z(), axisMask, (int) axis);
+        }
+
+        public void reset(BoundingBox boundingBox, double x, double y, double z, AxisMask axisMask, double axis) {
+            reset(boundingBox, x, y, z, axisMask, (int) axis);
+        }
+
         @Override
         public boolean hasNext() {
             return x <= maxX && y <= maxY && z <= maxZ;
         }
 
         @Override
-        public Point next() {
-            var res = new Vec(x + sx, y + sy, z + sz);
+        public MutablePoint next() {
+            point.set(x + sx, y + sy, z + sz);
 
             x++;
             if (x > maxX) {
@@ -229,7 +294,7 @@ public Point next() {
                     z++;
                 }
             }
-            return res;
+            return point;
         }
     }
 

src/main/java/net/minestom/server/collision/EntityCollision.java

@@ -12,7 +12,7 @@ public static PhysicsResult checkCollision(Instance instance, BoundingBox boundi
         double minimumRes = res != null ? res.res().res : Double.MAX_VALUE;
 
         if (instance == null) return null;
-        SweepResult sweepResult = new SweepResult(minimumRes, 0, 0, 0, null, null);
+        SweepResult sweepResult = new SweepResult(minimumRes, 0, 0, 0, null, 0, 0, 0);
 
         double closestDistance = minimumRes;
         Entity closestEntity = null;

src/main/java/net/minestom/server/collision/RayUtils.java

@@ -174,6 +174,6 @@ private static double epsilon(double value) {
     }
 
     public static boolean BoundingBoxRayIntersectionCheck(Vec start, Vec direction, BoundingBox boundingBox, Pos position) {
-        return BoundingBoxIntersectionCheck(BoundingBox.ZERO, start, direction, boundingBox, position, new SweepResult(Double.MAX_VALUE, 0, 0, 0, null, null));
+        return BoundingBoxIntersectionCheck(BoundingBox.ZERO, start, direction, boundingBox, position, new SweepResult(Double.MAX_VALUE, 0, 0, 0, null, 0, 0, 0));
     }
 }

src/main/java/net/minestom/server/collision/ShapeImpl.java

@@ -181,7 +181,10 @@ public boolean intersectBoxSwept(@NotNull Point rayStart, @NotNull Point rayDire
         for (BoundingBox blockSection : collisionBoundingBoxes) {
             // Update final result if the temp result collision is sooner than the current final result
             if (RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, blockSection, shapePos, finalResult)) {
-                finalResult.collidedPosition = rayStart.add(rayDirection.mul(finalResult.res));
+                finalResult.collidedPositionX = rayStart.x() + rayDirection.x() * finalResult.res;
+                finalResult.collidedPositionY = rayStart.y() + rayDirection.y() * finalResult.res;
+                finalResult.collidedPositionZ = rayStart.z() + rayDirection.z() * finalResult.res;
+
                 finalResult.collidedShape = this;
                 hitBlock = true;
             }

src/main/java/net/minestom/server/collision/SweepResult.java

@@ -1,15 +1,11 @@
 package net.minestom.server.collision;
 
-import net.minestom.server.coordinate.Point;
-import net.minestom.server.coordinate.Pos;
-
 public final class SweepResult {
-    public static SweepResult NO_COLLISION  = new SweepResult(Double.MAX_VALUE, 0, 0, 0, null, Pos.ZERO);
+    public static SweepResult NO_COLLISION  = new SweepResult(Double.MAX_VALUE, 0, 0, 0, null, 0, 0, 0);
 
     double res;
     double normalX, normalY, normalZ;
-    Point collidedPosition;
-    Point collidedPos;
+    double collidedPositionX, collidedPositionY, collidedPositionZ;
     Shape collidedShape;
 
     /**
@@ -20,12 +16,14 @@ public final class SweepResult {
      * @param normalY -1 if intersected on bottom, 1 if intersected on top
      * @param normalZ -1 if intersected on front, 1 if intersected on back
      */
-    public SweepResult(double res, double normalX, double normalY, double normalZ, Shape collidedShape, Point collidedPos) {
+    public SweepResult(double res, double normalX, double normalY, double normalZ, Shape collidedShape, double collidedPosX, double collidedPosY, double collidedPosZ) {
         this.res = res;
         this.normalX = normalX;
         this.normalY = normalY;
         this.normalZ = normalZ;
         this.collidedShape = collidedShape;
-        this.collidedPos = collidedPos;
+        this.collidedPositionX = collidedPosX;
+        this.collidedPositionY = collidedPosY;
+        this.collidedPositionZ = collidedPosZ;
     }
 }

src/main/java/net/minestom/server/entity/EntityCreature.java

@@ -1,6 +1,5 @@
 package net.minestom.server.entity;
 
-import com.extollit.gaming.ai.path.HydrazinePathFinder;
 import net.minestom.server.coordinate.Pos;
 import net.minestom.server.entity.ai.EntityAI;
 import net.minestom.server.entity.ai.EntityAIGroup;
@@ -56,8 +55,7 @@ public void update(long time) {
 
     @Override
     public CompletableFuture<Void> setInstance(@NotNull Instance instance, @NotNull Pos spawnPosition) {
-        this.navigator.setPathFinder(new HydrazinePathFinder(navigator.getPathingEntity(), instance.getInstanceSpace()));
-
+        this.navigator.reset();
         return super.setInstance(instance, spawnPosition);
     }
 

src/main/java/net/minestom/server/entity/Player.java

@@ -866,7 +866,7 @@ private boolean canFitWithBoundingBox(@NotNull Pose pose) {
         var iter = bb.getBlocks(getPosition());
         while (iter.hasNext()) {
             var pos = iter.next();
-            var block = instance.getBlock(pos, Block.Getter.Condition.TYPE);
+            var block = instance.getBlock(pos.blockX(), pos.blockY(), pos.blockZ(), Block.Getter.Condition.TYPE);
 
             // For now just ignore scaffolding. It seems to have a dynamic bounding box, or is just parsed
             // incorrectly in MinestomDataGenerator.

src/main/java/net/minestom/server/entity/PlayerProjectile.java

@@ -112,9 +112,9 @@ private Pos checkInsideBlock(@NotNull Instance instance) {
 
         while (iterator.hasNext()) {
             var block = iterator.next();
-            Block b = instance.getBlock(block);
-            var hit = b.registry().collisionShape().intersectBox(this.getPosition().sub(block), this.getBoundingBox());
-            if (hit) return Pos.fromPoint(block);
+            Block b = instance.getBlock(block.blockX(), block.blockY(), block.blockZ());
+            var hit = b.registry().collisionShape().intersectBox(this.getPosition().sub(block.x(), block.y(), block.z()), this.getBoundingBox());
+            if (hit) return new Pos(block.blockX(), block.blockY(), block.blockZ());
         }
 
         return null;

src/main/java/net/minestom/server/entity/ai/goal/FollowTargetGoal.java

@@ -75,7 +75,7 @@ public void tick(long time) {
             return;
         }
         final Pos targetPos = entityCreature.getTarget() != null ? entityCreature.getTarget().getPosition() : null;
-        if (targetPos != null && !targetPos.samePoint(lastTargetPos)) {
+        if (targetPos != null && !targetPos.sameBlock(lastTargetPos)) {
             this.lastUpdateTime = time;
             this.lastTargetPos = targetPos;
             this.entityCreature.getNavigator().setPathTo(targetPos);