From f2ab1c05987b47d3b902d16e90e67b432a2735b4 Mon Sep 17 00:00:00 2001 From: Drew DeVault Date: Thu, 2 Jul 2015 17:05:44 -0600 Subject: [PATCH] Implement A* pathfinding and relevant tests --- TrueCraft.API/PathResult.cs | 10 ++ TrueCraft.API/TrueCraft.API.csproj | 1 + TrueCraft.Core.Test/AI/PathFindingTest.cs | 105 ++++++++++++++++++ .../TrueCraft.Core.Test.csproj | 4 +- TrueCraft.Core.Test/packages.config | 2 +- TrueCraft.Core/AI/AStarPathFinder.cs | 68 ++++++++++++ TrueCraft.Core/AI/PriorityQueue.cs | 37 ++++++ TrueCraft.Core/TrueCraft.Core.csproj | 3 + 8 files changed, 228 insertions(+), 2 deletions(-) create mode 100644 TrueCraft.API/PathResult.cs create mode 100644 TrueCraft.Core.Test/AI/PathFindingTest.cs create mode 100644 TrueCraft.Core/AI/AStarPathFinder.cs create mode 100644 TrueCraft.Core/AI/PriorityQueue.cs diff --git a/TrueCraft.API/PathResult.cs b/TrueCraft.API/PathResult.cs new file mode 100644 index 0000000..23d7da8 --- /dev/null +++ b/TrueCraft.API/PathResult.cs @@ -0,0 +1,10 @@ +using System; +using System.Collections.Generic; + +namespace TrueCraft.API +{ + public class PathResult + { + public IList Waypoints; + } +} \ No newline at end of file diff --git a/TrueCraft.API/TrueCraft.API.csproj b/TrueCraft.API/TrueCraft.API.csproj index 5459bf8..f7dd7cb 100644 --- a/TrueCraft.API/TrueCraft.API.csproj +++ b/TrueCraft.API/TrueCraft.API.csproj @@ -109,6 +109,7 @@ + diff --git a/TrueCraft.Core.Test/AI/PathFindingTest.cs b/TrueCraft.Core.Test/AI/PathFindingTest.cs new file mode 100644 index 0000000..3f95674 --- /dev/null +++ b/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); + } + } +} \ No newline at end of file diff --git a/TrueCraft.Core.Test/TrueCraft.Core.Test.csproj b/TrueCraft.Core.Test/TrueCraft.Core.Test.csproj index 207f2ef..492ea99 100644 --- a/TrueCraft.Core.Test/TrueCraft.Core.Test.csproj +++ b/TrueCraft.Core.Test/TrueCraft.Core.Test.csproj @@ -35,7 +35,7 @@ ..\packages\NUnit.2.6.4\lib\nunit.framework.dll - ..\packages\Moq.4.2.1502.0911\lib\net40\Moq.dll + ..\packages\Moq.4.2.1507.0118\lib\net40\Moq.dll @@ -83,6 +83,7 @@ + @@ -90,5 +91,6 @@ + \ No newline at end of file diff --git a/TrueCraft.Core.Test/packages.config b/TrueCraft.Core.Test/packages.config index e3829fc..290692e 100644 --- a/TrueCraft.Core.Test/packages.config +++ b/TrueCraft.Core.Test/packages.config @@ -1,5 +1,5 @@  - + \ No newline at end of file diff --git a/TrueCraft.Core/AI/AStarPathFinder.cs b/TrueCraft.Core/AI/AStarPathFinder.cs new file mode 100644 index 0000000..9bf6c15 --- /dev/null +++ b/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 parents) + { + var list = new List(); + 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(); + var costs = new Dictionary(); + var frontier = new PriorityQueue(); + + 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; + } + } +} \ No newline at end of file diff --git a/TrueCraft.Core/AI/PriorityQueue.cs b/TrueCraft.Core/AI/PriorityQueue.cs new file mode 100644 index 0000000..fff85ad --- /dev/null +++ b/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 + { + private List> elements = new List>(); + + 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; + } + } +} \ No newline at end of file diff --git a/TrueCraft.Core/TrueCraft.Core.csproj b/TrueCraft.Core/TrueCraft.Core.csproj index 0dc194a..7494e45 100644 --- a/TrueCraft.Core/TrueCraft.Core.csproj +++ b/TrueCraft.Core/TrueCraft.Core.csproj @@ -328,6 +328,8 @@ + + @@ -352,6 +354,7 @@ +