public bool PathFound(HexKeyValuePair <int, DirectedPath> item)
{
var path = item.Value;
var hex = path.PathStep.Hex;
if (_closed.Contains(hex.Coords))
{
return(false);
}
_open.Add(item.Value.PathStep.Hex.Coords, item.Value);
TraceFlags.FindPathDequeue.Trace(
"Dequeue Path at {0} w/ cost={1,4} at {2}; estimate={3,4}:{4,4}.",
item.Value.PathStep.Hex.Coords, item.Value.TotalCost, item.Value.HexsideExit,
item.Key >> 16, item.Key & 0xFFFF);
if (hex.Coords.Equals(_start.Coords))
{
Path = path;
return(true);
}
_closed.Add(hex.Coords);
FastHexsideList.ForEach((Action <Hexside>)(hexside => Invoke(path, hexside)));
return(false);
}
public PathQueueFunctor(IHex start, IHex goal, Func <int, int> heuristic,
Func <IHex, Hexside, int> stepCost,
IPriorityQueue <int, DirectedPath> queue
)
{
_vectorGoal = goal.Coords.Canon - start.Coords.Canon;
TraceFlags.FindPathDetail.Trace(true, "Find path from {0} to {1}; vectorGoal = {2}",
start.Coords, goal.Coords, _vectorGoal);
Path = null;
_start = start;
_heuristic = heuristic;
_open = (IDictionary <HexCoords, IDirectedPath>) new Dictionary <HexCoords, IDirectedPath>();
_stepCost = stepCost;
_queue = queue;
}
public void Invoke(DirectedPath path, Hexside hexside)
{
var here = path.PathStep.Hex;
var there = here.Board[here.Coords.GetNeighbour(hexside)];
if (there != null && !_open.ContainsKey(there.Coords))
{
var cost = _stepCost(there, hexside.Reversed());
if (cost > 0)
{
var newPath = path.AddStep(there, hexside, cost);
var estimate = Estimate(_heuristic, _vectorGoal, _start.Coords, newPath.PathStep.Hex.Coords, newPath.TotalCost);
TraceFlags.FindPathEnqueue.Trace(" Enqueue {0}: estimate={1,4}:{2,3}",
there.Coords, estimate >> 16, estimate & 0xFFFF);
_queue.Enqueue(estimate, newPath);
}
}
}
BidirectionalPathfinder(IHex start, IHex goal,
LandmarkCollection landmarks, HashSet<HexCoords> closed, Func<double> getBestSoFar
)
{
_start = start;
_goal = goal;
_getBestSoFar = getBestSoFar;
_vectorGoal = goal.Coords.Canon - start.Coords.Canon;
_open = new Dictionary<HexCoords, DirectedPath>();
_closed = closed;
_queue = new HotPriorityQueue<DirectedPath>(16);
_landmark = landmarks
.OrderByDescending(l => l.HexDistance(goal.Coords) - l.HexDistance(start.Coords))
.FirstOrDefault();
_heuristic = c => _landmark.HexDistance(c) - _landmark.HexDistance(start.Coords);
var path = new DirectedPath(goal);
_open.Add(goal.Coords, path);
_queue.Enqueue(0, path);
}
public void SetBestSoFar(DirectedPath pathFwd, DirectedPath pathRev)
{
if (pathFwd == null || pathRev == null) return;
if (pathFwd.TotalCost + pathRev.TotalCost < BestSoFar)
{
_pathFwd = pathFwd;
_pathRev = pathRev;
BestSoFar = _pathFwd.TotalCost + pathRev.TotalCost;
KeySoFar = _pathFwd.Key + _pathRev.Key;
}
}
public static DirectedPath MergePaths(DirectedPath fwd, DirectedPath rev)
{
if (rev != null)
{
while (rev.PathSoFar != null)
{
var hexside = rev.PathStep.HexsideEntry;
var cost = rev.TotalCost - (rev = rev.PathSoFar).TotalCost;
fwd = fwd.AddStep(rev.PathStep.Hex, hexside, cost, (int)cost);
}
}
return fwd;
}
void ExpandHex(DirectedPath path, Hexside hexside)
{
var here = path.PathStep.Hex;
var there = here.Neighbour(hexside);
if (there != null && !_closed.Contains(there.Coords))
{
var cost = _stepCost(here, hexside, there);
if ((cost > 0)
)
{
if ((path.TotalCost + cost < _getBestSoFar() || !_open.ContainsKey(there.Coords))
)
{
var totalCost = cost + path.TotalCost;
var key = Estimate(here, there, totalCost);
var newPath = _addStep(path, there, hexside, cost);
DirectedPath oldPath;
if (!_open.TryGetValue(there.Coords, out oldPath))
{
_open.Add(there.Coords, newPath);
_queue.Enqueue(key, newPath);
}
else if (newPath.TotalCost < oldPath.TotalCost)
{
_open.Remove(there.Coords);
_open.Add(there.Coords, newPath);
_queue.Enqueue(key, newPath);
}
_setBestSoFar(newPath, Partner.GetPartnerPath(there.Coords));
}
}
}
}
