private void ExpandHex(IDirectedPath path, Hexside hexside)
{
var here = path.PathStep.Coords;
var there = here.GetNeighbour(hexside);
if (!ClosedSet.Contains(there))
{
TryStepCost(here, hexside).IfHasValueDo(cost => {
if (path.TotalCost + cost < BestSoFar || !OpenSet.ContainsKey(there))
{
Heuristic(there).IfHasValueDo(heuristic => {
var key = path.TotalCost + cost + heuristic;
var newPath = path.AddStep(there, HexsideDirection(hexside), cost);
PathfinderExtensions.TraceFindPathEnqueue(there, key, 0);
if (!OpenSet.TryGetValue(there, out var oldPath))
{
OpenSet.Add(there, newPath);
Queue.Enqueue(key, newPath);
}
else if (newPath.TotalCost < oldPath.TotalCost)
{
OpenSet.Remove(there);
OpenSet.Add(there, newPath);
Queue.Enqueue(key, newPath);
}
SetBestSoFar(newPath, PartnerPath(there));
});
}
});
}
}
public bool IsFinished()
{
if (Queue.TryDequeue(out var item))
{
var path = item.Value;
var coords = path.PathStep.Coords;
OpenSet.Remove(coords);
if (!ClosedSet.Contains(coords))
{
PathfinderExtensions.TraceFindPathDequeue(GetType().Name, coords, path, item.Key, 0);
if (item.Key < BestSoFar)
{
Partner.Heuristic(coords).IfHasValueDo(heuristic => {
if (path.TotalCost + Partner.FrontierMinimum() - heuristic < BestSoFar)
{
Hexside.ForEach(hexside => ExpandHex(path, hexside));
}
});
}
ClosedSet.Add(coords);
}
return(!Queue.Any());
}
return(true);
}
/// <inheritdoc/>
public void SetBestSoFar(IDirectedPath pathRev, IDirectedPath pathFwd)
{
if (pathFwd.TotalCost + pathRev.TotalCost < BestSoFar)
{
_pathRev = pathRev;
_pathFwd = pathFwd;
BestSoFar = _pathRev.TotalCost + _pathFwd.TotalCost;
PathfinderExtensions.TraceFindPathDetailBestSoFar(pathFwd, pathRev, BestSoFar);
}
}
/// <param name="pathHalves"></param>
/// <param name="isForward"></param>
public AltPathfinder(IPathHalves pathHalves, bool isForward)
{
IsForward = isForward;
ClosedSet = pathHalves.ClosedSet;
StartCoords = pathHalves.Start;
GoalCoords = pathHalves.Goal;
Landmarks = pathHalves.Board.Landmarks;
OpenSet = new Dictionary <HexCoords, IDirectedPath>();
Queue = HotPriorityQueue.New <IDirectedPath>(0, 256);
TryStepCost = IsForward ? (StepCost)pathHalves.Board.TryEntryCost : pathHalves.Board.TryExitCost;
PathHalves = pathHalves;
PathfinderExtensions.TraceFindPathDetailDirection(Direction, GoalCoords - StartCoords);
StartPath(IsForward ? StartCoords : GoalCoords);
}
/// <inheritdoc/>
public Maybe <IDirectedPath> GetPath(AltPathfinder pathfinderFwd, AltPathfinder pathfinderRev)
{
pathfinderFwd.Partner = pathfinderRev;
pathfinderRev.Partner = pathfinderFwd;
var pathfinder = pathfinderFwd;
// Until done: alternate searching from each direction and calling the other direction
while (!pathfinder.IsFinished())
{
pathfinder = pathfinder.Partner;
}
PathfinderExtensions.TraceFindPathDone(ClosedSet.Count);
return(_pathFwd.MergePaths(_pathRev));
}
