Refactor A* implementation leading up to JPS

TrueCraft.Core.Test/AI/PathFindingTest.cs

@@ -6,6 +6,7 @@ using TrueCraft.Core.AI;
 using TrueCraft.API.World;
 using TrueCraft.Core.World;
 using System.Linq;
+using System.Diagnostics;
 
 namespace TrueCraft.Core.Test.AI
 {
@@ -39,9 +40,15 @@ namespace TrueCraft.Core.Test.AI
         {
             var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
             var astar = new AStarPathFinder();
+
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(),
                new Coordinates3D(0, 4, 0), new Coordinates3D(5, 4, 0));
+            watch.Stop();
             DrawGrid(path, world);
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
+
             var expected = new[]
             {
                 new Coordinates3D(0, 4, 0),
@@ -62,8 +69,14 @@ namespace TrueCraft.Core.Test.AI
             var astar = new AStarPathFinder();
             var start = new Coordinates3D(0, 4, 0);
             var end = new Coordinates3D(5, 4, 5);
+
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(), start, end);
+            watch.Stop();
             DrawGrid(path, world);
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
+
             // Just test the start and end, the exact results need to be eyeballed
             Assert.AreEqual(start, path.Waypoints[0]);
             Assert.AreEqual(end, path.Waypoints[path.Waypoints.Count - 1]);
@@ -77,8 +90,14 @@ namespace TrueCraft.Core.Test.AI
             var start = new Coordinates3D(0, 4, 0);
             var end = new Coordinates3D(5, 4, 0);
             world.SetBlockID(new Coordinates3D(3, 4, 0), 1); // Obstacle
+
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(), start, end);
+            watch.Stop();
             DrawGrid(path, world);
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
+
             // Just test the start and end, the exact results need to be eyeballed
             Assert.AreEqual(start, path.Waypoints[0]);
             Assert.AreEqual(end, path.Waypoints[path.Waypoints.Count - 1]);
@@ -98,7 +117,12 @@ namespace TrueCraft.Core.Test.AI
             world.SetBlockID(start + Coordinates3D.North, 1);
             world.SetBlockID(start + Coordinates3D.South, 1);
 
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(), start, end);
+            watch.Stop();
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
+
             Assert.IsNull(path);
         }
 
@@ -120,8 +144,12 @@ namespace TrueCraft.Core.Test.AI
             for (int x = -4; x < 4; x++)
                 world.SetBlockID(new Coordinates3D(x, 4, 4), 1);
 
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(), start, end);
+            watch.Stop();
             DrawGrid(path, world);
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
 
             // Just test the start and end, the exact results need to be eyeballed
             Assert.AreEqual(start, path.Waypoints[0]);
@@ -146,8 +174,12 @@ namespace TrueCraft.Core.Test.AI
             for (int x = -4; x < 4; x++)
                 world.SetBlockID(new Coordinates3D(x, 4, 4), 1);
 
+            var watch = new Stopwatch();
+            watch.Start();
             var path = astar.FindPath(world, new BoundingBox(), start, end);
+            watch.Stop();
             DrawGrid(path, world);
+            Console.WriteLine(watch.ElapsedMilliseconds + "ms");
 
             // Just test the start and end, the exact results need to be eyeballed
             Assert.AreEqual(start, path.Waypoints[0]);

TrueCraft.Core/AI/AStarPathFinder.cs

@@ -43,6 +43,26 @@ namespace TrueCraft.Core.AI
             return id == 0;
         }
 
+        private IEnumerable<Coordinates3D> GetNeighbors(IWorld world, BoundingBox subject, Coordinates3D current)
+        {
+            for (int i = 0; i < Neighbors.Length; i++)
+            {
+                var next = Neighbors[i] + current;
+                if (CanOccupyVoxel(world, subject, next))
+                    yield return next;
+            }
+            for (int i = 0; i < DiagonalNeighbors.Length; i++)
+            {
+                var pair = DiagonalNeighbors[i];
+                var next = pair[0] + pair[1] + current;
+
+                if (CanOccupyVoxel(world, subject, next)
+                    && CanOccupyVoxel(world, subject, pair[0] + current)
+                    && CanOccupyVoxel(world, subject, pair[1] + current))
+                    yield return next;
+            }
+        }
+
         public PathResult FindPath(IWorld world, BoundingBox subject, Coordinates3D start, Coordinates3D goal)
         {
             // Thanks to www.redblobgames.com/pathfinding/a-star/implementation.html
@@ -64,41 +84,10 @@ namespace TrueCraft.Core.AI
 
                 closedset.Add(current);
 
-                // Test directly adjacent voxels
+                foreach (var next in GetNeighbors(world, subject, current))
-                for (int i = 0; i < Neighbors.Length; i++)
                 {
-                    var next = Neighbors[i] + current;
                     if (closedset.Contains(next))
                         continue;
-
-                    if (!CanOccupyVoxel(world, subject, next))
-                        continue;
-
-                    var cost = (int)(costs[current] + current.DistanceTo(next));
-                    if (!costs.ContainsKey(next) || cost < costs[next])
-                    {
-                        costs[next] = cost;
-                        var priority = cost + next.DistanceTo(goal);
-                        openset.Enqueue(next, priority);
-                        parents[next] = current;
-                    }
-                }
-
-                // Test diagonally
-                for (int i = 0; i < DiagonalNeighbors.Length; i++)
-                {
-                    var pair = DiagonalNeighbors[i];
-                    var next = pair[0] + pair[1] + current;
-                    if (closedset.Contains(next))
-                        continue;
-
-                    if (!CanOccupyVoxel(world, subject, next))
-                        continue;
-                    if (!CanOccupyVoxel(world, subject, pair[0] + current))
-                        continue;
-                    if (!CanOccupyVoxel(world, subject, pair[1] + current))
-                        continue;
-
                     var cost = (int)(costs[current] + current.DistanceTo(next));
                     if (!costs.ContainsKey(next) || cost < costs[next])
                     {