Update and Fix some NavMesh stuff

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();
@@ -205,15 +221,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 || 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);
             }
         });
     }

FightSystem_Core/src/de/steamwar/fightsystem/ai/LixfelAI.java

@@ -22,6 +22,7 @@ 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.Fight;
 import de.steamwar.fightsystem.fight.FightTeam;
 import de.steamwar.sql.SchematicNode;
 import de.steamwar.sql.SteamwarUser;
@@ -33,6 +34,7 @@ import java.util.Random;
 
 public class LixfelAI extends AI {
 
+    private final Random random = new Random();
     private final REntityServer entityServer = new REntityServer();
     private final FightTeam team;
     private final NavMesh navMesh;
@@ -45,9 +47,10 @@ public class LixfelAI extends AI {
 
     @Override
     public SchematicNode chooseSchematic() {
-        List<SchematicNode> 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);
+        // List<SchematicNode> publics = SchematicNode.getAllSchematicsOfType(0, Config.SchematicType.toDB());
+        // SchematicNode schem = publics.get(new Random().nextInt(publics.size()));
+        // return schem;
     }
 
     @Override
@@ -61,5 +64,20 @@ public class LixfelAI extends AI {
     @Override
     protected void plan() {
         setReady();
+
+        if (navMesh == null) return;
+
+        List<Vector> walkableBlocks = navMesh.getWalkableBlocks(getEntity().getLocation().toVector());
+        if (walkableBlocks.isEmpty()) return;
+        Vector destination = walkableBlocks.get(random.nextInt(walkableBlocks.size()));
+        List<Vector> path = navMesh.path(getEntity().getLocation().toVector(), destination);
+        if (path.isEmpty()) return;
+        chat(getEntity().getLocation().toVector() + " -> " + destination + " = " + path.size());
+        for(Vector p : path) {
+            move(toAIPosition(p));
+        }
+        for (int i = 0; i < 40; i++) {
+            move(path.get(path.size() - 1));
+        }
     }
 }

FightSystem_Core/src/de/steamwar/fightsystem/ai/navmesh/NavMesh.java

@@ -46,7 +46,18 @@ public class NavMesh implements Listener {
     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 BlockFace[] FACES = new BlockFace[]{BlockFace.NORTH, BlockFace.SOUTH, BlockFace.WEST, BlockFace.EAST};
+    private static final Set<Pos> 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;
 
@@ -56,17 +67,19 @@ public class NavMesh implements Listener {
         new OneShotStateDependent(ArenaMode.All, FightState.PostSchemSetup, () -> {
             Bukkit.getScheduler().runTaskLater(FightSystem.getPlugin(), () -> {
                 long time = System.currentTimeMillis();
-                fightTeam.getExtendRegion().forEach(this::checkWalkable);
+                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");
                 // For neighbour map -2-+2 blocks
                 System.out.println(fightTeam + " " + floorBlock.size());
-                iterateWalkableBlocks((vector, ceilingOffset, neightbourConnections) -> {
-                    String connecting = neightbourConnections.stream().map(face -> face.name().substring(0, 1)).collect(Collectors.joining());
+                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) + " " + connecting);
+                    armorStand.setDisplayName("+" + (ceilingOffset == null ? "∞" : ceilingOffset));
                 });
             }, 20);
         });
@@ -86,6 +99,12 @@ public class NavMesh implements Listener {
             this.z = z;
         }
 
+        public Pos(Vector vector) {
+            this.x = vector.getBlockX();
+            this.y = vector.getBlockY();
+            this.z = vector.getBlockZ();
+        }
+
         @Override
         public boolean equals(Object o) {
             if (this == o) return true;
@@ -98,11 +117,19 @@ public class NavMesh implements Listener {
         public int hashCode() {
             return Objects.hash(x, y, z);
         }
+
+        public Vector toVector() {
+            return new Vector(x, y, z);
+        }
+
+        public Pos offset(Pos pos) {
+            return new Pos(x + pos.x, y + pos.y, z + pos.z);
+        }
     }
 
     private Map<Pos, Double> floorBlock = new HashMap<>();
     private Map<Pos, Double> ceilingOffset = new HashMap<>();
-    private Map<Pos, Set<BlockFace>> neighbourConnections = new HashMap<>();
+    private Map<Pos, Set<Pos>> neighbourConnections = new HashMap<>();
 
     private void checkWalkable(int x, int y, int z) {
         Block block = WORLD.getBlockAt(x, y, z);
@@ -151,11 +178,8 @@ public class NavMesh implements Listener {
     }
 
     private void checkNeighbouring(Pos pos, double posFloorHeight) {
-        for (BlockFace blockFace : FACES) {
-            // TODO: Check FenceGates, Doors, usw.
-            for (int cy = pos.y - 2; cy <= pos.y + 2; cy++) {
-                // TODO: Fix Ladder in Underground
-                Pos other = new Pos(pos.x + blockFace.getModX(), cy, pos.z + blockFace.getModZ());
+        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;
             double floorDiff = Math.abs(posFloorHeight - otherFloorHeight);
@@ -164,20 +188,27 @@ public class NavMesh implements Listener {
             Double posCeilingOffset = ceilingOffset.get(pos);
             Double otherCeilingOffset = ceilingOffset.get(other);
             if (posCeilingOffset == null && otherCeilingOffset == null) {
-                    neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(blockFace);
+                neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck);
                 continue;
             }
 
-                if (posCeilingOffset != null && posFloorHeight + posCeilingOffset - otherFloorHeight >= PLAYER_HEIGHT) {
-                    neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(blockFace);
+            if (posCeilingOffset != null && otherCeilingOffset == null) {
+                if (posFloorHeight + posCeilingOffset - otherFloorHeight >= PLAYER_HEIGHT) {
+                    neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck);
                 }
-                if (otherCeilingOffset != null && otherFloorHeight + otherCeilingOffset - posFloorHeight >= PLAYER_HEIGHT) {
-                    neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(blockFace);
+                continue;
             }
+            if (otherCeilingOffset != null && posCeilingOffset == null) {
+                if (otherFloorHeight + otherCeilingOffset - posFloorHeight >= PLAYER_HEIGHT) {
+                    neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck);
                 }
+                continue;
             }
 
-        // TODO: Ladder movement
+            if (posFloorHeight + posCeilingOffset - otherFloorHeight >= PLAYER_HEIGHT && otherFloorHeight + otherCeilingOffset - posFloorHeight >= PLAYER_HEIGHT) {
+                neighbourConnections.computeIfAbsent(pos, __ -> new LinkedHashSet<>()).add(relativeCheck);
+            }
+        }
     }
 
     private boolean overlaps(VoxelShape voxelShape, double x, double y, double z) {
@@ -187,10 +218,10 @@ public class NavMesh implements Listener {
         return overlaps;
     }
 
-    public void iterateWalkableBlocks(TriConsumer<Vector, Double, Set<BlockFace>> consumer) {
+    private void iterateWalkableBlocks(BiConsumer<Vector, Double> consumer) {
         floorBlock.forEach((pos, aDouble) -> {
             Vector vector = new Vector(pos.x + 0.5, aDouble, pos.z + 0.5);
-            consumer.accept(vector, ceilingOffset.get(pos), neighbourConnections.getOrDefault(pos, new HashSet<>()));
+            consumer.accept(vector, ceilingOffset.get(pos));
         });
     }
 
@@ -198,4 +229,83 @@ public class NavMesh implements Listener {
 
         void accept(A a, B b, C c);
     }
+
+    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 List<Vector> getAllWalkableBlocks() {
+        return floorBlock.keySet().stream().map(Pos::toVector).collect(Collectors.toList());
+    }
+
+    public List<Vector> getWalkableBlocks(Vector fromVector) {
+        Pos from = toPos(fromVector);
+        if (from == null) {
+            return Collections.emptyList();
+        }
+
+        Set<Pos> checked = new HashSet<>();
+        List<Pos> 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.offset(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<Vector> path(Vector fromVector, Vector toVector) {
+        Pos from = toPos(fromVector);
+        Pos to = toPos(toVector);
+        if (from == null || to == null) {
+            return Collections.emptyList();
+        }
+
+        List<Pos> checking = new ArrayList<>(Arrays.asList(to));
+        Map<Pos, Pos> route = new HashMap<>();
+        while (!checking.isEmpty()) {
+            Set<Pos> toCheck = new HashSet<>();
+            for (Pos pos : checking) {
+                boolean foundFrom = neighbourConnections.getOrDefault(pos, new HashSet<>()).stream()
+                        .map(pos::offset)
+                        .filter(next -> !route.containsKey(next))
+                        .anyMatch(next -> {
+                            route.put(next, pos);
+                            toCheck.add(next);
+                            return next.equals(from);
+                        });
+
+                if (foundFrom) {
+                    List<Pos> path = new ArrayList<>();
+                    path.add(from);
+
+                    while (path.get(path.size() - 1) != to) {
+                        path.add(route.get(path.get(path.size() - 1)));
+                    }
+
+                    return path.stream().map(p -> {
+                        double floorHeight = floorBlock.get(p);
+                        return new Vector(p.x + 0.5, floorHeight, p.z + 0.5);
+                    }).collect(Collectors.toList());
+                }
+            }
+
+            checking.clear();
+            checking.addAll(toCheck);
+        }
+
+        return Collections.emptyList();
+    }
 }