paths contain their cost, & get BEST path method

src/main/java/xyz/valnet/hadean/pathfinding/AStarPathfinder.java

@@ -5,6 +5,8 @@ import java.util.Comparator;
 import java.util.List;
 import java.util.Stack;
 
+import xyz.valnet.engine.math.Vector2i;
+
 public class AStarPathfinder implements IPathfinder {
 
   private IPathable pathable;
@@ -105,7 +107,7 @@ public class AStarPathfinder implements IPathfinder {
 
         path.pop();
 
-        return new Path(path);
+        return new Path(path, current.getCost());
       }
 
       Node[] neighbors = getNeighbors(current, open, closed, x2, y2);
@@ -140,4 +142,18 @@ public class AStarPathfinder implements IPathfinder {
 
     return null;
   }
+
+  @Override
+  public Path getBestPath(Vector2i src, Vector2i[] dsts) {
+    int cost = Integer.MAX_VALUE;
+    Path bestPath = null;
+    for(Vector2i dst : dsts) {
+      Path path = getPath(src.x, src.y, dst.x, dst.y);
+      if(path.cost < cost) {
+        cost = path.cost;
+        bestPath = path;
+      }
+    }
+    return bestPath;
+  }
 }

src/main/java/xyz/valnet/hadean/pathfinding/IPathfinder.java

@@ -1,5 +1,8 @@
 package xyz.valnet.hadean.pathfinding;
 
+import xyz.valnet.engine.math.Vector2i;
+
 public interface IPathfinder {
   public Path getPath(int x1, int y1, int x2, int y2);
+  public Path getBestPath(Vector2i src, Vector2i[] dsts);
 }

src/main/java/xyz/valnet/hadean/pathfinding/Path.java

@@ -5,9 +5,11 @@ import java.util.Stack;
 
 public class Path implements Iterable<Node> {
   private Stack<Node> nodes;
+  public final int cost;
 
-  public Path(Stack<Node> nodes) {
+  public Path(Stack<Node> nodes, int cost) {
     this.nodes = nodes;
+    this.cost = cost;
   }
 
   public Node peek() {