added some pathfinding tests, as well as some minor code improvements

Demos/PathfindingDemo.cs

@@ -36,14 +36,14 @@ namespace Demos {
 
             // 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. 
+            // both have a cost of AStar2.InfiniteCost, 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
             float Cost(Point pos, Point nextPos) {
                 if (nextPos.X < 0 || nextPos.Y < 0 || nextPos.X >= 50 || nextPos.Y >= 50)
-                    return float.MaxValue;
+                    return AStar2.InfiniteCost;
-                return this.world[nextPos.X, nextPos.Y] ? 1 : float.MaxValue;
+                return this.world[nextPos.X, nextPos.Y] ? 1 : AStar2.InfiniteCost;
             }
 
             // Actually initialize the pathfinder with the cost function, as well as specify if moving diagonally between tiles should be

MLEM/Pathfinding/AStar.cs

@@ -82,7 +82,7 @@ namespace MLEM.Pathfinding {
         /// <param name="defaultCost">The default cost for each path point</param>
         /// <param name="maxTries">The maximum amount of tries before path finding is aborted</param>
         /// <param name="allowDiagonals">If diagonals should be looked at for path finding</param>
-        /// <returns>A stack of path points, where the top item is the first point to go to.</returns>
+        /// <returns>A stack of path points, where the top item is the first point to go to, or null if no path was found.</returns>
         public Stack<T> FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) {
             var stopwatch = Stopwatch.StartNew();
 
@@ -118,7 +118,7 @@ namespace MLEM.Pathfinding {
                 foreach (var dir in dirsUsed) {
                     var neighborPos = this.AddPositions(current.Pos, dir);
                     var cost = getCost(current.Pos, neighborPos);
-                    if (!float.IsInfinity(cost) && cost < float.MaxValue && !closed.ContainsKey(neighborPos)) {
+                    if (!float.IsPositiveInfinity(cost) && cost < float.MaxValue && !closed.ContainsKey(neighborPos)) {
                         var neighbor = new PathPoint<T>(neighborPos, this.GetManhattanDistance(neighborPos, goal), current, cost, defCost);
                         // check if we already have a waypoint at this location with a worse path
                         if (open.TryGetValue(neighborPos, out var alreadyNeighbor)) {

Tests/NumberTests.cs

@@ -43,7 +43,7 @@ namespace Tests {
         }
 
         [Test]
-        public void TestMatrixOps([Range(0.5F, 2, 1)] float scale, [Range(-0.5F, 0.5F, 1)] float rotationX, [Range(-0.5F, 0.5F, 1)] float rotationY, [Range(-0.5F, 0.5F, 0.5F)] float rotationZ) {
+        public void TestMatrixOps([Range(0.5F, 2, 0.5F)] float scale, [Range(-0.5F, 0.5F, 1)] float rotationX, [Range(-0.5F, 0.5F, 1)] float rotationY, [Range(-0.5F, 0.5F, 1)] float rotationZ) {
             var rotation = Matrix.CreateRotationX(rotationX) * Matrix.CreateRotationY(rotationY) * Matrix.CreateRotationZ(rotationZ);
             var matrix = rotation * Matrix.CreateScale(scale, scale, scale);
             Assert.IsTrue(matrix.Scale().Equals(new Vector3(scale), 0.001F), $"{matrix.Scale()} does not equal {new Vector2(scale)}");

Tests/PathfindingTests.cs

@@ -0,0 +1,74 @@
+using System.Collections.Generic;
+using System.Linq;
+using Microsoft.Xna.Framework;
+using MLEM.Pathfinding;
+using NUnit.Framework;
+
+namespace Tests {
+    public class PathfindingTests {
+
+        [Test]
+        public void TestConsistency() {
+            var area = new[] {
+                "XXXX",
+                "X  X",
+                "X  X",
+                "XXXX"
+            };
+
+            var noDiagonals = FindPathInArea(new Point(1, 1), new Point(2, 2), area, false).ToArray();
+            Assert.AreEqual(noDiagonals.Length, 3);
+            Assert.AreEqual(noDiagonals[0], new Point(1, 1));
+            Assert.AreEqual(noDiagonals[2], new Point(2, 2));
+
+            var diagonals = FindPathInArea(new Point(1, 1), new Point(2, 2), area, true).ToArray();
+            Assert.AreEqual(diagonals.Length, 2);
+            Assert.AreEqual(diagonals[0], new Point(1, 1));
+            Assert.AreEqual(diagonals[1], new Point(2, 2));
+        }
+
+        [Test]
+        public void TestPathCost() {
+            var area = new[] {
+                "XXXXXXXX",
+                "X 5   X",
+                "X   5 X",
+                "XXXXXXXX"
+            };
+
+            var firstPath = FindPathInArea(new Point(1, 1), new Point(3, 1), area, false).ToArray();
+            var firstExpected = new[] {new Point(1, 1), new Point(1, 2), new Point(2, 2), new Point(3, 2), new Point(3, 1)};
+            Assert.AreEqual(firstPath, firstExpected);
+
+            var secondPath = FindPathInArea(new Point(1, 1), new Point(5, 2), area, false).ToArray();
+            var secondExpected = firstExpected.Concat(new[] {new Point(4, 1), new Point(5, 1), new Point(5, 2)}).ToArray();
+            Assert.AreEqual(secondPath, secondExpected);
+        }
+
+        [Test]
+        public void TestBlocked() {
+            var area = new[] {
+                "XXXX",
+                "X XX",
+                "XX X",
+                "X  X",
+                "XXXX"
+            };
+            // non-diagonal pathfinding should get stuck in the corner
+            Assert.IsNull(FindPathInArea(new Point(1, 1), new Point(2, 3), area, false));
+            // diagonal pathfinding should be able to cross the diagonal gap
+            Assert.IsNotNull(FindPathInArea(new Point(1, 1), new Point(2, 3), area, true));
+        }
+
+        private static Stack<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals) {
+            var costs = area.Select(s => s.Select(c => c switch {
+                ' ' => 1,
+                'X' => AStar2.InfiniteCost,
+                _ => (float) char.GetNumericValue(c)
+            }).ToArray()).ToArray();
+            var pathFinder = new AStar2((p1, p2) => costs[p2.Y][p2.X], allowDiagonals);
+            return pathFinder.FindPath(start, end);
+        }
+
+    }
+}