Implement A* pathfinding and relevant tests

TrueCraft.API/PathResult.cs

@@ -0,0 +1,10 @@
+using System;
+using System.Collections.Generic;
+
+namespace TrueCraft.API
+{
+    public class PathResult
+    {
+        public IList<Coordinates3D> Waypoints;
+    }
+}

TrueCraft.API/TrueCraft.API.csproj

@@ -109,6 +109,7 @@
     <Compile Include="ToolType.cs" />
     <Compile Include="World\ChunkLoadedEventArgs.cs" />
     <Compile Include="IEventSubject.cs" />
+    <Compile Include="PathResult.cs" />
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup />

TrueCraft.Core.Test/AI/PathFindingTest.cs

@@ -0,0 +1,105 @@
+using System;
+using NUnit.Framework;
+using TrueCraft.Core.TerrainGen;
+using TrueCraft.API;
+using TrueCraft.Core.AI;
+using TrueCraft.API.World;
+using TrueCraft.Core.World;
+using System.Linq;
+
+namespace TrueCraft.Core.Test.AI
+{
+    [TestFixture]
+    public class PathFindingTest
+    {
+        private void DrawGrid(PathResult path, IWorld world)
+        {
+            for (int z = -8; z < 8; z++)
+            {
+                for (int x = -8; x < 8; x++)
+                {
+                    var coords = new Coordinates3D(x, 4, z);
+                    if (path.Waypoints.Contains(coords))
+                        Console.Write("o");
+                    else
+                    {
+                        var id = world.GetBlockID(coords);
+                        if (id != 0)
+                            Console.Write("x");
+                        else
+                            Console.Write("_");
+                    }
+                }
+                Console.WriteLine();
+            }
+        }
+
+        [Test]
+        public void TestAStarLinearPath()
+        {
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var astar = new AStarPathFinder();
+            var path = astar.FindPath(world, new BoundingBox(),
+               new Coordinates3D(0, 4, 0), new Coordinates3D(5, 4, 0));
+            DrawGrid(path, world);
+            var expected = new[]
+            {
+                new Coordinates3D(0, 4, 0),
+                new Coordinates3D(1, 4, 0),
+                new Coordinates3D(2, 4, 0),
+                new Coordinates3D(3, 4, 0),
+                new Coordinates3D(4, 4, 0),
+                new Coordinates3D(5, 4, 0)
+            };
+            for (int i = 0; i < path.Waypoints.Count; i++)
+                Assert.AreEqual(expected[i], path.Waypoints[i]);
+        }
+
+        [Test]
+        public void TestAStarDiagonalPath()
+        {
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var astar = new AStarPathFinder();
+            var start = new Coordinates3D(0, 4, 0);
+            var end = new Coordinates3D(5, 4, 5);
+            var path = astar.FindPath(world, new BoundingBox(), start, end);
+            DrawGrid(path, world);
+            // 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]);
+        }
+
+        [Test]
+        public void TestAStarObstacle()
+        {
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var astar = new AStarPathFinder();
+            var start = new Coordinates3D(0, 4, 0);
+            var end = new Coordinates3D(5, 4, 0);
+            world.SetBlockID(new Coordinates3D(3, 4, 0), 1); // Obstacle
+            var path = astar.FindPath(world, new BoundingBox(), start, end);
+            DrawGrid(path, world);
+            // 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]);
+            Assert.IsFalse(path.Waypoints.Contains(new Coordinates3D(3, 4, 0)));
+        }
+
+        [Test]
+        public void TestAStarImpossible()
+        {
+            var world = new TrueCraft.Core.World.World("default", new FlatlandGenerator());
+            var astar = new AStarPathFinder();
+            var start = new Coordinates3D(0, 4, 0);
+            var end = new Coordinates3D(5, 4, 0);
+
+            world.SetBlockID(start + Coordinates3D.East, 1);
+            world.SetBlockID(start + Coordinates3D.West, 1);
+            world.SetBlockID(start + Coordinates3D.North, 1);
+            world.SetBlockID(start + Coordinates3D.South, 1);
+
+            var path = astar.FindPath(world, new BoundingBox(), start, end);
+            Assert.IsNull(path);
+        }
+    }
+}

TrueCraft.Core.Test/TrueCraft.Core.Test.csproj

@@ -35,7 +35,7 @@
       <HintPath>..\packages\NUnit.2.6.4\lib\nunit.framework.dll</HintPath>
     </Reference>
     <Reference Include="Moq">
-      <HintPath>..\packages\Moq.4.2.1502.0911\lib\net40\Moq.dll</HintPath>
+      <HintPath>..\packages\Moq.4.2.1507.0118\lib\net40\Moq.dll</HintPath>
     </Reference>
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
@@ -83,6 +83,7 @@
     <Compile Include="Windows\CraftingWindowAreaTest.cs" />
     <Compile Include="Logic\BlockProviderTest.cs" />
     <Compile Include="Lighting\WorldLighterTest.cs" />
+    <Compile Include="AI\PathFindingTest.cs" />
   </ItemGroup>
   <ItemGroup>
     <Folder Include="World\" />
@@ -90,5 +91,6 @@
     <Folder Include="Windows\" />
     <Folder Include="Logic\" />
     <Folder Include="Lighting\" />
+    <Folder Include="AI\" />
   </ItemGroup>
 </Project>

TrueCraft.Core.Test/packages.config

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
 <packages>
-  <package id="Moq" version="4.2.1502.0911" targetFramework="net45" />
+  <package id="Moq" version="4.2.1507.0118" targetFramework="net45" />
   <package id="NUnit" version="2.6.4" targetFramework="net45" />
 </packages>

TrueCraft.Core/AI/AStarPathFinder.cs

@@ -0,0 +1,68 @@
+using System;
+using System.Collections.Generic;
+using TrueCraft.API;
+using TrueCraft.API.World;
+
+namespace TrueCraft.Core.AI
+{
+    public class AStarPathFinder
+    {
+        private readonly Coordinates3D[] Neighbors =
+        {
+            Coordinates3D.North,
+            Coordinates3D.East,
+            Coordinates3D.South,
+            Coordinates3D.West
+        };
+
+        private PathResult TracePath(Coordinates3D start, Coordinates3D goal, Dictionary<Coordinates3D, Coordinates3D> parents)
+        {
+            var list = new List<Coordinates3D>();
+            var current = goal;
+            while (current != start)
+            {
+                current = parents[current];
+                list.Insert(0, current);
+            }
+            list.Add(goal);
+            return new PathResult { Waypoints = list };
+        }
+
+        public PathResult FindPath(IWorld world, BoundingBox subject, Coordinates3D start, Coordinates3D goal)
+        {
+            // Thanks to www.redblobgames.com/pathfinding/a-star/implementation.html
+            
+            var parents = new Dictionary<Coordinates3D, Coordinates3D>();
+            var costs = new Dictionary<Coordinates3D, int>();
+            var frontier = new PriorityQueue<Coordinates3D>();
+
+            frontier.Enqueue(start, 0);
+            parents[start] = start;
+            costs[start] = 0;
+
+            while (frontier.Count > 0)
+            {
+                var current = frontier.Dequeue();
+                if (current == goal)
+                    return TracePath(start, goal, parents);
+                for (int i = 0; i < Neighbors.Length; i++)
+                {
+                    var next = Neighbors[i] + current;
+                    var id = world.GetBlockID(next);
+                    if (id != 0)
+                        continue; // TODO: Make this more sophisticated
+                    var cost = (int)(costs[current] + current.DistanceTo(next));
+                    if (!costs.ContainsKey(next) || cost < costs[next])
+                    {
+                        costs[next] = cost;
+                        var priority = (int)(cost + next.DistanceTo(goal));
+                        frontier.Enqueue(next, priority);
+                        parents[next] = current;
+                    }
+                }
+            }
+
+            return null;
+        }
+    }
+}

TrueCraft.Core/AI/PriorityQueue.cs

@@ -0,0 +1,37 @@
+using System;
+using System.Collections.Generic;
+
+namespace TrueCraft.Core.AI
+{
+    // TODO: Replace this with something better eventually
+    // Thanks to www.redblobgames.com/pathfinding/a-star/implementation.html
+    public class PriorityQueue<T>
+    {
+        private List<Tuple<T, int>> elements = new List<Tuple<T, int>>();
+
+        public int Count
+        {
+            get { return elements.Count; }
+        }
+        
+        public void Enqueue(T item, int priority)
+        {
+            elements.Add(Tuple.Create(item, priority));
+        }
+
+        public T Dequeue()
+        {
+            int bestIndex = 0;
+
+            for (int i = 0; i < elements.Count; i++) {
+                if (elements[i].Item2 < elements[bestIndex].Item2) {
+                    bestIndex = i;
+                }
+            }
+
+            T bestItem = elements[bestIndex].Item1;
+            elements.RemoveAt(bestIndex);
+            return bestItem;
+        }
+    }
+}

TrueCraft.Core/TrueCraft.Core.csproj

@@ -328,6 +328,8 @@
     <Compile Include="TerrainGen\EmptyGenerator.cs" />
     <Compile Include="Logic\ItemRepository.cs" />
     <Compile Include="Lighting\WorldLighting.cs" />
+    <Compile Include="AI\AStarPathFinder.cs" />
+    <Compile Include="AI\PriorityQueue.cs" />
   </ItemGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>
@@ -352,6 +354,7 @@
   </ProjectExtensions>
   <ItemGroup>
     <Folder Include="Lighting\" />
+    <Folder Include="AI\" />
   </ItemGroup>
   <ItemGroup>
     <None Include="packages.config" />