Fix NavMesh

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

@@ -25,6 +25,7 @@ import de.steamwar.fightsystem.ai.navmesh.NavMesh;
 import de.steamwar.fightsystem.fight.FightTeam;
 import de.steamwar.sql.SchematicNode;
 import de.steamwar.sql.SteamwarUser;
+import org.bukkit.Material;
 import org.bukkit.entity.Player;
 import org.bukkit.util.Vector;
 
@@ -46,11 +47,14 @@ public class LixfelAI extends AI {
 
     @Override
     public SchematicNode chooseSchematic() {
-        // return SchematicNode.byIdAndUser(SteamwarUser.get(0), 111476);
+        if (false) {
             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);
+        return SchematicNode.byIdAndUser(SteamwarUser.get(0), 98711);
+    }
 
     @Override
     public boolean acceptJoinRequest(Player player, FightTeam team) {
@@ -69,6 +73,7 @@ public class LixfelAI extends AI {
         setReady();
 
         if (navMesh == null) return;
+        if (!getEntity().isOnGround() && getEntity().getLocation().getBlock().getType() != Material.LADDER) return;
 
         if (source == null || destination == null) {
             source = getEntity().getLocation().toVector();

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

@@ -110,6 +110,11 @@ public class NavMesh implements Listener {
             this.z = vector.getBlockZ();
         }
 
+        @Override
+        public String toString() {
+            return x + "," + y + "," + z;
+        }
+
         @Override
         public boolean equals(Object o) {
             if (this == o) return true;
@@ -127,14 +132,19 @@ public class NavMesh implements Listener {
             return new Vector(x, y, z);
         }
 
-        public Pos offset(Pos pos) {
+        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<Pos, Double> floorBlock = new HashMap<>();
     private Map<Pos, Double> ceilingOffset = new HashMap<>();
     private Map<Pos, Set<Pos>> neighbourConnections = new HashMap<>();
+    private Map<Pos, Set<Pos>> reverseNeighbourConnections = new HashMap<>();
 
     private void checkWalkable(int x, int y, int z) {
         Pos pos = new Pos(x, y, z);
@@ -187,37 +197,47 @@ public class NavMesh implements Listener {
     }
 
     private void checkNeighbouring(Pos pos, double posFloorHeight) {
-        neighbourConnections.remove(pos);
+        Set<Pos> 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;
-            double floorDiff = Math.abs(posFloorHeight - otherFloorHeight);
+            if (otherFloorHeight > posFloorHeight && otherFloorHeight - posFloorHeight > PLAYER_JUMP_HEIGHT) continue;
-            if (floorDiff > 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);
             }
         }
     }
@@ -276,7 +296,7 @@ public class NavMesh implements Listener {
             checked.add(pos);
 
             neighbourConnections.getOrDefault(pos, new HashSet<>()).forEach(p -> {
-                Pos n = pos.offset(p);
+                Pos n = pos.add(p);
                 if (checked.contains(n)) return;
                 if (checking.contains(n)) return;
                 checking.add(n);
@@ -304,14 +324,15 @@ public class NavMesh implements Listener {
         while (!checking.isEmpty()) {
             Set<Pos> toCheck = new HashSet<>();
             for (Pos pos : checking) {
-                boolean foundFrom = neighbourConnections.getOrDefault(pos, new HashSet<>()).stream()
+                boolean foundFrom = false;
-                        .map(pos::offset)
+                Set<Pos> successors = reverseNeighbourConnections.get(pos);
-                        .filter(next -> !route.containsKey(next))
+                for (Pos p : successors) {
-                        .anyMatch(next -> {
+                    if (route.containsKey(p)) continue;
-                            route.put(next, pos);
+                    route.put(p, pos);
-                            toCheck.add(next);
+                    toCheck.add(p);
-                            return next.equals(from);
+                    foundFrom = p.equals(from);
-                        });
+                    if (foundFrom) break;
+                }
 
                 if (foundFrom) {
                     List<Pos> path = new ArrayList<>();
@@ -321,9 +342,18 @@ public class NavMesh implements Listener {
                         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<Vector> vectors = path.stream().map(p -> {
-                        double floorHeight = floorBlock.get(p);
+                        Double floorHeight = floorBlock.get(p);
-                        return new Vector(p.x + 0.5, floorHeight, p.z + 0.5);
+                        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<Vector> last = new AtomicReference<>();
@@ -334,6 +364,7 @@ public class NavMesh implements Listener {
                         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));