private IDirectedPath GetPath()
{
VectorGoal = Target.Coords.Canon - Source.Coords.Canon;
OpenSet = new HashSet <HexCoords>();
Queue = new DictionaryPriorityQueue <int, IDirectedPath>();
TraceFindPathDetailInit(Source.Coords, Target.Coords);
Queue.Enqueue(0, new DirectedPath(Target));
HexKeyValuePair <int, IDirectedPath> item;
while (Queue.TryDequeue(out item))
{
var path = item.Value;
var step = path.PathStep.Hex;
OpenSet.Add(step.Coords);
if (ClosedSet.Contains(step.Coords))
{
continue;
}
TraceFindPathDequeue("Rev", step.Coords, path.TotalCost, path.HexsideExit, item.Key >> 16,
(int)((int)(item.Key & 0xFFFFu) - 0x7FFF));
if (step.Equals(Source))
{
return(path);
}
ClosedSet.Add(step.Coords);
foreach (var neighbour in Board.GetNeighbourHexes(step))
{
ExpandNeighbour(path, neighbour);
}
}
return(null);
}
internal void FillLandmark(IBoard <IHex> board)
{
if (board != null)
{
backingStore = new BoardStorage <short> .BlockedBoardStorage32x32(board.MapSizeHexes, c => - 1);
var start = board[Coords];
var queue = DictionaryPriorityQueue <int, IHex> .NewQueue();
TraceFlags.FindPathDetail.Trace(true, "Find distances from {0}", start.Coords);
queue.Enqueue(0, start);
HexKeyValuePair <int, IHex> item;
while (queue.TryDequeue(out item))
{
var here = item.Value;
var key = item.Key;
if (backingStore[here.Coords] > 0)
{
continue;
}
TraceFlags.FindPathDetail.Trace("Dequeue Path at {0} w/ cost={1,4}.", here, key);
backingStore[here.Coords] = (short)key;
foreach (var there in here.GetAllNeighbours().Where(n => n != null && n.Hex.IsOnboard()))
{
var cost = board.DirectedStepCost(here, there.HexsideEntry);
if (cost > 0 && backingStore[there.Hex.Coords] == -1)
{
TraceFlags.FindPathDetail.Trace(" Enqueue {0}: {1,4}", there.Hex.Coords, cost);
queue.Enqueue(key + cost, there.Hex);
}
}
}
}
}
/// <summary>Returns an <c>IPath</c> for the optimal path from coordinates <c>start</c> to <c>goal</c>.</summary>
/// <param name="start">Coordinates for the <c>last</c> step on the desired path.</param>
/// <param name="goal">Coordinates for the <c>first</c> step on the desired path.</param>
/// <param name="stepCost">Cost to extend path by hex at <c>coords</c> from hex at direction <c>hexside</c>.</param>
/// <param name="heuristic">Returns a monotonic (ie locally admissible) cost estimate from a range value.</param>
/// <returns></returns>
/// ///<remarks>Note that <c>heuristic</c> <b>must</b> be monotonic in order for the algorithm to perform properly.</remarks>
#pragma warning disable 1658, 1584
/// <seealso cref="http://www.cs.trincoll.edu/~ram/cpsc352/notes/astar.html"/>
#pragma warning restore 1658, 1584
public static IDirectedPath FindDirectedPathFwd(
IHex start,
IHex goal,
Func <IHex, Hexside, int> stepCost,
Func <int, int> heuristic
)
{
if (start == null)
{
throw new ArgumentNullException("start");
}
if (goal == null)
{
throw new ArgumentNullException("goal");
}
if (stepCost == null)
{
throw new ArgumentNullException("stepCost");
}
var vectorGoal = goal.Coords.Canon - start.Coords.Canon;
var closed = new HashSet <HexCoords>();
var open = new HashSet <HexCoords>();
var queue = new DictionaryPriorityQueue <int, DirectedPath>();
TraceFlags.FindPathDetail.Trace(true, "Find path from {0} to {1}; vectorGoal = {2}",
start.Coords, goal.Coords, vectorGoal);
queue.Enqueue(0, new DirectedPath(goal));
HexKeyValuePair <int, DirectedPath> item;
while (queue.TryDequeue(out item))
{
var path = item.Value;
open.Add(path.PathStep.Hex.Coords);
if (closed.Contains(path.PathStep.Hex.Coords))
{
continue;
}
TraceFlags.FindPathDequeue.Trace(
"Dequeue Path at {0} w/ cost={1,4} at {2}; estimate={3,4}:{4,4}.",
path.PathStep.Hex.Coords, path.TotalCost, path.HexsideExit, item.Key >> 16,
(int)((int)(item.Key & 0xFFFFu) - 0x7FFF));
if (path.PathStep.Hex.Equals(start))
{
TraceFlags.FindPathDequeue.Trace("Closed: {0,7}", closed.Count);
return(path);
}
closed.Add(path.PathStep.Hex.Coords);
foreach (var neighbour in path.PathStep.Hex.GetNeighbourHexes()
)
{
if (!open.Contains(neighbour.Hex.Coords))
{
var cost = stepCost(neighbour.Hex, neighbour.HexsideExit);
if (cost > 0)
{
var newPath = path.AddStep(neighbour, cost);
var key = Estimate(heuristic, vectorGoal, start.Coords, neighbour.Hex.Coords,
newPath.TotalCost);
TraceFlags.FindPathEnqueue.Trace(" Enqueue {0}: estimate={1,4}:{2,3}",
neighbour.Hex.Coords, key >> 16, key & 0xFFFFu);
queue.Enqueue(key, newPath);
}
}
}
}
return(null);
}
/// <summary>Returns an <c>IPath</c> for the optimal path from coordinates <c>start</c> to <c>goal</c>.</summary>
/// <param name="start">Coordinates for the <c>last</c> step on the desired path.</param>
/// <param name="goal">Coordinates for the <c>first</c> step on the desired path.</param>
/// <param name="stepCost">Cost to extend path by hex at <c>coords</c> from hex at direction <c>hexside</c>.</param>
/// <param name="heuristic">Returns a cost estimate from a range value.</param>
/// <param name="isOnboard">Returns whether the coordinates specified are "on board".</param>
public static IPath FindPath(
HexCoords start,
HexCoords goal,
Func <HexCoords, Hexside, int> stepCost,
Func <int, int> heuristic,
Func <HexCoords, bool> isOnboard
)
{
if (stepCost == null)
{
throw new ArgumentNullException("stepCost");
}
if (heuristic == null)
{
throw new ArgumentNullException("heuristic");
}
if (isOnboard == null)
{
throw new ArgumentNullException("isOnboard");
}
var vectorGoal = goal.Canon - start.Canon;
var closed = new HashSet <HexCoords>();
var open = new HashSet <HexCoords>();
var queue = DictionaryPriorityQueue <int, Path> .NewQueue();
TraceFlags.FindPathDetail.Trace(true, "Find path from {0} to {1}; vectorGoal = {0}",
start.Canon, goal.Canon, vectorGoal);
queue.Enqueue(0, new Path(start));
HexKeyValuePair <int, Path> item;
while (queue.TryDequeue(out item))
{
var path = item.Value;
if (closed.Contains(path.StepCoords))
{
continue;
}
open.Add(item.Value.StepCoords);
TraceFlags.FindPathDetail.Trace(
"Dequeue Path at {0} w/ cost={1,4} at {2}; estimate={3,4}:{4,4}.",
item.Value.StepCoords, item.Value.TotalCost, item.Value.HexsideExit,
item.Key >> 16, item.Key & 0xFFFF);
if (path.StepCoords.Equals(goal))
{
return(path);
}
closed.Add(path.StepCoords);
foreach (var neighbour in path.StepCoords.GetNeighbours()
.Where(n => isOnboard(n.Coords))
)
{
var cost = stepCost(path.StepCoords, neighbour.Hexside);
if (cost > 0)
{
var newPath = path.AddStep(neighbour, (ushort)cost);
var estimate = Estimate(heuristic, vectorGoal, goal, newPath.StepCoords, newPath.TotalCost);
TraceFlags.FindPathDetail.Trace(" Enqueue {0}: {1,4}:{2,3}",
neighbour.Coords, estimate >> 16, estimate & 0xFFFF);
queue.Enqueue(estimate, newPath);
}
}
}
return(null);
}