From 48cf157370e749d908875ce537ca11f87696706b Mon Sep 17 00:00:00 2001 From: KrystilizeNevaDies Date: Sat, 12 Mar 2022 17:32:01 +1000 Subject: [PATCH] Cleanup, pathfind demo, and PathfinderImpl improvements --- .../src/main/java/net/minestom/demo/Main.java | 2 +- .../java/net/minestom/demo/PlayerInit.java | 5 +- .../demo/commands/PathfindCommand.java | 49 ++++++ .../minestom/demo/commands/SummonCommand.java | 25 +++- .../server/entity/pathfinding/Navigator.java | 48 +++--- .../entity/pathfinding/PathfindUtils.java | 24 ++- .../server/entity/pathfinding/Pathfinder.java | 30 +++- .../entity/pathfinding/PathfinderImpl.java | 139 +++++++++++++----- .../server/utils/position/PositionUtils.java | 19 +++ 9 files changed, 276 insertions(+), 65 deletions(-) create mode 100644 demo/src/main/java/net/minestom/demo/commands/PathfindCommand.java diff --git a/demo/src/main/java/net/minestom/demo/Main.java b/demo/src/main/java/net/minestom/demo/Main.java index 2bc7554c1..289be362c 100644 --- a/demo/src/main/java/net/minestom/demo/Main.java +++ b/demo/src/main/java/net/minestom/demo/Main.java @@ -52,7 +52,7 @@ public class Main { commandManager.register(new AutoViewCommand()); commandManager.register(new SaveCommand()); commandManager.register(new GamemodeCommand()); - + commandManager.register(new PathfindCommand()); commandManager.setUnknownCommandCallback((sender, command) -> sender.sendMessage(Component.text("Unknown command", NamedTextColor.RED))); diff --git a/demo/src/main/java/net/minestom/demo/PlayerInit.java b/demo/src/main/java/net/minestom/demo/PlayerInit.java index 0c6d81850..d52530199 100644 --- a/demo/src/main/java/net/minestom/demo/PlayerInit.java +++ b/demo/src/main/java/net/minestom/demo/PlayerInit.java @@ -31,6 +31,7 @@ import net.minestom.server.item.metadata.BundleMeta; import net.minestom.server.monitoring.BenchmarkManager; import net.minestom.server.monitoring.TickMonitor; import net.minestom.server.utils.MathUtils; +import net.minestom.server.utils.NamespaceID; import net.minestom.server.utils.time.TimeUnit; import net.minestom.server.world.DimensionType; @@ -121,11 +122,13 @@ public class PlayerInit { }); static { + DimensionType fullbright = DimensionType.builder(NamespaceID.from("minestom:fullbright")).ambientLight(2.0F).build(); + MinecraftServer.getDimensionTypeManager().addDimension(fullbright); InstanceManager instanceManager = MinecraftServer.getInstanceManager(); ChunkGeneratorDemo chunkGeneratorDemo = new ChunkGeneratorDemo(); NoiseTestGenerator noiseTestGenerator = new NoiseTestGenerator(); - InstanceContainer instanceContainer = instanceManager.createInstanceContainer(DimensionType.OVERWORLD); + InstanceContainer instanceContainer = instanceManager.createInstanceContainer(fullbright); instanceContainer.setChunkGenerator(chunkGeneratorDemo); if (false) { diff --git a/demo/src/main/java/net/minestom/demo/commands/PathfindCommand.java b/demo/src/main/java/net/minestom/demo/commands/PathfindCommand.java new file mode 100644 index 000000000..b7aa88646 --- /dev/null +++ b/demo/src/main/java/net/minestom/demo/commands/PathfindCommand.java @@ -0,0 +1,49 @@ +package net.minestom.demo.commands; + +import net.minestom.server.command.CommandSender; +import net.minestom.server.command.builder.Command; +import net.minestom.server.command.builder.CommandContext; +import net.minestom.server.coordinate.Pos; +import net.minestom.server.entity.Entity; +import net.minestom.server.entity.EntityCreature; +import net.minestom.server.utils.entity.EntityFinder; + +import java.util.List; + +import static net.minestom.server.command.builder.arguments.ArgumentType.*; + +public class PathfindCommand extends Command { + + public PathfindCommand() { + super("pathfind"); + + addSyntax( + this::usageE2E, + Entity("from"), + Entity("to") + ); + } + + private void usageE2E(CommandSender sender, CommandContext context) { + EntityFinder from = context.get("from"); + EntityFinder to = context.get("to"); + + List fromList = from.find(sender); + Entity toEntity = to.findFirstEntity(sender); + + if (toEntity == null) { + sender.sendMessage("No entity found"); + return; + } + + Pos destination = toEntity.getPosition(); + destination = destination.add(toEntity.getBoundingBox().relativeStart()); + + for (Entity fromEntity : fromList) { + if (fromEntity instanceof EntityCreature creature) { + sender.sendMessage("Pathfinding from " + fromEntity + " to " + toEntity); + creature.getNavigator().setPathTo(destination); + } + } + } +} \ No newline at end of file diff --git a/demo/src/main/java/net/minestom/demo/commands/SummonCommand.java b/demo/src/main/java/net/minestom/demo/commands/SummonCommand.java index a12dea635..99300c148 100644 --- a/demo/src/main/java/net/minestom/demo/commands/SummonCommand.java +++ b/demo/src/main/java/net/minestom/demo/commands/SummonCommand.java @@ -18,6 +18,7 @@ public class SummonCommand extends Command { private final ArgumentEntityType entity; private final Argument pos; private final Argument entityClass; + private final Argument count; public SummonCommand() { super("summon"); @@ -32,14 +33,17 @@ public class SummonCommand extends Command { entityClass = ArgumentType.Enum("class", EntityClass.class) .setFormat(ArgumentEnum.Format.LOWER_CASED) .setDefaultValue(EntityClass.CREATURE); - addSyntax(this::execute, entity, pos, entityClass); - setDefaultExecutor((sender, context) -> sender.sendMessage("Usage: /summon ")); + count = ArgumentType.Integer("count").setDefaultValue(1); + addSyntax(this::execute, entity, pos, entityClass, count); + setDefaultExecutor((sender, context) -> sender.sendMessage("Usage: /summon ")); } private void execute(@NotNull CommandSender commandSender, @NotNull CommandContext commandContext) { - final Entity entity = commandContext.get(entityClass).instantiate(commandContext.get(this.entity)); - //noinspection ConstantConditions - One couldn't possibly execute a command without being in an instance - entity.setInstance(((Player) commandSender).getInstance(), commandContext.get(pos).fromSender(commandSender)); + for (int i = 0; i < commandContext.get(count); i++) { + final Entity entity = commandContext.get(entityClass).instantiate(commandContext.get(this.entity)); + //noinspection ConstantConditions - One couldn't possibly execute a command without being in an instance + entity.setInstance(((Player) commandSender).getInstance(), commandContext.get(pos).fromSender(commandSender)); + } } @SuppressWarnings("unused") @@ -54,6 +58,10 @@ public class SummonCommand extends Command { } public Entity instantiate(EntityType type) { + if (type == EntityType.BEE && this == CREATURE) { + // Flying creature for development + return new FlyingEntity(type); + } return factory.newInstance(type); } } @@ -61,4 +69,11 @@ public class SummonCommand extends Command { interface EntityFactory { Entity newInstance(EntityType type); } + + private static final class FlyingEntity extends EntityCreature { + public FlyingEntity(@NotNull EntityType entityType) { + super(entityType); + setGravity(0.0, 0.0); + } + } } diff --git a/src/main/java/net/minestom/server/entity/pathfinding/Navigator.java b/src/main/java/net/minestom/server/entity/pathfinding/Navigator.java index 79af1872e..9061f58cc 100644 --- a/src/main/java/net/minestom/server/entity/pathfinding/Navigator.java +++ b/src/main/java/net/minestom/server/entity/pathfinding/Navigator.java @@ -7,13 +7,14 @@ import net.minestom.server.coordinate.Pos; import net.minestom.server.coordinate.Vec; import net.minestom.server.entity.Entity; import net.minestom.server.entity.LivingEntity; +import net.minestom.server.particle.Particle; +import net.minestom.server.particle.ParticleCreator; +import net.minestom.server.utils.PacketUtils; import net.minestom.server.utils.position.PositionUtils; import org.jetbrains.annotations.ApiStatus; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; -// TODO all pathfinding requests could be processed in another thread - /** * Necessary object for all {@link NavigableEntity}. */ @@ -23,7 +24,7 @@ public final class Navigator { public Navigator(@NotNull Entity entity) { this.entity = entity; - this.pathfinder = new PathfinderImpl(entity); + this.pathfinder = new PathfinderImpl(entity, null, null); } /** @@ -36,21 +37,34 @@ public final class Navigator { */ public void moveTowards(@NotNull Point direction, double speed) { final Pos position = entity.getPosition(); - final double dx = direction.x() - position.x(); - final double dy = direction.y() - position.y(); - final double dz = direction.z() - position.z(); + + // Find the direction + double dx = direction.x() - position.x(); + double dy = direction.y() - position.y(); + double dz = direction.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; + 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); - // Prevent ghosting + // Find the movement speed + double radians = Math.atan2(dz, dx); + double speedX = Math.cos(radians) * speed; + double speedY = dy * speed; + double speedZ = Math.sin(radians) * speed; + + // Now calculate the new yaw/pitch + float oldYaw = position.yaw(); + float oldPitch = position.pitch(); + float newYaw = PositionUtils.getLookYaw(dx, dz); + float newPitch = PositionUtils.getLookPitch(dx, dy, dz); + + // Average the pitch and yaw to avoid jittering + float yaw = PositionUtils.averageYaw(PositionUtils.averageYaw(oldYaw, newYaw), oldYaw); + float pitch = PositionUtils.averagePitch(PositionUtils.averagePitch(oldPitch, newPitch), oldPitch); + + // Prevent ghosting, and refresh position final var physicsResult = CollisionUtils.handlePhysics(entity, new Vec(speedX, speedY, speedZ)); this.entity.refreshPosition(physicsResult.newPosition().withView(yaw, pitch)); } @@ -71,6 +85,7 @@ public final class Navigator { final Point next = this.pathfinder.nextPoint(entity.getPosition()); if (next != null) { moveTowards(next, getAttributeValue(Attribute.MOVEMENT_SPEED)); + PathfindUtils.debugParticle(next, Particle.WHITE_ASH); final double entityY = entity.getPosition().y(); if (entityY < next.y()) { jump(1); @@ -84,14 +99,13 @@ public final class Navigator { * @return the target pathfinder position, null if there is no one */ public @Nullable Point getPathPosition() { - return null; // TODO - //return pathfinder.pathPosition; + return this.pathfinder.nextPoint(entity.getPosition()); } private float getAttributeValue(@NotNull Attribute attribute) { if (entity instanceof LivingEntity) { return ((LivingEntity) entity).getAttributeValue(attribute); } - return 0f; + return attribute.defaultValue(); } } diff --git a/src/main/java/net/minestom/server/entity/pathfinding/PathfindUtils.java b/src/main/java/net/minestom/server/entity/pathfinding/PathfindUtils.java index aceca1df4..77e64522c 100644 --- a/src/main/java/net/minestom/server/entity/pathfinding/PathfindUtils.java +++ b/src/main/java/net/minestom/server/entity/pathfinding/PathfindUtils.java @@ -3,15 +3,26 @@ package net.minestom.server.entity.pathfinding; import net.minestom.server.collision.BoundingBox; import net.minestom.server.coordinate.Point; import net.minestom.server.instance.block.Block; +import net.minestom.server.particle.Particle; +import net.minestom.server.particle.ParticleCreator; +import net.minestom.server.utils.PacketUtils; import org.jetbrains.annotations.NotNull; final class PathfindUtils { public static boolean isBlocked(@NotNull Point point, @NotNull BoundingBox box, - @NotNull Block.Getter getter, double entityPadding) { + @NotNull Block.Getter getter) { Point relStart = box.relativeStart(); Point relEnd = box.relativeEnd(); - relStart = relStart.mul(2, 0, 2).sub(entityPadding, 0, entityPadding); - relEnd = relEnd.mul(2, 0, 2).add(entityPadding, 0, entityPadding); + + // Double for some reason (Necessary, I don't know why) + relStart = relStart.mul(2, 0, 2); + relEnd = relEnd.mul(2, 0, 2); + + // Add a little padding so pathfinding is not against the very edge of the block + relStart = relStart.add(-0.1, 0, -0.1); + relEnd = relEnd.add(0.1, 0, 0.1); + + // TODO: Use BlockIterator instead return getter.getBlock(point.add(relStart.x(), relStart.y(), relStart.z())).isSolid() || getter.getBlock(point.add(relStart.x(), relStart.y(), relEnd.z())).isSolid() || getter.getBlock(point.add(relStart.x(), relEnd.y(), relStart.z())).isSolid() || @@ -21,4 +32,11 @@ final class PathfindUtils { getter.getBlock(point.add(relEnd.x(), relEnd.y(), relStart.z())).isSolid() || getter.getBlock(point.add(relEnd.x(), relEnd.y(), relEnd.z())).isSolid(); } + + public static void debugParticle(@NotNull Point point, @NotNull Particle particle) { + // TODO: Remove this debug + PacketUtils.broadcastPacket(ParticleCreator.createParticlePacket( + particle, point.x(), point.y(), point.z(), + 0, 0, 0, 1)); + } } diff --git a/src/main/java/net/minestom/server/entity/pathfinding/Pathfinder.java b/src/main/java/net/minestom/server/entity/pathfinding/Pathfinder.java index c1a5ec690..3401903b4 100644 --- a/src/main/java/net/minestom/server/entity/pathfinding/Pathfinder.java +++ b/src/main/java/net/minestom/server/entity/pathfinding/Pathfinder.java @@ -1,13 +1,37 @@ package net.minestom.server.entity.pathfinding; import net.minestom.server.coordinate.Point; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import java.util.List; +/** + * A pathfinder is used to maintain a dynamic path to a target. + * This dynamic path may change target, or be cancelled with {@link Pathfinder#updatePath(Point)}. + * It may be forcibly generated with {@link Pathfinder#forcePath(Point)}. + * And you may query the path with {@link Pathfinder#nextPoint(Point)}. + *

+ * Implementations of this interface may be threaded, as long as the interface methods are thread-safe. + */ public interface Pathfinder { - Point nextPoint(Point currentPoint); + /** + * This query will return the next point to reach the target, from the current position. + * @param currentPoint the current position + * @return the next point to reach the target, or null if a path does not currently exist + */ + @Nullable Point nextPoint(@NotNull Point currentPoint); - void updatePath(Point target); + /** + * This method will update the path to the given target, starting a new path if none exists. + * @param target the new target, or null to cancel the path + */ + void updatePath(@Nullable Point target); - List forcePath(Point target); + /** + * This method will force the path to the given target. + * @param target the new target + * @return the list of points to reach the target + */ + @Nullable List<@NotNull Point> forcePath(Point target); } diff --git a/src/main/java/net/minestom/server/entity/pathfinding/PathfinderImpl.java b/src/main/java/net/minestom/server/entity/pathfinding/PathfinderImpl.java index 37debb29b..0dd3d18e1 100644 --- a/src/main/java/net/minestom/server/entity/pathfinding/PathfinderImpl.java +++ b/src/main/java/net/minestom/server/entity/pathfinding/PathfinderImpl.java @@ -1,29 +1,49 @@ package net.minestom.server.entity.pathfinding; +import net.minestom.server.collision.BoundingBox; import net.minestom.server.coordinate.Point; import net.minestom.server.entity.Entity; import net.minestom.server.instance.Instance; import net.minestom.server.instance.block.Block; import net.minestom.server.particle.Particle; -import net.minestom.server.particle.ParticleCreator; -import net.minestom.server.utils.PacketUtils; +import net.minestom.server.utils.block.BlockIterator; +import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.util.*; +/** + * A simplified, synchronized A* pathfinder. + * This pathfinder is focussed on stability, not performance. + *

+ * If you would like to limit the entity to a + *

+ */ final class PathfinderImpl implements Pathfinder { + // The delta is used as a failsafe to avoid incorrect pathfinding, generally 0.01 is enough private static final double DELTA = 0.01; - private final Entity entity; + + private final @NotNull Entity entity; + private final @NotNull BlockedPredicate blocked; + private final @NotNull CostFunction cost; volatile Point pathPosition; volatile List path; - PathfinderImpl(Entity entity) { + /** + * Creates an instance of this pathfinder. + * @param entity the entity to find a path for + * @param blocked the predicate to check if a block is blocked, {@link BlockedPredicate#BLOCK_SOLID_BLOCKS} if null + * @param cost the cost function to calculate the cost of a block, {@link CostFunction#BLOCK_SPEED_FACTOR} if null + */ + PathfinderImpl(@NotNull Entity entity, @Nullable BlockedPredicate blocked, @Nullable CostFunction cost) { this.entity = entity; + this.blocked = blocked == null ? BlockedPredicate.BLOCK_SOLID_BLOCKS : blocked; + this.cost = cost == null ? CostFunction.BLOCK_SPEED_FACTOR : cost; } @Override - public Point nextPoint(Point currentPoint) { + public Point nextPoint(@NotNull Point currentPoint) { var path = this.path; if (path == null || path.isEmpty()) { return null; @@ -46,25 +66,28 @@ final class PathfinderImpl implements Pathfinder { } @Override - public void updatePath(Point target) { + public void updatePath(@Nullable Point target) { var start = entity.getPosition(); - var result = findPath(start, target, 1); - this.path = result.stream().toList(); + this.path = findPath(start, target); this.pathPosition = target; } @Override - public List forcePath(Point target) { + public @Nullable List forcePath(Point target) { updatePath(target); return path; } - @Nullable Queue findPath(Point start, Point goal, double step) { + @Nullable List findPath(Point start, Point goal) { + // The step is half of the lowest dimension of the entity's bounding box + // This is used so that the entity will never be able to skip over any point in the path + BoundingBox box = entity.getBoundingBox(); + double step = Math.min(box.width(), Math.min(box.height(), box.depth())) / 2; + + // The distance cost is the distance between the current point and the goal, plus the cost of the current point Comparator distanceCost = Comparator.comparingDouble(p -> - p.distance(start) + - p.distance(goal) + - getCost(p, p) - ); + p.distance(start) + p.distance(goal) + cost.getCost(entity, p, p)); + // The queue of nodes to be evaluated next Queue next = new PriorityQueue<>(distanceCost); Set nextSet = new HashSet<>(); @@ -81,9 +104,7 @@ final class PathfinderImpl implements Pathfinder { nextSet.remove(current); // TODO: Remove this debug - PacketUtils.broadcastPacket(ParticleCreator.createParticlePacket( - Particle.FLAME, current.x(), current.y(), current.z(), - 0, 0, 0, 1)); + PathfindUtils.debugParticle(current, Particle.SMOKE); // Return if the current node is the goal if (current.distance(goal) - DELTA <= step) { @@ -98,7 +119,7 @@ final class PathfinderImpl implements Pathfinder { } // If the neighbor is not walkable, skip it - if (isBlocked(neighbor)) { + if (blocked.test(entity, current, neighbor)) { continue; } @@ -115,18 +136,21 @@ final class PathfinderImpl implements Pathfinder { return null; } - private static Queue reconstructPath(Map cameFrom, Point current) { + private static List reconstructPath(Map cameFrom, Point current) { Deque path = new ArrayDeque<>(); path.add(current); while (cameFrom.containsKey(current)) { current = cameFrom.get(current); path.addFirst(current); } - return path; + for (Point point : path) { + PathfindUtils.debugParticle(point, Particle.FLAME); + } + return List.copyOf(path); } private static Point[] neighbors(Point point, double step) { - return new Point[]{ + return new Point[] { // Direct neighbors point.add(step, 0, 0), point.add(-step, 0, 0), @@ -157,22 +181,67 @@ final class PathfinderImpl implements Pathfinder { }; } - public double getCost(Point from, Point to) { - // TODO: Implement line intersection algorithm to determine the cost - // The current algorithm is flawed and may tell the navigator to move through very - // specific corners that are not actually possible - Instance instance = entity.getInstance(); - Objects.requireNonNull(instance, "The navigator must be in an instance while pathfinding."); - Block block = instance.getBlock(to); - if (block.isSolid()) { - return Double.POSITIVE_INFINITY; + public interface BlockedPredicate { + /** + * A predicate used as default that blocks movement if any of the blocks are solid. + */ + BlockedPredicate BLOCK_SOLID_BLOCKS = (entity, from, to) -> { + Instance instance = entity.getInstance(); + Objects.requireNonNull(instance, "The navigator must be in an instance while pathfinding."); + return PathfindUtils.isBlocked(to, entity.getBoundingBox(), instance); + }; + + /** + * Returns true if the given entity cannot move between the two points, false otherwise. + * @param entity The entity to check. + * @param from The starting point. + * @param to The ending point. + * @return True if the entity cannot move between the two points, false otherwise. + */ + boolean test(@NotNull Entity entity, @NotNull Point from, @NotNull Point to); + + /** + * Combines this predicate with another one. + * @param other The other predicate. + * @return A new predicate that returns true if either this or the other predicate returns true. + */ + default @NotNull BlockedPredicate combine(@NotNull BlockedPredicate other) { + return (entity, from, to) -> test(entity, from, to) || other.test(entity, from, to); } - return block.registry().speedFactor(); } - public boolean isBlocked(Point point) { - Instance instance = entity.getInstance(); - Objects.requireNonNull(instance, "The navigator must be in an instance while pathfinding."); - return PathfindUtils.isBlocked(point, entity.getBoundingBox(), instance, 0.1); + public interface CostFunction { + /** + * A cost function used as default that returns the block speed factor + */ + CostFunction BLOCK_SPEED_FACTOR = (entity, from, to) -> { + // TODO: Implement line intersection algorithm to determine the cost + // The current algorithm is flawed and may tell the navigator to move through very + // specific corners that are not actually possible + Instance instance = entity.getInstance(); + Objects.requireNonNull(instance, "The navigator must be in an instance while pathfinding."); + Block block = instance.getBlock(to); + if (block.isSolid()) { + return Double.POSITIVE_INFINITY; + } + return block.registry().speedFactor(); + }; + + /** + * Returns the cost of moving from one point to another. + * @param from The starting point. + * @param to The ending point. + * @return The cost of moving from one point to another. + */ + double getCost(@NotNull Entity entity, @NotNull Point from, @NotNull Point to); + + /** + * Combines this cost function with another cost function. + * @param other The other cost function. + * @return A new cost function that combines this cost function with the other cost function. + */ + default @NotNull CostFunction combine(@NotNull CostFunction other) { + return (entity, from, to) -> getCost(entity, from, to) + other.getCost(entity, from, to); + } } } diff --git a/src/main/java/net/minestom/server/utils/position/PositionUtils.java b/src/main/java/net/minestom/server/utils/position/PositionUtils.java index b00cd2800..8c0dcb282 100644 --- a/src/main/java/net/minestom/server/utils/position/PositionUtils.java +++ b/src/main/java/net/minestom/server/utils/position/PositionUtils.java @@ -24,4 +24,23 @@ public final class PositionUtils { final double radians = -Math.atan2(dy, Math.max(Math.abs(dx), Math.abs(dz))); return (float) Math.toDegrees(radians); } + + private static float averageWrapped(float valueA, float valueB, float wrap) { + float diff = valueB - valueA; + while (diff > wrap / 2) { + diff -= wrap; + } + while (diff < -wrap / 2) { + diff += wrap; + } + return valueA + diff / 2; + } + + public static float averagePitch(float pitchA, float pitchB) { + return averageWrapped(pitchA, pitchB, 180); + } + + public static float averageYaw(float yawA, float yawB) { + return averageWrapped(yawA, yawB, 360); + } }