a bunch more testing, split off MoveController from navigator and mak…

…e navigator generic.

modules/motion/build.gradle.kts

@@ -5,4 +5,5 @@ plugins {
 dependencies {
     implementation("com.github.mworzala.mc_debug_renderer:minestom:1.19.2-rv1")
 
+    testImplementation(project(":modules:schem"))
 }

modules/motion/src/main/java/net/hollowcube/motion/MotionNavigator.java

@@ -26,21 +26,35 @@
 
 import java.time.Duration;
 import java.util.ArrayList;
+import java.util.function.Supplier;
 
 /**
  * Consumes paths from a {@link Pathfinder} to navigate an {@link Entity} in an instance.
  */
 public final class MotionNavigator {
-    private final Cooldown jumpCooldown = new Cooldown(Duration.of(40, TimeUnit.SERVER_TICK));
-    private final Cooldown debugCooldown = new Cooldown(Duration.of(1, TimeUnit.SERVER_TICK));
-    private final Entity entity;
+    private final LivingEntity entity;
+    private final PathGenerator generator;
+    private final Pathfinder pathfinder;
+    private final PathOptimizer optimizer;
+    private final MoveController controller;
 
+    private final Cooldown debugCooldown = new Cooldown(Duration.of(1, TimeUnit.SERVER_TICK));
     private Point goal = null;
     private Path path = null;
     private int index = 0;
 
-    public MotionNavigator(@NotNull Entity entity) {
+    public MotionNavigator(
+            @NotNull LivingEntity entity,
+            @NotNull PathGenerator pathGenerator,
+            @NotNull Pathfinder pathfinder,
+            @Nullable PathOptimizer optimizer,
+            @NotNull Supplier<MoveController> moveController
+    ) {
         this.entity = entity;
+        this.generator = pathGenerator;
+        this.pathfinder = pathfinder;
+        this.optimizer = optimizer;
+        this.controller = moveController.get();
     }
 
     public boolean isActive() {
@@ -87,19 +101,22 @@ public synchronized boolean setPathTo(@Nullable Point point) {
             return false;
 
         // Attempt to find a path
-        path = Pathfinder.A_STAR.findPath(PathGenerator.LAND, instance,
+        path = pathfinder.findPath(generator, instance,
                 entity.getPosition(), point, entity.getBoundingBox());
-
         boolean success = path != null;
+        if (success && optimizer != null) {
+            path = optimizer.optimize(path, instance, entity.getBoundingBox());
+        }
+
         goal = success ? point : null;
         return success;
     }
 
     public void tick(long time) {
         if (goal == null || path == null) return; // No path
-        if (entity instanceof LivingEntity livingEntity && livingEntity.isDead()) return;
+        if (entity.isDead()) return;
 
-        // If we are close enough to the goal position, just stop
+        // If we are close enough to the goal position, stop
         float minDistance = 0.8f; //todo move me
         if (entity.getDistance(goal) < minDistance) {
             reset();
@@ -114,16 +131,9 @@ public void tick(long time) {
 
         Point current = index < path.size() ? path.get(index) : goal;
 
-        float movementSpeed = 0.1f;
-        if (entity instanceof LivingEntity livingEntity) {
-            movementSpeed = livingEntity.getAttribute(Attribute.MOVEMENT_SPEED).getBaseValue();
-        }
-
-        // Move towards the current target, trying to jump if stuck
-        boolean isStuck = moveTowards(current, movementSpeed);
-        //todo jump if stuck
+        controller.moveTowards(entity, current);
 
-        // Move to next point if stuck
+        // Move to next point
         if (entity.getPosition().distanceSquared(current) < 0.4) {
             index++;
         }

modules/motion/src/main/java/net/hollowcube/motion/MoveController.java

@@ -0,0 +1,86 @@
+package net.hollowcube.motion;
+
+import net.minestom.server.attribute.Attribute;
+import net.minestom.server.collision.CollisionUtils;
+import net.minestom.server.coordinate.Point;
+import net.minestom.server.coordinate.Pos;
+import net.minestom.server.coordinate.Vec;
+import net.minestom.server.entity.LivingEntity;
+import net.minestom.server.utils.position.PositionUtils;
+import net.minestom.server.utils.time.Cooldown;
+import net.minestom.server.utils.time.TimeUnit;
+import org.jetbrains.annotations.NotNull;
+
+import java.time.Duration;
+import java.util.function.Supplier;
+
+/**
+ * A movement controller chooses how to move an entity towards a position.
+ * <p>
+ * The goal position is not necessarily the actual pathfinder goal, but rather the position the
+ *  entity is currently trying to reach.
+ */
+public interface MoveController {
+
+    void moveTowards(
+            @NotNull LivingEntity entity,
+            @NotNull Point goal
+    );
+
+    interface Factory {
+        MoveController create();
+    }
+
+    /**
+     * Default land-bound movement controller. Currently very basic (eg does not know how to swim)
+     */
+    Supplier<MoveController> WALKING = () -> new MoveController() {
+        private final Cooldown jumpCooldown = new Cooldown(Duration.of(20, TimeUnit.SERVER_TICK));
+
+        @Override
+        public void moveTowards(@NotNull LivingEntity entity, @NotNull Point goal) {
+            var now = System.currentTimeMillis();
+            var speed = (double) entity.getAttributeValue(Attribute.MOVEMENT_SPEED);
+
+            final Pos position = entity.getPosition();
+            final double dx = goal.x() - position.x();
+            final double dy = goal.y() - position.y();
+            final double dz = goal.z() - position.z();
+
+            // the purpose of these few lines is to slow down entities when they reach their destination
+            final double distSquared = dx * dx + dy * dy + dz * dz;
+            if (speed > distSquared) {
+                speed = distSquared;
+            }
+
+            final double radians = Math.atan2(dz, dx);
+            final double speedX = Math.cos(radians) * speed;
+            final double speedY = dy * speed;
+            final double speedZ = Math.sin(radians) * speed;
+
+            final float yaw = PositionUtils.getLookYaw(dx, dz);
+            final float pitch = PositionUtils.getLookPitch(dx, dy, dz);
+
+            final var physicsResult = CollisionUtils.handlePhysics(entity, new Vec(speedX, speedY, speedZ));
+            boolean willCollide = physicsResult.collisionX() || physicsResult.collisionY() || physicsResult.collisionZ();
+            if (dy > 0 && willCollide && jumpCooldown.isReady(now)) {
+                jumpCooldown.refreshLastUpdate(now);
+                //todo magic
+                entity.setVelocity(new Vec(0, 3.5f * 2.5f, 0));
+            } else {
+                entity.refreshPosition(Pos.fromPoint(physicsResult.newPosition()).withView(yaw, pitch));
+            }
+        }
+    };
+
+//    /**
+//     * Similar to {@link #WALKING}, but will hop towards the target (eg, slimes/rabbits)
+//     */
+//    MoveController HOPPING = new MoveController() {};
+
+//    /**
+//     * Moves directly towards the target position, ignoring gravity.
+//     */
+//    MoveController DIRECT = new MoveController() {};
+
+}

modules/motion/src/main/java/net/hollowcube/motion/PathGenerator.java

@@ -57,23 +57,41 @@ public interface PathGenerator {
         for (int x = -1; x <= 1; x++) {
             for (int z = -1; z <= 1; z++) {
                 if (x == 0 && z == 0) continue;
-                //todo up and down horizontals
 
-                var neighbor = pos.add(x, 0, z);
-                // Block below must be solid, or we cannot move to it
-                try {
-                    if (!world.getBlock(neighbor.add(0, -1, 0), Condition.TYPE).isSolid()) continue;
-                } catch (RuntimeException e) {
-                    //todo need a better solution here. Instance throws an exception if the chunk is unloaded
-                    //     but that is kinda awful behavior here. Probably i will need to check if the chunk
-                    //     is loaded, but then i cant use a block getter
-                    continue;
+                int minY = x == 0 || z == 0 ? -1 : 0;
+                int maxY = x == 0 || z == 0 ? 1 : 0;
+                for (int y = minY; y <= maxY; y++) {
+                    var neighbor = pos.add(x, y, z);
+
+                    // Block below must be solid, or we cannot move to it
+                    try {
+                        if (!world.getBlock(neighbor.add(0, -1, 0), Condition.TYPE).isSolid()) continue;
+                    } catch (RuntimeException e) {
+                        //todo need a better solution here. Instance throws an exception if the chunk is unloaded
+                        //     but that is kinda awful behavior here. Probably i will need to check if the chunk
+                        //     is loaded, but then i cant use a block getter
+                        continue;
+                    }
+
+                    // Ensure the movement from pos to neighbor is valid
+                    // This seems fairly slow, might be able to do a faster check for simple cases
+                    if (neighbor.y() > pos.y()) {
+                        // If the target is above the current, try to move to the targetPosition, then over
+                        var target = pos.withY(neighbor.y());
+                        if (PhysicsUtil.testCollisionSwept(world, bb, pos, target)) continue;
+                        if (PhysicsUtil.testCollisionSwept(world, bb, target, neighbor)) continue;
+                    } else if (neighbor.y() < pos.y()) {
+                        // If the target is below the current, try to move to the targetPosition, then down
+                        var target = neighbor.withY(pos.y());
+                        if (PhysicsUtil.testCollisionSwept(world, bb, pos, target)) continue;
+                        if (PhysicsUtil.testCollisionSwept(world, bb, target, neighbor)) continue;
+                    } else {
+                        // Same Y, so we can just make sure the direct movement is valid.
+                        if (PhysicsUtil.testCollisionSwept(world, bb, pos, neighbor)) continue;
+                    }
+
+                    neighbors.add(neighbor);
                 }
-
-                // Ensure the movement from pos to neighbor is valid
-                if (PhysicsUtil.testCollisionSwept(world, bb, pos, neighbor)) continue;
-
-                neighbors.add(neighbor);
             }
         }
         return neighbors;

modules/motion/src/main/java/net/hollowcube/motion/Pathfinder.java

@@ -91,10 +91,6 @@ public interface Pathfinder {
             result.add(0, current);
         }
 
-        Path path = new Path(result);
-        //todo optimize the path
-        path = PathOptimizer.STRING_PULL.optimize(path, world, bb);
-
-        return path;
+        return new Path(result);
     };
 }

modules/motion/src/test/java/net/hollowcube/motion/TestPathGeneratorLandDiagonal.java

@@ -35,6 +35,32 @@ public void testSingleNeighbor() {
         );
     }
 
+    @Test
+    public void testSingleNeighborDown() {
+        var bb = new BoundingBox(0.1, 0.1, 0.1);
+        var world = MockBlockGetter.block(0, 1, 0, Block.STONE)
+                .set(1, 0, 0, Block.STONE);
+        var start = new Vec(0, 2, 0);
+
+        var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
+        assertThat(result).containsExactly(
+                new Vec(1.5, 1, 0.5)
+        );
+    }
+
+    @Test
+    public void testSingleNeighborUp() {
+        var bb = new BoundingBox(0.1, 0.1, 0.1);
+        var world = MockBlockGetter.block(0, 0, 0, Block.STONE)
+                .set(1, 1, 0, Block.STONE);
+        var start = new Vec(0, 1, 0);
+
+        var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
+        assertThat(result).containsExactly(
+                new Vec(1.5, 2, 0.5)
+        );
+    }
+
     @Test
     public void testAllNeighborsFlat() {
         var bb = new BoundingBox(0.1, 0.1, 0.1);
@@ -89,7 +115,8 @@ public void testDiagonalCornerBlocking() {
         var bb = new BoundingBox(0.1, 0.1, 0.1);
         var world = MockBlockGetter.block(0, 0, 0, Block.STONE)
                 .set(1, 0, 1, Block.STONE)
-                .set(1, 1, 0, Block.STONE);
+                .set(1, 1, 0, Block.STONE)
+                .set(1, 2, 0, Block.STONE);
         var start = new Vec(0, 1, 0);
 
         var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
@@ -103,7 +130,7 @@ public void testDiagonalCornerBlockingWalkAround() {
                 0, 0, 0,
                 1, 0, 1,
                 Block.STONE
-        ).set(1, 1, 0, Block.STONE);
+        ).set(1, 1, 0, Block.STONE).set(1, 2, 0, Block.STONE);
         var start = new Vec(0, 1, 0);
 
         var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
@@ -112,4 +139,39 @@ public void testDiagonalCornerBlockingWalkAround() {
         );
     }
 
+    @Test
+    public void testSingleNeighborDownHeadHit() {
+        var bb = new BoundingBox(0.6, 1.95, 0.6); // Villager size
+        var world = MockBlockGetter.block(0, 1, 0, Block.STONE)
+                .set(1, 0, 0, Block.STONE)
+                .set(1, 3, 0, Block.STONE); // hits head on this block
+        var start = new Vec(0, 2, 0);
+
+        var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
+        assertThat(result).isEmpty();
+    }
+
+    @Test
+    public void testSingleNeighborUpHeadHit() {
+        var bb = new BoundingBox(0.6, 1.95, 0.6); // Villager size
+        var world = MockBlockGetter.block(0, 0, 0, Block.STONE)
+                .set(1, 1, 0, Block.STONE)
+                .set(0, 3, 0, Block.STONE); // hits head on this block
+        var start = new Vec(0, 1, 0);
+
+        var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
+        assertThat(result).isEmpty();
+    }
+
+    @Test
+    public void testCannotGoDiagonallyAndVertically() {
+        var bb = new BoundingBox(0.6, 1.95, 0.6); // Villager size
+        var world = MockBlockGetter.block(0, 0, 0, Block.STONE)
+                .set(1, 1, 1, Block.STONE);
+        var start = new Vec(0, 1, 0);
+
+        var result = PathGenerator.LAND_DIAGONAL.generate(world, start, bb);
+        assertThat(result).isEmpty();
+    }
+
 }

modules/motion/src/test/java/net/hollowcube/motion/TestPathfinderAStar.java

@@ -1,12 +1,15 @@
 package net.hollowcube.motion;
 
+import net.hollowcube.motion.util.SchemBlockGetter;
 import net.hollowcube.test.MockBlockGetter;
 import net.minestom.server.collision.BoundingBox;
 import net.minestom.server.coordinate.Point;
 import net.minestom.server.coordinate.Vec;
 import net.minestom.server.instance.block.Block;
 import net.minestom.server.utils.Direction;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -62,14 +65,30 @@ public void testBasicAvoidance() {
         );
     }
 
+    @ParameterizedTest(name = "{0}")
+    @ValueSource(strings = {
+            "valid/2_2_corner",
+            "valid/3_3_around",
+            "valid/5_5_maze",
+            "valid/1_4_4_staircase",
+    })
+    public void testSchematics(String name) {
+        var bb = new BoundingBox(0.1, 0.1, 0.1);
+        var world = new SchemBlockGetter(name);
+
+        var result = Pathfinder.A_STAR.findPath(ALL, world, world.start(), world.goal(), bb);
+        assertThat(result).isNotNull();
+        System.out.println(result);
+    }
+
 
-    // A path generator which returns any solid block in a direction (up/down/nsew)
+    // A path generator which returns any air block in a direction (up/down/nsew)
     private static final PathGenerator ALL = (world, pos, bb) -> {
         pos = new Vec(pos.blockX() + 0.5, pos.blockY(), pos.blockZ() + 0.5);
         List<Point> neighbors = new ArrayList<>();
         for (Direction direction : Direction.values()) {
             var neighbor = pos.add(direction.normalX(), direction.normalY(), direction.normalZ());
-            if (world.getBlock(neighbor, Condition.TYPE).isSolid()) continue;
+            if (!world.getBlock(neighbor, Condition.TYPE).isAir()) continue;
             neighbors.add(neighbor);
         }
         return neighbors;