Implemented variation of A* that includes support for clearance (see #63)

Added SizeComponent and implemented auto sizing within engine entity creation

core/src/com/riiablo/engine/Engine.java

@@ -33,6 +33,7 @@ import com.riiablo.engine.component.ObjectComponent;
 import com.riiablo.engine.component.PathComponent;
 import com.riiablo.engine.component.PlayerComponent;
 import com.riiablo.engine.component.PositionComponent;
+import com.riiablo.engine.component.SizeComponent;
 import com.riiablo.engine.component.TypeComponent;
 import com.riiablo.engine.component.VelocityComponent;
 import com.riiablo.engine.component.WarpComponent;
@@ -308,6 +309,9 @@ public class Engine extends PooledEngine {
       // FIXME: SizeX and SizeY appear to always be equal -- is this method sufficient?
     }
 
+    SizeComponent sizeComponent = createComponent(SizeComponent.class);
+    sizeComponent.size = monstats2.SizeX; // FIXME: see above note
+
     Entity entity = createEntity(monstats.Id);
     entity.add(typeComponent);
     entity.add(cofComponent);
@@ -319,6 +323,7 @@ public class Engine extends PooledEngine {
     entity.add(monsterComponent);
     entity.add(labelComponent);
     if (interactableComponent != null) entity.add(interactableComponent);
+    entity.add(sizeComponent);
 
     labelComponent.actor.setUserObject(entity);
 
@@ -445,6 +450,9 @@ public class Engine extends PooledEngine {
     mapComponent.map = map;
     mapComponent.zone = zone;
 
+    SizeComponent sizeComponent = createComponent(SizeComponent.class);
+    sizeComponent.size = SizeComponent.MEDIUM;
+
     Entity entity = createEntity("player");
     entity.add(typeComponent);
     entity.add(cofComponent);
@@ -457,6 +465,7 @@ public class Engine extends PooledEngine {
     entity.add(mapComponent);
     entity.add(playerComponent);
     entity.add(zoneAwareComponent);
+    entity.add(sizeComponent);
 
     return entity;
   }

core/src/com/riiablo/engine/component/SizeComponent.java

@@ -0,0 +1,18 @@
+package com.riiablo.engine.component;
+
+import com.badlogic.ashley.core.Component;
+import com.badlogic.gdx.utils.Pool;
+
+public class SizeComponent implements Component, Pool.Poolable {
+  public static final int INSIGNIFICANT = 0;
+  public static final int SMALL         = 1;
+  public static final int MEDIUM        = 2;
+  public static final int LARGE         = 3;
+
+  public int size = INSIGNIFICANT;
+
+  @Override
+  public void reset() {
+    size = INSIGNIFICANT;
+  }
+}

core/src/com/riiablo/map/Map.java

@@ -3,7 +3,7 @@ package com.riiablo.map;
 import com.badlogic.gdx.Gdx;
 import com.badlogic.gdx.ai.pfa.GraphPath;
 import com.badlogic.gdx.ai.pfa.SmoothableGraphPath;
-import com.badlogic.gdx.ai.pfa.indexed.IndexedAStarPathFinder;
+import com.riiablo.map.pfa.IndexedAStarPathFinder;
 import com.badlogic.gdx.ai.utils.Collision;
 import com.badlogic.gdx.ai.utils.Ray;
 import com.badlogic.gdx.assets.AssetDescriptor;
@@ -679,7 +679,11 @@ public class Map implements Disposable {
   private IndexedAStarPathFinder<MapGraph.Point2> pathFinder = new IndexedAStarPathFinder<>(mapGraph, true);
 
   public boolean findPath(Vector2 src, Vector2 dst, GraphPath<MapGraph.Point2> path) {
-    return mapGraph.searchNodePath(pathFinder, src, dst, path);
+    return findPath(0, src, dst, path);
+  }
+
+  public boolean findPath(int size, Vector2 src, Vector2 dst, GraphPath<MapGraph.Point2> path) {
+    return mapGraph.searchNodePath(pathFinder.setSize(size), src, dst, path);
   }
 
   public void smoothPath(SmoothableGraphPath<MapGraph.Point2, Vector2> path) {

core/src/com/riiablo/map/MapGraph.java

@@ -43,6 +43,53 @@ public class MapGraph implements IndexedGraph<MapGraph.Point2> {
   final Path tmpPath = new Path();
   final ObjectSet<Path> pathIdent = new ObjectSet<>();
 
+  final ObjectIntMap<Point2> clearance = new ObjectIntMap<>();
+  private static final int[][][] NEAR = {
+      { // 1
+        { 0,  0}
+      },
+      { // 2
+        {-1, -1},
+        { 0, -1},
+        { 1, -1},
+
+        {-1,  0},
+      //{ 0,  0},
+        { 1,  0},
+
+        {-1,  1},
+        { 0,  1},
+        { 1,  1},
+      },
+      { // 3
+        {-1, -2},
+        { 0, -2},
+        { 1, -2},
+
+        {-2, -1},
+      //{-1, -1},
+      //{ 0, -1},
+      //{ 1, -1},
+        { 2, -1},
+
+        {-2,  0},
+      //{-1,  0},
+      //{ 0,  0},
+      //{ 1,  0},
+        { 2,  0},
+
+        {-2,  1},
+      //{-1,  1},
+      //{ 0,  1},
+      //{ 1,  1},
+        { 2,  1},
+
+        {-1,  2},
+        { 0,  2},
+        { 1,  2},
+      }
+  };
+
   public MapGraph(Map map) {
     this.map = map;
     rayCaster = new MapRaycastCollisionDetector(this);
@@ -55,6 +102,7 @@ public class MapGraph implements IndexedGraph<MapGraph.Point2> {
       existing = new Point2(src);
       identity.add(existing);
       indexes.put(existing, index++);
+      calculateClearance(existing);
     }
 
     return existing;
@@ -132,6 +180,24 @@ public class MapGraph implements IndexedGraph<MapGraph.Point2> {
     connections.add(existing);
   }
 
+  private void calculateClearance(Point2 src) {
+    int i;
+size:
+    for (i = 0; i < NEAR.length; i++) {
+      for (int[] p : NEAR[i]) {
+        if (map.flags(src.x + p[0], src.y + p[1]) != 0) {
+          break size;
+        }
+      }
+    }
+
+    clearance.put(src, i);
+  }
+
+  public int getClearance(Point2 src) {
+    return clearance.get(src, 0);
+  }
+
   public static class Point2 {
     public int x;
     public int y;

core/src/com/riiablo/map/pfa/IndexedAStarPathFinder.java

@@ -0,0 +1,220 @@
+package com.riiablo.map.pfa;
+
+import com.badlogic.gdx.ai.pfa.Connection;
+import com.badlogic.gdx.ai.pfa.GraphPath;
+import com.badlogic.gdx.ai.pfa.Heuristic;
+import com.badlogic.gdx.ai.pfa.PathFinder;
+import com.badlogic.gdx.ai.pfa.PathFinderQueue;
+import com.badlogic.gdx.ai.pfa.PathFinderRequest;
+import com.badlogic.gdx.ai.pfa.indexed.IndexedGraph;
+import com.badlogic.gdx.utils.Array;
+import com.badlogic.gdx.utils.BinaryHeap;
+import com.badlogic.gdx.utils.TimeUtils;
+import com.riiablo.map.MapGraph;
+
+//refactor of com.badlogic.gdx.ai.pfa.indexed.IndexedAStarPathFinder
+public class IndexedAStarPathFinder<N> implements PathFinder<N> {
+  IndexedGraph<N> graph;
+  MapGraph casted;
+  NodeRecord<N>[] nodeRecords;
+  BinaryHeap<NodeRecord<N>> openList;
+  NodeRecord<N> current;
+  public Metrics metrics;
+
+  private int searchId;
+
+  private static final int UNVISITED = 0;
+  private static final int OPEN = 1;
+  private static final int CLOSED = 2;
+
+  private int size;
+
+  public IndexedAStarPathFinder<N> setSize(int size) {
+    this.size = size;
+    return this;
+  }
+
+  public IndexedAStarPathFinder(MapGraph graph) {
+    this(graph, false);
+  }
+
+  @SuppressWarnings("unchecked")
+  public IndexedAStarPathFinder (MapGraph graph, boolean calculateMetrics) {
+    this.graph = (IndexedGraph<N>) graph;
+    this.casted = graph;
+    this.nodeRecords = (NodeRecord<N>[])new NodeRecord[graph.getNodeCount()];
+    this.openList = new BinaryHeap<>();
+    if (calculateMetrics) this.metrics = new Metrics();
+  }
+
+  @Override
+  public boolean searchConnectionPath(N startNode, N endNode, Heuristic<N> heuristic, GraphPath<Connection<N>> outPath) {
+    boolean found = search(startNode, endNode, heuristic);
+    if (found) generateConnectionPath(startNode, outPath);
+    return found;
+  }
+
+  @Override
+  public boolean searchNodePath(N startNode, N endNode, Heuristic<N> heuristic, GraphPath<N> outPath) {
+    boolean found = search(startNode, endNode, heuristic);
+    if (found) generateNodePath(startNode, outPath);
+    return found;
+  }
+
+
+  protected boolean search (N startNode, N endNode, Heuristic<N> heuristic) {
+    initSearch(startNode, endNode, heuristic);
+    do {
+      current = openList.pop();
+      current.category = CLOSED;
+      if (current.node == endNode) return true;
+      visitChildren(endNode, heuristic);
+    } while (openList.size > 0);
+    return false;
+  }
+
+
+  @Override
+  public boolean search (PathFinderRequest<N> request, long timeToRun) {
+    long lastTime = TimeUtils.nanoTime();
+    if (request.statusChanged) {
+      initSearch(request.startNode, request.endNode, request.heuristic);
+      request.statusChanged = false;
+    }
+
+    do {
+      long currentTime = TimeUtils.nanoTime();
+      timeToRun -= currentTime - lastTime;
+      if (timeToRun <= PathFinderQueue.TIME_TOLERANCE) return false;
+      current = openList.pop();
+      current.category = CLOSED;
+      if (current.node == request.endNode) {
+        request.pathFound = true;
+        generateNodePath(request.startNode, request.resultPath);
+        return true;
+      }
+
+      visitChildren(request.endNode, request.heuristic);
+      lastTime = currentTime;
+    } while (openList.size > 0);
+    request.pathFound = false;
+    return true;
+  }
+
+  protected void initSearch (N startNode, N endNode, Heuristic<N> heuristic) {
+    if (metrics != null) metrics.reset();
+    if (++searchId < 0) searchId = 1;
+    openList.clear();
+
+    NodeRecord<N> startRecord = getNodeRecord(startNode);
+    startRecord.node = startNode;
+    startRecord.connection = null;
+    startRecord.costSoFar = 0;
+    addToOpenList(startRecord, heuristic.estimate(startNode, endNode));
+
+    current = null;
+  }
+
+  protected void visitChildren (N endNode, Heuristic<N> heuristic) {
+    Array<Connection<N>> connections = graph.getConnections(current.node);
+
+    for (int i = 0; i < connections.size; i++) {
+      if (metrics != null) metrics.visitedNodes++;
+
+      Connection<N> connection = connections.get(i);
+
+      N node = connection.getToNode();
+      float nodeCost = current.costSoFar + connection.getCost();
+
+      float nodeHeuristic;
+      NodeRecord<N> nodeRecord = getNodeRecord(node);
+      if (nodeRecord.category == CLOSED) {
+        if (nodeRecord.costSoFar <= nodeCost) continue;
+        nodeHeuristic = nodeRecord.getEstimatedTotalCost() - nodeRecord.costSoFar;
+      } else if (nodeRecord.category == OPEN) {
+        if (nodeRecord.costSoFar <= nodeCost) continue;
+        openList.remove(nodeRecord);
+        nodeHeuristic = nodeRecord.getEstimatedTotalCost() - nodeRecord.costSoFar;
+      } else if (casted.getClearance((MapGraph.Point2) node) < size) {
+        continue;
+      } else {
+        nodeHeuristic = heuristic.estimate(node, endNode);
+      }
+
+      nodeRecord.costSoFar = nodeCost;
+      nodeRecord.connection = connection;
+
+      addToOpenList(nodeRecord, nodeCost + nodeHeuristic);
+    }
+  }
+
+  protected void generateConnectionPath (N startNode, GraphPath<Connection<N>> outPath) {
+    while (current.node != startNode) {
+      outPath.add(current.connection);
+      current = nodeRecords[graph.getIndex(current.connection.getFromNode())];
+    }
+    outPath.reverse();
+  }
+
+  protected void generateNodePath (N startNode, GraphPath<N> outPath) {
+    while (current.connection != null) {
+      outPath.add(current.node);
+      current = nodeRecords[graph.getIndex(current.connection.getFromNode())];
+    }
+    outPath.add(startNode);
+    outPath.reverse();
+  }
+
+  protected void addToOpenList (NodeRecord<N> nodeRecord, float estimatedTotalCost) {
+    openList.add(nodeRecord, estimatedTotalCost);
+    nodeRecord.category = OPEN;
+    if (metrics != null) {
+      metrics.openListAdditions++;
+      metrics.openListPeak = Math.max(metrics.openListPeak, openList.size);
+    }
+  }
+
+  protected NodeRecord<N> getNodeRecord (N node) {
+    int index = graph.getIndex(node);
+    NodeRecord<N> nr = nodeRecords[index];
+    if (nr != null) {
+      if (nr.searchId != searchId) {
+        nr.category = UNVISITED;
+        nr.searchId = searchId;
+      }
+      return nr;
+    }
+    nr = nodeRecords[index] = new NodeRecord<N>();
+    nr.node = node;
+    nr.searchId = searchId;
+    return nr;
+  }
+
+  static class NodeRecord<N> extends BinaryHeap.Node {
+    N node;
+    Connection<N> connection;
+    float costSoFar;
+    int category;
+    int searchId;
+
+    public NodeRecord () {
+      super(0);
+    }
+
+    public float getEstimatedTotalCost () {
+      return getValue();
+    }
+  }
+
+  public static class Metrics {
+    public int visitedNodes;
+    public int openListAdditions;
+    public int openListPeak;
+
+    public void reset () {
+      visitedNodes = 0;
+      openListAdditions = 0;
+      openListPeak = 0;
+    }
+  }
+}