AstralTransRocketries/TrueCraft
Archived
1
0
Fork 0
Code Issues 48 Pull Requests 1 Packages Projects Releases Wiki Activity

Refactor A* implementation leading up to JPS

Browse Source
This commit is contained in:
Drew DeVault 2015-07-03 10:40:00 -06:00
parent ebadeed59d
commit ff0ee58b37
2 changed files with 53 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
TrueCraft.Core.Test/AI/PathFindingTest.cs
View File

@ -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]);

53
TrueCraft.Core/AI/AStarPathFinder.cs
View File

@ -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
for (int i = 0; i < Neighbors.Length; i++)
foreach (var next in GetNeighbors(world, subject, current))
{
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])
{