diff --git a/FightSystem_Core/src/de/steamwar/fightsystem/FightSystem.java b/FightSystem_Core/src/de/steamwar/fightsystem/FightSystem.java index 5de02c4..2aa5739 100644 --- a/FightSystem_Core/src/de/steamwar/fightsystem/FightSystem.java +++ b/FightSystem_Core/src/de/steamwar/fightsystem/FightSystem.java @@ -22,6 +22,7 @@ package de.steamwar.fightsystem; import com.comphenix.tinyprotocol.TinyProtocol; import de.steamwar.core.Core; import de.steamwar.fightsystem.ai.LixfelAI; +import de.steamwar.fightsystem.ai.navmesh.NavMesh; import de.steamwar.fightsystem.commands.*; import de.steamwar.fightsystem.countdown.*; import de.steamwar.fightsystem.event.HellsBells; @@ -42,6 +43,7 @@ import de.steamwar.fightsystem.utils.*; import de.steamwar.fightsystem.winconditions.*; import de.steamwar.message.Message; import de.steamwar.sql.SchematicNode; +import de.steamwar.sql.SteamwarUser; import org.bukkit.Bukkit; import org.bukkit.plugin.java.JavaPlugin; @@ -167,6 +169,14 @@ public class FightSystem extends JavaPlugin { }else if(Config.mode == ArenaMode.PREPARE) { Fight.getUnrotated().setSchem(SchematicNode.getSchematicNode(Config.PrepareSchemID)); } + + FightStatistics.unrank(); + FightWorld.forceLoad(); + + Bukkit.getScheduler().runTask(getPlugin(), () -> { + new LixfelAI(Fight.getBlueTeam(), "Lixfel.AI"); + new LixfelAI(Fight.getRedTeam(), "YoyoNow.AI"); + }); } @Override diff --git a/FightSystem_Core/src/de/steamwar/fightsystem/ai/AI.java b/FightSystem_Core/src/de/steamwar/fightsystem/ai/AI.java index 94213f9..1dd718f 100644 --- a/FightSystem_Core/src/de/steamwar/fightsystem/ai/AI.java +++ b/FightSystem_Core/src/de/steamwar/fightsystem/ai/AI.java @@ -120,6 +120,22 @@ public abstract class AI { Chat.broadcastChat("PARTICIPANT_CHAT", team.getColoredName(), entity.getName(), message); } + protected Vector toAIPosition(Vector location) { + Region extend = team.getExtendRegion(); + if(Fight.getUnrotated() == team) + return new Vector( + location.getX() - extend.getMinX(), + location.getY() - team.getSchemRegion().getMinY(), + location.getZ() - extend.getMinZ() + ); + else + return new Vector( + extend.getMaxX() - location.getX(), + location.getY() - team.getSchemRegion().getMinY(), + extend.getMaxZ() - location.getZ() + ); + } + protected Vector getPosition() { Location location = entity.getLocation(); Region extend = team.getExtendRegion(); @@ -209,15 +225,17 @@ public abstract class AI { public void run() { Location location = entity.getLocation(); Location target = translate(pos, false); - if(Math.abs(location.getX() - target.getX()) > 1.9 || Math.abs(location.getY() - target.getY()) > 1.9 || Math.abs(location.getZ() - target.getZ()) > 1.9) { + /* + if(Math.abs(location.getX() - target.getX()) > 1 || Math.abs(location.getY() - target.getY()) > 1.2 || Math.abs(location.getZ() - target.getZ()) > 1) { FightSystem.getPlugin().getLogger().log(Level.INFO, () -> entity.getName() + ": Overdistance movement " + location.toVector() + " " + target.toVector()); return; } + */ if(!team.getFightPlayer(entity).canEntern() && !team.getExtendRegion().inRegion(target)) return; - entity.teleport(target, PlayerTeleportEvent.TeleportCause.COMMAND); + entity.teleport(target, PlayerTeleportEvent.TeleportCause.PLUGIN); } }); } diff --git a/FightSystem_Core/src/de/steamwar/fightsystem/ai/LixfelAI.java b/FightSystem_Core/src/de/steamwar/fightsystem/ai/LixfelAI.java index c79d837..b01104a 100644 --- a/FightSystem_Core/src/de/steamwar/fightsystem/ai/LixfelAI.java +++ b/FightSystem_Core/src/de/steamwar/fightsystem/ai/LixfelAI.java @@ -19,11 +19,15 @@ package de.steamwar.fightsystem.ai; +import de.steamwar.entity.REntityServer; import de.steamwar.fightsystem.Config; +import de.steamwar.fightsystem.ai.navmesh.NavMesh; import de.steamwar.fightsystem.fight.FightTeam; import de.steamwar.fightsystem.states.FightState; import de.steamwar.sql.SchematicNode; import de.steamwar.sql.SteamwarUser; +import org.bukkit.Material; +import org.bukkit.entity.Player; import org.bukkit.util.Vector; import java.util.*; @@ -31,52 +35,40 @@ import java.util.function.Function; public class LixfelAI extends AI { - private Random random; - private LixfelPathplanner pathplanner; - private Action action; + private final Random random = new Random(); + private final REntityServer entityServer = new REntityServer(); + private final FightTeam team; + private final NavMesh navMesh; public LixfelAI(FightTeam team, String user) { super(team, SteamwarUser.get(user)); + this.team = team; + navMesh = new NavMesh(team, entityServer); } @Override public SchematicNode chooseSchematic() { - random = new Random(); - - List publics = SchematicNode.getAllSchematicsOfType(0, Config.SchematicType.toDB()); - SchematicNode schem = publics.get(random.nextInt(publics.size())); - schem = publics.stream().filter(s -> s.getName().equals("TheUnderground")).findAny().orElse(schem); - - pathplanner = new LixfelPathplanner(schem); - - action = new BlockAction(Collections.emptyList(), Collections.singletonList(new Vector(25, 15, 25)), - new MoveToRangeAction(new Vector(21, 15, 24), - new FunctionAction(ai -> { - setReady(); - return FightState.getFightState() == FightState.PRE_RUNNING; - }, - new WaitAction(420, - new BlockAction(Collections.emptyList(), Collections.singletonList(new Vector(21, 15, 24)), - new FunctionAction(ai -> FightState.getFightState() == FightState.RUNNING, - new ChooseCannonAction( - new Cannon(new Vector(11,25,11), 10,23,14, 12,23,14, 10,23,12, 12,23,12, 12,24,15, 10,24,14, 12,23,15, 12,24,14, 11,24,12, 10,24,15, 12,24,12, 10,23,15, 10,24,12), - new Cannon(new Vector(39,25,11), 38,24,15, 38,23,12, 40,23,14, 40,24,12, 40,23,15, 40,24,14, 39,24,12, 38,23,14, 40,24,15, 40,23,12, 38,23,15, 38,24,12, 38,24,14), - new Cannon(new Vector(12,18,11), 13,17,15, 13,17,16, 13,18,16, 13,18,14, 14,18,14, 14,17,15, 14,17,16, 14,18,16, 13,17,14, 14,17,14, 13,18,15, 14,18,15), - new Cannon(new Vector(38,18,11), 37,18,14, 36,18,14, 36,17,15, 37,17,15, 37,18,15, 36,18,15, 36,17,14, 37,17,14, 36,17,16, 37,17,16, 36,18,16, 37,18,16), - new Cannon(new Vector(8,9,16), 10,11,17, 10,8,19, 10,8,17, 11,8,19, 11,8,15, 10,8,15, 12,8,15, 10,10,15, 12,7,19, 11,10,15, 12,8,19, 12,7,15, 11,7,15, 10,9,17, 11,9,19, 12,9,19, 10,9,19, 10,7,17, 11,7,19, 10,7,15, 10,7,19, 10,10,17, 12,9,15, 10,9,15, 11,9,15, 12,10,19, 11,10,19, 12,10,15, 10,6,17, 10,10,19), - new Cannon(null, 23,5,9, 23,6,9, 23,7,9, 23,8,9), - new Cannon(null, 27,5,9, 27,6,9, 27,7,9, 27,8,9) - ) - ) - ) - ) - ) - ) - ); - - return schem; + if (false) { + List publics = SchematicNode.getAllSchematicsOfType(0, Config.SchematicType.toDB()); + SchematicNode schem = publics.get(new Random().nextInt(publics.size())); + return schem; + } + // return SchematicNode.byIdAndUser(SteamwarUser.get(0), 111476); + return SchematicNode.byIdAndUser(SteamwarUser.get(0), 98711); } + @Override + public boolean acceptJoinRequest(Player player, FightTeam team) { + if (team == this.team) { + entityServer.addPlayer(player); + } + return super.acceptJoinRequest(player, team); + } + + private Vector source = null; + private Vector destination = null; + private int index = 0; + LixfelPathplanner getPathplanner() { return pathplanner; } @@ -87,167 +79,52 @@ public class LixfelAI extends AI { @Override protected void plan() { - action.plan(this); - } + setReady(); - private abstract static class Action { - private Action next; + if (navMesh == null) return; + if (!getEntity().isOnGround() && getEntity().getLocation().getBlock().getType() != Material.LADDER) return; - public Action(Action next) { - this.next = next; + if (source == null || destination == null) { + source = getEntity().getLocation().toVector(); + List walkableBlocks = navMesh.getWalkableBlocks(source); + if (walkableBlocks.isEmpty()) return; + destination = walkableBlocks.get(random.nextInt(walkableBlocks.size())); + index = 0; } - public abstract void plan(LixfelAI ai); - - public void setNext(Action followup) { - this.next = followup; + List oldRoute = navMesh.path(source, destination); + navMesh.update(getEntity().getLocation().toVector()); + List path = navMesh.path(source, destination); + // TODO: New Route detection + if (path.isEmpty()) { + source = null; + destination = null; + chat("no route"); + return; + } + if (!oldRoute.equals(path)) { + source = getEntity().getLocation().toVector(); + index = 0; + chat("new route"); + return; + } + if (index == 0) { + chat(source + " -> " + destination + " = " + path.size()); } - protected void next(LixfelAI ai) { - ai.setAction(next); - next.plan(ai); - } - } - - private static class FunctionAction extends Action { - - private final Function action; - public FunctionAction(Function action, Action followup) { - super(followup); - this.action = action; + if (index > path.size()) { + source = null; + destination = null; + chat("route cancelled"); + return; } - @Override - public void plan(LixfelAI ai) { - if(action.apply(ai)) - next(ai); - } - } + Vector location = path.get(index++); + move(toAIPosition(location)); - private static class WaitAction extends Action { - - private int remaining; - public WaitAction(int ticks, Action followup) { - super(followup); - this.remaining = ticks; - } - - @Override - public void plan(LixfelAI ai) { - if(--remaining == 0) - next(ai); - } - } - - private static class MoveToRangeAction extends Action { - - private final Vector target; - public MoveToRangeAction(Vector target, Action next) { - super(next); - this.target = target; - } - - @Override - public void plan(LixfelAI ai) { - Vector position = ai.getPosition(); - Vector eyePosition = new Vector(0, ai.getEntity().getEyeHeight(), 0).add(position); - - if(eyePosition.distance(target) > 5) { - List path = new ArrayList<>(ai.getPathplanner().planToRange(position, target, 5.0)); - if(path.isEmpty()) - path.add(new Vector(0, 1, 0).add(position)); - - ai.move(path.get(0)); - } else { - next(ai); - } - } - } - - private static class BlockAction extends Action { - private final List tntToPlace; - private final List interactables; - - public BlockAction(List tntToPlace, List interactables, Action followup) { - super(followup); - this.tntToPlace = new ArrayList<>(tntToPlace); - this.interactables = new ArrayList<>(interactables); - } - - @Override - public void plan(LixfelAI ai) { - if(!tntToPlace.isEmpty()) { - if(outOfRange(ai, tntToPlace.get(0))) - return; - - ai.setTNT(tntToPlace.remove(0)); - return; - } - - if(!interactables.isEmpty()) { - if(outOfRange(ai, interactables.get(0))) - return; - - ai.interact(interactables.remove(0)); - return; - } - - next(ai); - } - - private boolean outOfRange(LixfelAI ai, Vector location) { - Vector position = ai.getPosition(); - Vector eyePosition = new Vector(0, ai.getEntity().getEyeHeight(), 0).add(position); - boolean outOfRange = eyePosition.distance(location) > 5; - - if(outOfRange) { - List path = new ArrayList<>(ai.getPathplanner().planToRange(position, location, 5.0)); - if(path.isEmpty()) - path.add(new Vector(0, 1, 0).add(position)); - - System.out.println(ai.getEntity().getName() + ": " + position + "->" + path.get(0)); - ai.move(path.get(0)); - } - - return outOfRange; - } - } - - private static class ChooseCannonAction extends Action { - - private final List cannons; - private final List shootingList = new ArrayList<>(); - - public ChooseCannonAction(Cannon... cannons) { - super(null); - this.cannons = Arrays.asList(cannons); - Collections.shuffle(this.cannons); - } - - @Override - public void plan(LixfelAI ai) { - if(shootingList.isEmpty()) - shootingList.addAll(cannons); - - setNext(shootingList.remove(0).toAction(this)); - next(ai); - } - } - - private class Cannon { - private final Vector activator; - private final List tnt = new ArrayList<>(); - - public Cannon(Vector activator, int... tntpos) { - this.activator = activator; - - for(int i = 0; i < tntpos.length; i+=3) { - tnt.add(new Vector(tntpos[i], tntpos[i+1], tntpos[i+2])); - } - } - - public BlockAction toAction(Action next) { - return new BlockAction(new ArrayList<>(tnt), activator == null ? Collections.emptyList() : Collections.singletonList(activator), next); + if (index == path.size()) { + source = null; + destination = null; } } } diff --git a/FightSystem_Core/src/de/steamwar/fightsystem/ai/navmesh/NavMesh.java b/FightSystem_Core/src/de/steamwar/fightsystem/ai/navmesh/NavMesh.java new file mode 100644 index 0000000..d746c71 --- /dev/null +++ b/FightSystem_Core/src/de/steamwar/fightsystem/ai/navmesh/NavMesh.java @@ -0,0 +1,388 @@ +/* + * This file is a part of the SteamWar software. + * + * Copyright (C) 2023 SteamWar.de-Serverteam + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + */ + +package de.steamwar.fightsystem.ai.navmesh; + +import de.steamwar.entity.RArmorStand; +import de.steamwar.entity.REntity; +import de.steamwar.entity.REntityServer; +import de.steamwar.fightsystem.ArenaMode; +import de.steamwar.fightsystem.FightSystem; +import de.steamwar.fightsystem.fight.FightTeam; +import de.steamwar.fightsystem.states.FightState; +import de.steamwar.fightsystem.states.OneShotStateDependent; +import org.bukkit.Bukkit; +import org.bukkit.Material; +import org.bukkit.World; +import org.bukkit.block.Block; +import org.bukkit.block.BlockFace; +import org.bukkit.event.Listener; +import org.bukkit.util.BoundingBox; +import org.bukkit.util.Vector; +import org.bukkit.util.VoxelShape; + +import java.util.*; +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.BiConsumer; +import java.util.stream.Collectors; + +public class NavMesh implements Listener { + + private static final World WORLD = Bukkit.getWorlds().get(0); + private static final double PLAYER_JUMP_HEIGHT = 1.25; + private static final double PLAYER_HEIGHT = 1.8125; + private static final BoundingBox PLAYER_SHADOW = new BoundingBox(0.2, 0, 0.2, 0.8, 1, 0.8); + private static final Set RELATIVE_BLOCKS_TO_CHECK = new HashSet<>(); + + static { + for (int y = -2; y <= 2; y++) { + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.NORTH.getDirection().setY(y))); + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.SOUTH.getDirection().setY(y))); + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.EAST.getDirection().setY(y))); + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.WEST.getDirection().setY(y))); + } + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.UP.getDirection())); + RELATIVE_BLOCKS_TO_CHECK.add(new Pos(BlockFace.DOWN.getDirection())); + } + + private FightTeam fightTeam; + private List rEntities = new ArrayList<>(); + private REntityServer entityServer; + + public NavMesh(FightTeam fightTeam, REntityServer entityServer) { + this.fightTeam = fightTeam; + this.entityServer = entityServer; + + new OneShotStateDependent(ArenaMode.All, FightState.PostSchemSetup, () -> { + Bukkit.getScheduler().runTaskLater(FightSystem.getPlugin(), () -> { + long time = System.currentTimeMillis(); + fightTeam.getExtendRegion().forEach((x, y, z) -> { + if (y < fightTeam.getSchemRegion().getMinY()) return; + checkWalkable(x, y, z); + }); + floorBlock.forEach(this::checkNeighbouring); + System.out.println(System.currentTimeMillis() - time + " ms"); + /* + iterateWalkableBlocks((vector, ceilingOffset) -> { + RArmorStand armorStand = new RArmorStand(entityServer, vector.toLocation(WORLD), RArmorStand.Size.MARKER); + armorStand.setNoGravity(true); + armorStand.setInvisible(true); + armorStand.setDisplayName("+" + (ceilingOffset == null ? "∞" : ceilingOffset)); + }); + */ + }, 20); + }); + new OneShotStateDependent(ArenaMode.All, FightState.Spectate, () -> { + floorBlock.clear(); + }); + } + + private static class Pos { + private int x; + private int y; + private int z; + + public Pos(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + public Pos(Vector vector) { + this.x = vector.getBlockX(); + this.y = vector.getBlockY(); + this.z = vector.getBlockZ(); + } + + @Override + public String toString() { + return x + "," + y + "," + z; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof Pos)) return false; + Pos pos = (Pos) o; + return x == pos.x && y == pos.y && z == pos.z; + } + + @Override + public int hashCode() { + return Objects.hash(x, y, z); + } + + public Vector toVector() { + return new Vector(x, y, z); + } + + public Pos add(Pos pos) { + return new Pos(x + pos.x, y + pos.y, z + pos.z); + } + + public Pos subtract(Pos pos) { + return new Pos(x - pos.x, y - pos.y, z - pos.z); + } + } + + private Map floorBlock = new HashMap<>(); + private Map ceilingOffset = new HashMap<>(); + private Map> neighbourConnections = new HashMap<>(); + private Map> reverseNeighbourConnections = new HashMap<>(); + + private void checkWalkable(int x, int y, int z) { + Pos pos = new Pos(x, y, z); + floorBlock.remove(pos); + ceilingOffset.remove(pos); + + Block block = WORLD.getBlockAt(x, y, z); + VoxelShape floor = block.getCollisionShape(); + if (block.getType() != Material.LADDER && !overlaps(floor, 0, 0, 0)) return; + + Double floorHeight = null; + for (BoundingBox box : floor.getBoundingBoxes()) { + double by = box.getMaxY(); + if (!overlaps(floor, 0, by, 0)) { + if (floorHeight == null) { + floorHeight = by; + } else { + floorHeight = Math.min(floorHeight, by); + } + } + } + if (floorHeight == null) return; + + Double ceilingOffset = null; + for (int cy = y + 1; cy < fightTeam.getExtendRegion().getMaxY(); cy++) { + VoxelShape current = WORLD.getBlockAt(x, cy, z).getCollisionShape(); + if (!overlaps(current, 0, 0, 0)) continue; + + Double ceilingHeight = null; + for (BoundingBox box : current.getBoundingBoxes()) { + double by = box.getMinY(); + if (!overlaps(current, 0, -(1 - by), 0)) { + if (ceilingHeight == null) { + ceilingHeight = by; + } else { + ceilingHeight = Math.max(ceilingHeight, by); + } + } + } + if (ceilingHeight != null) { + ceilingOffset = cy - y + ceilingHeight - floorHeight; + } + break; + } + + if (ceilingOffset != null && ceilingOffset < PLAYER_HEIGHT) return; + + floorBlock.put(pos, y + floorHeight); + this.ceilingOffset.put(pos, ceilingOffset); + } + + private void checkNeighbouring(Pos pos, double posFloorHeight) { + Set connections = neighbourConnections.remove(pos); + if (connections != null) { + connections.forEach(p -> { + reverseNeighbourConnections.getOrDefault(pos.add(p), Collections.emptySet()).remove(pos); + }); + } + + for (Pos relativeCheck : RELATIVE_BLOCKS_TO_CHECK) { + Pos other = new Pos(pos.x + relativeCheck.x, pos.y + relativeCheck.y, pos.z + relativeCheck.z); + Double otherFloorHeight = floorBlock.get(other); + if (otherFloorHeight == null) continue; + if (otherFloorHeight > posFloorHeight && otherFloorHeight - posFloorHeight > PLAYER_JUMP_HEIGHT) continue; + // double floorDiff = Math.abs(posFloorHeight - otherFloorHeight); + // if (floorDiff > PLAYER_JUMP_HEIGHT) continue; + + Double posCeilingOffset = ceilingOffset.get(pos); + Double otherCeilingOffset = ceilingOffset.get(other); + if (posCeilingOffset == null && otherCeilingOffset == null) { + neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck); + reverseNeighbourConnections.computeIfAbsent(pos.add(relativeCheck), __ -> new LinkedHashSet<>()).add(pos); + continue; + } + + if (posCeilingOffset != null && otherCeilingOffset == null) { + if (posFloorHeight + posCeilingOffset - otherFloorHeight >= PLAYER_HEIGHT) { + neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck); + reverseNeighbourConnections.computeIfAbsent(pos.add(relativeCheck), __ -> new LinkedHashSet<>()).add(pos); + } + continue; + } + if (otherCeilingOffset != null && posCeilingOffset == null) { + if (otherFloorHeight + otherCeilingOffset - posFloorHeight >= PLAYER_HEIGHT) { + neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck); + reverseNeighbourConnections.computeIfAbsent(pos.add(relativeCheck), __ -> new LinkedHashSet<>()).add(pos); + } + continue; + } + + if (posFloorHeight + posCeilingOffset - otherFloorHeight >= PLAYER_HEIGHT && otherFloorHeight + otherCeilingOffset - posFloorHeight >= PLAYER_HEIGHT) { + neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck); + reverseNeighbourConnections.computeIfAbsent(pos.add(relativeCheck), __ -> new LinkedHashSet<>()).add(pos); + } + } + } + + private boolean overlaps(VoxelShape voxelShape, double x, double y, double z) { + PLAYER_SHADOW.shift(x, y, z); + boolean overlaps = voxelShape.overlaps(PLAYER_SHADOW); + PLAYER_SHADOW.shift(-x, -y, -z); + return overlaps; + } + + private void iterateWalkableBlocks(BiConsumer consumer) { + floorBlock.forEach((pos, aDouble) -> { + Vector vector = new Vector(pos.x + 0.5, aDouble, pos.z + 0.5); + consumer.accept(vector, ceilingOffset.get(pos)); + }); + } + + private Pos toPos(Vector vector) { + Pos pos = new Pos(vector); + if (floorBlock.containsKey(pos)) return pos; + pos = new Pos(pos.x, pos.y - 1, pos.z); + if (floorBlock.containsKey(pos)) return pos; + return null; + } + + public void update(Vector posVector) { + Pos pos = toPos(posVector); + if (pos == null) return; + + for (int x = -2; x <= 2; x++) { + for (int z = -2; z <= 2; z++) { + for (int y = fightTeam.getSchemRegion().getMinY(); y <= pos.y + 2; y++) { + checkWalkable(pos.x + x, y, pos.z + z); + } + } + } + floorBlock.forEach(this::checkNeighbouring); + } + + public List getAllWalkableBlocks() { + return floorBlock.keySet().stream().map(Pos::toVector).collect(Collectors.toList()); + } + + public List getWalkableBlocks(Vector fromVector) { + Pos from = toPos(fromVector); + if (from == null) { + return Collections.emptyList(); + } + + Set checked = new HashSet<>(); + List checking = new ArrayList<>(); + checking.add(from); + while (!checking.isEmpty()) { + Pos pos = checking.remove(0); + checked.add(pos); + + neighbourConnections.getOrDefault(pos, new HashSet<>()).forEach(p -> { + Pos n = pos.add(p); + if (checked.contains(n)) return; + if (checking.contains(n)) return; + checking.add(n); + }); + } + + return checked.stream().map(Pos::toVector).collect(Collectors.toList()); + } + + public List path(Vector fromVector, Vector toVector) { + rEntities.forEach(REntity::die); + rEntities.clear(); + + Pos from = toPos(fromVector); + Pos to = toPos(toVector); + if (from == null || to == null) { + return Collections.emptyList(); + } + if (from.equals(to)) { + return Collections.emptyList(); + } + + List checking = new ArrayList<>(Arrays.asList(to)); + Map route = new HashMap<>(); + while (!checking.isEmpty()) { + Set toCheck = new HashSet<>(); + for (Pos pos : checking) { + boolean foundFrom = false; + Set successors = reverseNeighbourConnections.get(pos); + for (Pos p : successors) { + if (route.containsKey(p)) continue; + route.put(p, pos); + toCheck.add(p); + foundFrom = p.equals(from); + if (foundFrom) break; + } + + if (foundFrom) { + List path = new ArrayList<>(); + path.add(from); + + while (path.get(path.size() - 1) != to) { + path.add(route.get(path.get(path.size() - 1))); + } + + for (int i = path.size() - 1; i > 0; i--) { + Pos current = path.get(i); + Pos last = path.get(i - 1); + if (last.y > current.y) { + path.set(i, new Pos(current.x, last.y, current.z)); + } + } + + List vectors = path.stream().map(p -> { + Double floorHeight = floorBlock.get(p); + if (p.y > (floorHeight == null ? p.y : floorHeight)) floorHeight = null; + return new Vector(p.x + 0.5, floorHeight == null ? p.y : floorHeight, p.z + 0.5); + }).collect(Collectors.toList()); + + AtomicReference last = new AtomicReference<>(); + vectors.forEach(vector -> { + RArmorStand armorStand = new RArmorStand(entityServer, vector.toLocation(WORLD), RArmorStand.Size.MARKER); + armorStand.setInvisible(true); + armorStand.setNoGravity(true); + armorStand.setDisplayName("+"); + rEntities.add(armorStand); + + if (true) return; + Vector lastVector = last.getAndSet(vector); + if (lastVector == null) return; + lastVector = lastVector.clone().add(vector).divide(new Vector(2, 2, 2)); + armorStand = new RArmorStand(entityServer, lastVector.toLocation(WORLD), RArmorStand.Size.MARKER); + armorStand.setInvisible(true); + armorStand.setNoGravity(true); + armorStand.setDisplayName("+"); + rEntities.add(armorStand); + }); + + return vectors; + } + } + + checking.clear(); + checking.addAll(toCheck); + } + + return Collections.emptyList(); + } +}