protected void StartPath(IHex start)
{
var path = new DirectedPath(start);
OpenSet.Add(path.PathStep.Hex.Coords, path);
Queue.Enqueue(0, path);
}
/// <summary>TODO</summary>
internal DirectedPath(DirectedPath nextSteps, NeighbourHex neighbour, int totalCost, int key)
{
PathStep = neighbour;
PathSoFar = nextSteps;
TotalCost = totalCost;
Key = key;
TotalSteps = nextSteps == null ? 0 : nextSteps.TotalSteps + 1;
}
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, cost);
}
}
return(fwd);
}
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;
TraceFlags.FindPathDetail.Trace(" SetBestSoFar: pathFwd at {0}; pathRev at {1}; Cost = {2}",
pathFwd.PathStep.Hex.Coords, _pathRev.PathStep.Hex.Coords, BestSoFar);
}
}
BidirectionalPathfinder(IHex start, IHex goal,
LandmarkCollection landmarks, HashSet <HexCoords> closed, Func <int> 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);
}
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);
TraceFlags.FindPathEnqueue.Trace(" Enqueue {0}: estimate={1,4}:{2,4}",
there.Coords, key >> 16, key & 0xFFFF);
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));
}
}
}
}