refactor: started refactoring NodeFollower

api/src/main/java/ru/dragonestia/msb3/api/ai/AI.java

@@ -2,20 +2,26 @@ package ru.dragonestia.msb3.api.ai;
 
 import lombok.Getter;
 import lombok.Setter;
+import lombok.extern.log4j.Log4j2;
 import net.minestom.server.coordinate.Point;
 import net.minestom.server.coordinate.Pos;
 import net.minestom.server.coordinate.Vec;
+import net.minestom.server.entity.attribute.Attribute;
+import ru.dragonestia.msb3.api.ai.movement.GroundMovementFollower;
+import ru.dragonestia.msb3.api.ai.movement.MovementFollower;
 import ru.dragonestia.msb3.api.ai.navigator.*;
 import ru.dragonestia.msb3.api.ai.navigator.follower.GroundNodeFollower;
 import ru.dragonestia.msb3.api.ai.navigator.follower.NodeFollower;
 import ru.dragonestia.msb3.api.ai.navigator.node.GroundNodeGenerator;
 import ru.dragonestia.msb3.api.ai.navigator.node.NodeGenerator;
+import ru.dragonestia.msb3.api.debug.DebugMessage;
 import ru.dragonestia.msb3.api.entity.EntityAI;
 import ru.dragonestia.msb3.api.util.UncheckedRunnable;
 
 import java.util.Objects;
 import java.util.concurrent.CompletableFuture;
 
+@Log4j2
 @Getter
 public class AI {
 
@@ -23,11 +29,14 @@ public class AI {
     private DestinationPoint destinationData;
     @Setter private NodeGenerator nodeGenerator = new GroundNodeGenerator();
     @Setter private NodeFollower nodeFollower;
+    private final MovementFollower movementFollower;
     @Getter private final Actor actor;
+    @Setter private Point destinationPoint;
 
     public AI(EntityAI entity) {
         this.entity = Objects.requireNonNull(entity, "AI is null");
         nodeFollower = new GroundNodeFollower(entity);
+        movementFollower = new GroundMovementFollower(entity);
         actor = new Actor(entity);
     }
 
@@ -108,9 +117,34 @@ public class AI {
             if (action != null) action.tick(actor, entity, delta);
 
             if (destinationData != null) tickNavigator();
+            if (destinationPoint != null) tickNewNavigator();
         }
     }
 
+    private void tickNewNavigator() {
+        if (checkDestinationPointCompleted()) return;
+
+        var speed = Math.min(getMovementSpeed(), Math.sqrt(movementFollower.squaredDistance(destinationPoint)));
+        var prevPosition = entity.getPosition();
+        movementFollower.moveTo(destinationPoint, speed, destinationPoint);
+        var movementDistance = movementFollower.squaredDistance(prevPosition);
+        if (movementDistance == 0) {
+            // Deadlocked?
+        }
+
+        checkDestinationPointCompleted();
+    }
+
+    private boolean checkDestinationPointCompleted() {
+        double distSquared = movementFollower.squaredDistance(destinationPoint);
+        if (distSquared < 0.01 * 0.01) {
+            destinationPoint = null;
+            DebugMessage.broadcast("Destination point completed");
+            return true;
+        }
+        return false;
+    }
+
     private void tickNavigator() {
         if (entity.getPosition().distance(destinationData.position()) < destinationData.arrivalDistance()) {
             var future = destinationData.future();
@@ -123,7 +157,7 @@ public class AI {
         var nextTarget = path.getNext();
 
         if (currentTarget == null || path.getCurrentType() == PathNode.Type.REPATH || path.getCurrentType() == null) {
-            recalculatePath(path);
+            recalculatePath();
             return;
         }
 
@@ -142,7 +176,10 @@ public class AI {
         checkFinishedPath(path, destinationData.future());
     }
 
-    private void recalculatePath(Path originalPath) {
+    public void recalculatePath() {
+        if (destinationData == null) return;
+
+        var originalPath = destinationData.path();
         var newPath = PathGenerator.generate(entity.getInstance(),
                 entity.getPosition(),
                 destinationData.position(),
@@ -173,4 +210,8 @@ public class AI {
     public boolean isCompleted() {
         return getCurrentPathState().isCompleted();
     }
+
+    public double getMovementSpeed() {
+        return entity.getAttribute(Attribute.MOVEMENT_SPEED).getValue();
+    }
 }

api/src/main/java/ru/dragonestia/msb3/api/ai/movement/GroundMovementFollower.java

@@ -0,0 +1,85 @@
+package ru.dragonestia.msb3.api.ai.movement;
+
+import net.minestom.server.collision.CollisionUtils;
+import net.minestom.server.collision.PhysicsResult;
+import net.minestom.server.coordinate.Point;
+import net.minestom.server.coordinate.Vec;
+import ru.dragonestia.msb3.api.entity.EntityAI;
+
+public class GroundMovementFollower extends MovementFollower {
+
+    private PhysicsResult lastPhysicResult;
+
+    public GroundMovementFollower(EntityAI entity) {
+        super(entity);
+    }
+
+    @Override
+    public double squaredDistance(Point destination) {
+        return getEntity().getPosition().withY(0).distanceSquared(destination.withY(0));
+    }
+
+    @Override
+    public PhysicsResult moveResult(Point direction, double speed) {
+        var radians = Math.atan2(direction.z(), direction.x());
+        var speedX = Math.cos(radians) * speed;
+        var speedZ = Math.sin(radians) * speed;
+
+        double speedY = 0;
+        if (lastPhysicResult != null) {
+            double highestDeltaCollision = 0;
+
+            for (int i = 0; i < lastPhysicResult.collisionShapes().length; i++) {
+                var shape = lastPhysicResult.collisionShapes()[i];
+                var shapePos = lastPhysicResult.collisionShapePositions()[i];
+                if (shape == null || shapePos == null) continue;
+
+                var point = shapePos.add(shape.relativeEnd());
+                var deltaH = point.y() - getEntity().getPosition().y();
+                if (deltaH > getMaxHeightStep() && deltaH > getJumpHeight()) continue;
+                highestDeltaCollision = Math.max(highestDeltaCollision, deltaH);
+            }
+
+            if (highestDeltaCollision > 0 && getEntity().isOnGround()) {
+                speedY = highestDeltaCollision + 0.08;
+            }
+        }
+
+        return lastPhysicResult = CollisionUtils.handlePhysics(getEntity(), new Vec(speedX, speedY, speedZ));
+    }
+
+    @Override
+    public PhysicsResult move(Point direction, double speed) {
+        return super.move(direction, speed);
+    }
+
+    @Override
+    public PhysicsResult move(Point direction, double speed, Point lookDirection) {
+        return super.move(direction, speed, lookDirection);
+    }
+
+    public double getJumpHeight() {
+        return 1.1;
+    }
+
+    public double getMaxHeightStep() {
+        return 0.6;
+    }
+
+    public void jump() {
+        jump(0, 0);
+    }
+
+    public void jump(Point direction) {
+        jump(direction.x(), direction.y(), direction.z());
+    }
+
+    public void jump(double x, double z) {
+        jump(x, getEntity().getJumpHeight(), z);
+    }
+
+    public void jump(double x, double y, double z) {
+        if (!getEntity().isOnGround()) return;
+        getEntity().setVelocity(new Vec(x, y, z).mul(7.1));
+    }
+}

api/src/main/java/ru/dragonestia/msb3/api/ai/movement/MovementFollower.java

@@ -0,0 +1,44 @@
+package ru.dragonestia.msb3.api.ai.movement;
+
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+import net.minestom.server.collision.PhysicsResult;
+import net.minestom.server.coordinate.Point;
+import net.minestom.server.coordinate.Pos;
+import net.minestom.server.utils.position.PositionUtils;
+import ru.dragonestia.msb3.api.entity.EntityAI;
+
+@Getter
+@RequiredArgsConstructor
+public abstract class MovementFollower {
+
+    private final EntityAI entity;
+
+    public double squaredDistance(Point destination) {
+        return entity.getDistanceSquared(destination);
+    }
+
+    public abstract PhysicsResult moveResult(Point direction, double speed);
+
+    public PhysicsResult move(Point direction, double speed) {
+        var physicResult = moveResult(direction, speed);
+        var newPosition = entity.getPosition().withCoord(physicResult.newPosition());
+        entity.refreshPosition(newPosition);
+        return physicResult;
+    }
+
+    public PhysicsResult move(Point direction, double speed, Point lookDirection) {
+        var yaw = PositionUtils.getLookYaw(lookDirection.x(), direction.z());
+        var pitch = PositionUtils.getLookPitch(lookDirection.x(), lookDirection.y(), direction.z());
+        var physicResult = moveResult(direction, speed);
+        var newPosition = Pos.fromPoint(physicResult.newPosition()).withView(yaw, pitch);
+        entity.refreshPosition(newPosition);
+        return physicResult;
+    }
+
+    public PhysicsResult moveTo(Point destination, double speed, Point lookAt) {
+        var direction = destination.sub(entity.getPosition());
+        var lookDirection = lookAt.sub(entity.getPosition());
+        return move(direction, speed, lookDirection);
+    }
+}

api/src/main/java/ru/dragonestia/msb3/api/command/PuppeteerSubcommand.java

@@ -8,6 +8,7 @@ import net.minestom.server.command.builder.CommandContext;
 import net.minestom.server.command.builder.arguments.ArgumentType;
 import net.minestom.server.entity.EntityType;
 import net.minestom.server.entity.Player;
+import ru.dragonestia.msb3.api.ai.movement.GroundMovementFollower;
 import ru.dragonestia.msb3.api.entity.EntityAI;
 
 public class PuppeteerSubcommand extends Command {
@@ -21,7 +22,9 @@ public class PuppeteerSubcommand extends Command {
         addSyntax(this::createEntity, ArgumentType.Literal("create"));
         addSyntax(this::removeEntity, ArgumentType.Literal("remove"));
         addSyntax(this::comeHere, ArgumentType.Literal("come_here"));
+        addSyntax(this::justComeHere, ArgumentType.Literal("just_come_here"));
         addSyntax(this::teleport, ArgumentType.Literal("teleport"));
+        addSyntax(this::jump, ArgumentType.Literal("jump"));
     }
 
     private void defaultExecutor(CommandSender sender, CommandContext ctx) {
@@ -65,6 +68,14 @@ public class PuppeteerSubcommand extends Command {
         });
     }
 
+    private void justComeHere(CommandSender sender, CommandContext ctx) {
+        if (isNotCreated(sender)) return;
+
+        var player = (Player) sender;
+        sender.sendMessage(Component.text("Set entity path target without PathFinder."));
+        entity.getAi().setDestinationPoint(player.getPosition());
+    }
+
     private void teleport(CommandSender sender, CommandContext ctx) {
         if (isNotCreated(sender)) return;
 
@@ -74,6 +85,15 @@ public class PuppeteerSubcommand extends Command {
         });
     }
 
+    private void jump(CommandSender sender, CommandContext ctx) {
+        if (isNotCreated(sender)) return;
+
+        if (entity.getAi().getMovementFollower() instanceof GroundMovementFollower follower) {
+            follower.jump();
+            sender.sendMessage(Component.text("Jumped.", NamedTextColor.YELLOW));
+        }
+    }
+
     private boolean isNotCreated(CommandSender sender) {
         boolean failed = false;
         if (entity == null) {

api/src/main/java/ru/dragonestia/msb3/api/entity/EntityAI.java

@@ -77,6 +77,6 @@ public class EntityAI extends LivingEntity {
     }
 
     public double getJumpHeight() {
-        return 2.1;
+        return 1.1;
     }
 }