Improve PathFinder - add support for finding Path<T extends Movable>

api/src/main/java/com/bartlomiejpluta/base/api/move/Direction.java

@@ -50,4 +50,22 @@ public enum Direction {
          return UP;
       }
    }
+
+   public static Direction ofVector(int x, int y) {
+      // X Versor = [1, 0]
+      // dot = 1 * vector.x + 0 * vector.y = vector.x
+      // det = 1 * vector.y - 0 * vector.x = vector.y
+      // angle = atan2(det, dot) = atan2(vector.y, vector.x)
+      float angle = atan2(y, x);
+
+      if (-PI / 4 < angle && angle < PI / 4) {
+         return RIGHT;
+      } else if (PI / 4 <= angle && angle <= 3 * PI / 4) {
+         return DOWN;
+      } else if (3 * PI / 4 < angle && angle < 5 * PI / 4) {
+         return LEFT;
+      } else {
+         return UP;
+      }
+   }
 }

api/src/main/java/com/bartlomiejpluta/base/lib/ai/FollowEntityAI.java

@@ -30,12 +30,12 @@ public class FollowEntityAI implements AI {
       var distance = npc.manhattanDistance(target);
 
       if (!npc.isMoving() && 1 < distance && distance < range && accumulator >= recalculateInterval) {
-         var path = pathFinder.findPath(layer, npc.getCoordinates(), target.getCoordinates());
+         var path = pathFinder.findSequence(layer, npc.getCoordinates(), target.getCoordinates());
 
          if (!path.isEmpty()) {
             accumulator = recalculateInterval;
 
-            var node = path.getLast().sub(npc.getCoordinates(), new Vector2i());
+            var node = path.getFirst().sub(npc.getCoordinates(), new Vector2i());
             var direction = Direction.ofVector(node);
             var movement = npc.prepareMovement(direction);
             layer.pushMovement(movement);

api/src/main/java/com/bartlomiejpluta/base/lib/ai/FollowPathAI.java

@@ -3,52 +3,18 @@ package com.bartlomiejpluta.base.lib.ai;
 import com.bartlomiejpluta.base.api.ai.AI;
 import com.bartlomiejpluta.base.api.ai.NPC;
 import com.bartlomiejpluta.base.api.map.layer.object.ObjectLayer;
-import com.bartlomiejpluta.base.api.move.Direction;
+import com.bartlomiejpluta.base.util.path.Path;
-import com.bartlomiejpluta.base.util.path.NPCPath;
 import com.bartlomiejpluta.base.util.path.PathExecutor;
 
-public class FollowPathAI implements AI {
+public class FollowPathAI<T extends NPC> implements AI {
-   private final PathExecutor<NPC> executor;
+   private final PathExecutor<T> executor;
-   private final NPCPath path;
 
-   public FollowPathAI(NPC npc) {
+   public FollowPathAI(T npc, Integer repeat, Path<T> path) {
-      this(npc, null);
-   }
-
-   public FollowPathAI(NPC npc, Integer repeat) {
-      var path = new NPCPath();
       this.executor = new PathExecutor<>(npc, repeat, path);
-      this.path = path;
    }
 
    @Override
    public void nextActivity(ObjectLayer layer, float dt) {
       executor.execute(layer, dt);
    }
-
-
-   public FollowPathAI move(Direction direction) {
-      path.move(direction);
-      return this;
-   }
-
-   public FollowPathAI move(Direction direction, boolean ignore) {
-      path.move(direction, ignore);
-      return this;
-   }
-
-   public FollowPathAI turn(Direction direction) {
-      path.turn(direction);
-      return this;
-   }
-
-   public FollowPathAI wait(float seconds) {
-      path.wait(seconds);
-      return this;
-   }
-
-   public FollowPathAI run(Runnable runnable) {
-      path.run(runnable);
-      return this;
-   }
 }

api/src/main/java/com/bartlomiejpluta/base/util/path/NPCPath.java

@@ -9,4 +9,40 @@ public class NPCPath extends Path<NPC> {
       path.add(new TurnSegment<>(direction));
       return this;
    }
+
+   @Override
+   public NPCPath add(PathSegment<NPC> segment) {
+      super.add(segment);
+      return this;
+   }
+
+   @Override
+   public NPCPath addFirst(PathSegment<NPC> segment) {
+      super.addFirst(segment);
+      return this;
+   }
+
+   @Override
+   public NPCPath move(Direction direction) {
+      super.move(direction);
+      return this;
+   }
+
+   @Override
+   public NPCPath move(Direction direction, boolean ignore) {
+      super.move(direction, ignore);
+      return this;
+   }
+
+   @Override
+   public NPCPath wait(float seconds) {
+      super.wait(seconds);
+      return this;
+   }
+
+   @Override
+   public NPCPath run(Runnable runnable) {
+      super.run(runnable);
+      return this;
+   }
 }

api/src/main/java/com/bartlomiejpluta/base/util/path/Path.java

@@ -18,6 +18,11 @@ public class Path<T extends Movable> {
       return this;
    }
 
+   public Path<T> addFirst(PathSegment<T> segment) {
+      path.add(0, segment);
+      return this;
+   }
+
    public Path<T> move(Direction direction) {
       path.add(new MoveSegment<>(direction));
       return this;

api/src/main/java/com/bartlomiejpluta/base/util/pathfinder/AstarPathFinder.java

@@ -1,6 +1,10 @@
 package com.bartlomiejpluta.base.util.pathfinder;
 
 import com.bartlomiejpluta.base.api.map.layer.object.ObjectLayer;
+import com.bartlomiejpluta.base.api.move.Direction;
+import com.bartlomiejpluta.base.api.move.Movable;
+import com.bartlomiejpluta.base.util.path.MoveSegment;
+import com.bartlomiejpluta.base.util.path.Path;
 import org.joml.Vector2i;
 import org.joml.Vector2ic;
 
@@ -13,6 +17,7 @@ import java.util.function.Function;
 import static java.lang.Math.abs;
 
 public class AstarPathFinder implements PathFinder {
+   private static final LinkedList<Vector2ic> EMPTY_LINKED_LIST = new LinkedList<>();
 
    /*
     * We are interested in following adjacent
@@ -38,7 +43,51 @@ public class AstarPathFinder implements PathFinder {
    }
 
    @Override
-   public LinkedList<Vector2ic> findPath(ObjectLayer layer, Vector2ic start, Vector2ic end) {
+   public <T extends Movable> Path<T> findPath(ObjectLayer layer, T start, Vector2ic end) {
+      return astar(layer, start.getCoordinates(), end, this::recreatePath);
+   }
+
+   private <T extends Movable> Path<T> recreatePath(Node node) {
+      if (node == null) {
+         return new Path<>();
+      }
+
+      var path = new Path<T>();
+      var current = node;
+
+      while (current.parent != null) {
+         path.addFirst(new MoveSegment<>(Direction.ofVector(
+               current.position.x() - current.parent.position.x(),
+               current.position.y() - current.parent.position.y()
+         )));
+         current = current.parent;
+      }
+
+      return path;
+   }
+
+   @Override
+   public LinkedList<Vector2ic> findSequence(ObjectLayer layer, Vector2ic start, Vector2ic end) {
+      return astar(layer, start, end, this::recreateSequence);
+   }
+
+   private LinkedList<Vector2ic> recreateSequence(Node node) {
+      if (node == null) {
+         return EMPTY_LINKED_LIST;
+      }
+
+      var list = new LinkedList<Vector2ic>();
+      var current = node;
+
+      while (current.parent != null) {
+         list.addFirst(current.position);
+         current = current.parent;
+      }
+
+      return list;
+   }
+
+   private <P> P astar(ObjectLayer layer, Vector2ic start, Vector2ic end, Function<Node, P> pathProducer) {
       var columns = layer.getMap().getColumns();
       var rows = layer.getMap().getRows();
 
@@ -65,7 +114,7 @@ public class AstarPathFinder implements PathFinder {
          // It determines the maximum algorithm depth
          // It's not the part of model A* algorithm.
          if (closed.size() > maxNodes) {
-            return new LinkedList<>();
+            return pathProducer.apply(null);
          }
 
          // We are retrieving the node with the **smallest** f score
@@ -78,7 +127,7 @@ public class AstarPathFinder implements PathFinder {
          // If we found the node with f score and it is
          // actually an end node, we have most likely found a best path
          if (current.equals(endNode)) {
-            return recreatePath(current);
+            return pathProducer.apply(current);
          }
 
          adjacent:
@@ -156,25 +205,13 @@ public class AstarPathFinder implements PathFinder {
 
       // If open list is empty and we didn't reach the end node
       // it means that the path probably does not exist at all
-      return new LinkedList<>();
+      return pathProducer.apply(null);
    }
 
    private float manhattanDistance(Vector2ic a, Vector2ic b) {
       return (abs(a.x() - b.x()) + abs(a.y() - b.y()));
    }
 
-   private LinkedList<Vector2ic> recreatePath(Node node) {
-      var current = node;
-      var list = new LinkedList<Vector2ic>();
-
-      while (current.parent != null) {
-         list.add(current.position);
-         current = current.parent;
-      }
-
-      return list;
-   }
-
    private static class Node implements Comparable<Node> {
       public Node parent;
       public final Vector2ic position;

api/src/main/java/com/bartlomiejpluta/base/util/pathfinder/PathFinder.java

@@ -1,10 +1,14 @@
 package com.bartlomiejpluta.base.util.pathfinder;
 
 import com.bartlomiejpluta.base.api.map.layer.object.ObjectLayer;
+import com.bartlomiejpluta.base.api.move.Movable;
+import com.bartlomiejpluta.base.util.path.Path;
 import org.joml.Vector2ic;
 
 import java.util.LinkedList;
 
 public interface PathFinder {
-   LinkedList<Vector2ic> findPath(ObjectLayer layer, Vector2ic start, Vector2ic end);
+   <T extends Movable> Path<T> findPath(ObjectLayer layer, T start, Vector2ic end);
+
+   LinkedList<Vector2ic> findSequence(ObjectLayer layer, Vector2ic start, Vector2ic end);
 }