overhauled pathfinding and added a demo for it

Demos/PathfindingDemo.cs

@@ -0,0 +1,97 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Graphics;
+using MLEM.Extensions;
+using MLEM.Input;
+using MLEM.Pathfinding;
+using MLEM.Startup;
+using MonoGame.Extended;
+
+namespace Demos {
+    public class PathfindingDemo : MlemGame {
+
+        private bool[,] world;
+        private AStar2 pathfinder;
+        private List<Point> path;
+
+        private void Init() {
+            // generate a simple random world for testing, where true is walkable area, and false is a wall
+            var random = new Random();
+            this.world = new bool[50, 50];
+            for (var x = 0; x < 50; x++) {
+                for (var y = 0; y < 50; y++) {
+                    if (random.NextDouble() >= 0.25)
+                        this.world[x, y] = true;
+                }
+            }
+
+            // Create a cost function, which determines how expensive (or difficult) it should be to move from a given position
+            // to the next, adjacent position. In our case, the only restriction should be walls and out-of-bounds positions, which
+            // both have a cost of float.MaxValue, meaning they are completely unwalkable. 
+            // If your game contains harder-to-move-on areas like, say, a muddy pit, you can return a higher cost value for those
+            // locations. If you want to scale your cost function differently, you can specify a different default cost in your 
+            // pathfinder's constructor
+            AStar<Point>.GetCost cost = (pos, nextPos) => {
+                if (nextPos.X < 0 || nextPos.Y < 0 || nextPos.X >= 50 || nextPos.Y >= 50)
+                    return float.MaxValue;
+                return this.world[nextPos.X, nextPos.Y] ? 1 : float.MaxValue;
+            };
+            // Actually initialize the pathfinder with the cost function, as well as specify if moving diagonally between tiles should be
+            // allowed or not (in this case it's not)
+            this.pathfinder = new AStar2(cost, false);
+
+            // Now find a path from the top left to the bottom right corner and store it in a variable
+            // If no path can be found after the maximum amount of tries (10000 by default), the pathfinder will abort and return no path (null)
+            var foundPath = this.pathfinder.FindPath(Point.Zero, new Point(49, 49));
+            this.path = foundPath != null ? foundPath.ToList() : null;
+
+            // print out some info
+            Console.WriteLine("Pathfinding took " + this.pathfinder.LastTriesNeeded + " tries");
+            if (this.path == null)
+                Console.WriteLine("Couldn't find a path, press the left mouse button to try again");
+        }
+
+        protected override void LoadContent() {
+            base.LoadContent();
+            this.Init();
+        }
+
+        protected override void Update(GameTime gameTime) {
+            base.Update(gameTime);
+
+            // when pressing the left mouse button, generate a new world and find a new path
+            if (Input.IsMouseButtonPressed(MouseButton.Left)) {
+                this.Init();
+            }
+        }
+
+        protected override void DoDraw(GameTime gameTime) {
+            this.GraphicsDevice.Clear(Color.White);
+
+            this.SpriteBatch.Begin(SpriteSortMode.Deferred, null, SamplerState.PointClamp, transformMatrix: Matrix.CreateScale(14));
+            // draw the world with simple shapes
+            for (var x = 0; x < 50; x++) {
+                for (var y = 0; y < 50; y++) {
+                    if (!this.world[x, y]) {
+                        this.SpriteBatch.FillRectangle(new Vector2(x, y), new Size2(1, 1), Color.Black);
+                    }
+                }
+            }
+            // draw the path
+            // in a real game, you'd obviously make your characters walk along the path instead of drawing it
+            if (this.path != null) {
+                for (var i = 1; i < this.path.Count; i++) {
+                    var first = this.path[i - 1];
+                    var second = this.path[i];
+                    this.SpriteBatch.DrawLine(new Vector2(first.X + 0.5F, first.Y + 0.5F), new Vector2(second.X + 0.5F, second.Y + 0.5F), Color.Blue, 0.25F);
+                }
+            }
+            this.SpriteBatch.End();
+
+            base.DoDraw(gameTime);
+        }
+
+    }
+}

Demos/Program.cs

@@ -12,6 +12,7 @@ namespace Demos {
             Demos.Add("Ui", () => new UiDemo());
             Demos.Add("AutoTiling", () => new AutoTilingDemo());
             Demos.Add("Animation", () => new AnimationDemo());
+            Demos.Add("Pathfinding", () => new PathfindingDemo());
         }
 
         public static void Main(string[] args) {

MLEM/Pathfinding/AStar.cs

@@ -1,34 +1,39 @@
-using System;
 using System.Collections.Generic;
-using System.Linq;
-using Microsoft.Xna.Framework;
 
 namespace MLEM.Pathfinding {
-    public static class AStar {
+    public abstract class AStar<T> {
 
-        private static readonly Point[] AdjacentDirections = {
+        public readonly T[] AllDirections;
-            new Point(1, 0),
+        public readonly T[] AdjacentDirections;
-            new Point(-1, 0),
+        public GetCost DefaultCostFunction;
-            new Point(0, 1),
+        public float DefaultCost;
-            new Point(0, -1)
+        public int DefaultMaxTries;
-        };
+        public bool DefaultAllowDiagonals;
+        public int LastTriesNeeded { get; private set; }
 
-        private static readonly Point[] AllDirections = AdjacentDirections.Concat(new[] {
+        protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) {
-            new Point(1, 1),
+            this.AllDirections = allDirections;
-            new Point(-1, 1),
+            this.AdjacentDirections = adjacentDirections;
-            new Point(1, -1),
+            this.DefaultCostFunction = defaultCostFunction;
-            new Point(-1, -1)
+            this.DefaultCost = defaultCost;
-        }).ToArray();
+            this.DefaultMaxTries = defaultMaxTries;
+            this.DefaultAllowDiagonals = defaultAllowDiagonals;
+        }
 
-        public static Stack<Point> FindPath(Point start, Point goal, int defaultCost, GetCost getCost, int maxTries = 10000, bool allowDiagonals = false) {
+        public Stack<T> FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) {
-            var open = new List<PathPoint>();
+            var getCost = costFunction ?? this.DefaultCostFunction;
-            var closed = new List<PathPoint>();
+            var diags = allowDiagonals ?? this.DefaultAllowDiagonals;
-            open.Add(new PathPoint(start, goal, null, 0, defaultCost));
+            var tries = maxTries ?? this.DefaultMaxTries;
+            var defCost = defaultCost ?? this.DefaultCost;
+
+            var open = new List<PathPoint<T>>();
+            var closed = new List<PathPoint<T>>();
+            open.Add(new PathPoint<T>(start, this.GetManhattanDistance(start, goal), null, 0, defCost));
 
             var count = 0;
             while (open.Count > 0) {
-                PathPoint current = null;
+                PathPoint<T> current = null;
-                var lowestF = int.MaxValue;
+                var lowestF = float.MaxValue;
                 foreach (var point in open)
                     if (point.F < lowestF) {
                         current = point;
@@ -40,15 +45,17 @@ namespace MLEM.Pathfinding {
                 open.Remove(current);
                 closed.Add(current);
 
-                if (current.Pos.Equals(goal))
+                if (current.Pos.Equals(goal)) {
+                    this.LastTriesNeeded = count;
                     return CompilePath(current);
+                }
 
-                var dirsUsed = allowDiagonals ? AllDirections : AdjacentDirections;
+                var dirsUsed = diags ? this.AllDirections : this.AdjacentDirections;
                 foreach (var dir in dirsUsed) {
-                    var neighborPos = current.Pos + dir;
+                    var neighborPos = this.AddPositions(current.Pos, dir);
-                    var cost = getCost(neighborPos);
+                    var cost = getCost(current.Pos, neighborPos);
-                    if (cost < int.MaxValue) {
+                    if (cost < float.MaxValue) {
-                        var neighbor = new PathPoint(neighborPos, goal, current, cost, defaultCost);
+                        var neighbor = new PathPoint<T>(neighborPos, this.GetManhattanDistance(neighborPos, goal), current, cost, defCost);
                         if (!closed.Contains(neighbor)) {
                             var alreadyIndex = open.IndexOf(neighbor);
                             if (alreadyIndex < 0) {
@@ -65,14 +72,19 @@ namespace MLEM.Pathfinding {
                 }
 
                 count++;
-                if (count >= maxTries)
+                if (count >= tries)
                     break;
             }
+            this.LastTriesNeeded = count;
             return null;
         }
 
-        private static Stack<Point> CompilePath(PathPoint current) {
+        protected abstract T AddPositions(T first, T second);
-            var path = new Stack<Point>();
+
+        protected abstract float GetManhattanDistance(T first, T second);
+
+        private static Stack<T> CompilePath(PathPoint<T> current) {
+            var path = new Stack<T>();
             while (current != null) {
                 path.Push(current.Pos);
                 current = current.Parent;
@@ -80,30 +92,29 @@ namespace MLEM.Pathfinding {
             return path;
         }
 
-        public delegate int GetCost(Point pos);
+        public delegate float GetCost(T currPos, T nextPos);
 
     }
 
-    public class PathPoint {
+    public class PathPoint<T> {
 
-        public readonly PathPoint Parent;
+        public readonly PathPoint<T> Parent;
-        public readonly Point Pos;
+        public readonly T Pos;
-        public readonly int F;
+        public readonly float F;
-        public readonly int G;
+        public readonly float G;
 
-        public PathPoint(Point pos, Point goal, PathPoint parent, int terrainCostForThisPos, int defaultCost) {
+        public PathPoint(T pos, float distance, PathPoint<T> parent, float terrainCostForThisPos, float defaultCost) {
             this.Pos = pos;
             this.Parent = parent;
 
             this.G = (parent == null ? 0 : parent.G) + terrainCostForThisPos;
-            var manhattan = (Math.Abs(goal.X - pos.X) + Math.Abs(goal.Y - pos.Y)) * defaultCost;
+            this.F = this.G + distance * defaultCost;
-            this.F = this.G + manhattan;
         }
 
         public override bool Equals(object obj) {
             if (obj == this)
                 return true;
-            return obj is PathPoint point && point.Pos.Equals(this.Pos);
+            return obj is PathPoint<T> point && point.Pos.Equals(this.Pos);
         }
 
         public override int GetHashCode() {

MLEM/Pathfinding/AStar2.cs

@@ -0,0 +1,35 @@
+using System;
+using System.Linq;
+using Microsoft.Xna.Framework;
+
+namespace MLEM.Pathfinding {
+    public class AStar2 : AStar<Point> {
+
+        private static readonly Point[] AdjacentDirs = {
+            new Point(1, 0),
+            new Point(-1, 0),
+            new Point(0, 1),
+            new Point(0, -1)
+        };
+
+        private static readonly Point[] AllDirs = AdjacentDirs.Concat(new[] {
+            new Point(1, 1),
+            new Point(-1, 1),
+            new Point(1, -1),
+            new Point(-1, -1)
+        }).ToArray();
+
+        public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
+            base(AllDirs, AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {
+        }
+
+        protected override Point AddPositions(Point first, Point second) {
+            return first + second;
+        }
+
+        protected override float GetManhattanDistance(Point first, Point second) {
+            return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y);
+        }
+
+    }
+}

MLEM/Pathfinding/AStar3.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using Microsoft.Xna.Framework;
+
+namespace MLEM.Pathfinding {
+    public class AStar3 : AStar<Vector3> {
+
+        private static readonly Vector3[] AdjacentDirs = {
+            new Vector3(1, 0, 0),
+            new Vector3(-1, 0, 0),
+            new Vector3(0, 1, 0),
+            new Vector3(0, -1, 0),
+            new Vector3(0, 0, 1),
+            new Vector3(0, 0, -1)
+        };
+
+        private static readonly Vector3[] AllDirs;
+
+        static AStar3() {
+            var dirs = new List<Vector3>();
+            for (var x = -1; x <= 1; x++) {
+                for (var y = -1; y <= 1; y++) {
+                    for (var z = -1; z <= 1; z++) {
+                        if (x == 0 && y == 0 && z == 0)
+                            continue;
+                        dirs.Add(new Vector3(x, y, z));
+                    }
+                }
+            }
+            AllDirs = dirs.ToArray();
+        }
+
+        public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
+            base(AllDirs, AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {
+        }
+
+        protected override Vector3 AddPositions(Vector3 first, Vector3 second) {
+            return first + second;
+        }
+
+        protected override float GetManhattanDistance(Vector3 first, Vector3 second) {
+            return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y) + Math.Abs(second.Z - first.Z);
+        }
+
+    }
+}