public TargetPath(Node start, double distSoFar, List <PartitionPass> partitions, SortedSet <TargetPath> queue, DirectionsSettings settings)
{
Id = _currentId;
_currentId++;
CurrentNode = start;
DistSoFar = distSoFar;
PartitionsLeft = partitions;
BestDist = PartitionsLeft.Sum(pp => pp.BestDist) + distSoFar;
if (partitions.Any())
{
BestDist += settings.WeightedDist(start, partitions.First().Target);
}
PathQueue = queue;
}
private IEnumerable <ThreadInfo> GetShortestPath(ThreadManager TM, int to)
{
var currentThread = TM.CurrentThread;
// See if the start node is already in the goal location
yield return(TM.MakeDbCall(currentThread, Program.ConnectionString, "spGetLocationNodes", new SqlParameter("@location", to)));
using (var table = TM.GetResult <DataTable>(currentThread)) {
if (table.Rows.Count == 0)
{
// Error - no nodes in goal location
yield return(TM.Return(currentThread));
}
foreach (DataRow row in table.Rows)
{
if (new Node(row).Id == _start.Id)
{
// Already at goal
yield return(TM.Return(currentThread));
}
}
}
// Get the nodes on the boundary of the goal location
var goalNodes = new Dictionary <int, SortedSet <Node> >();
yield return(TM.MakeDbCall(currentThread, Program.ConnectionString, "spGetLocationBoundaryNodes", new SqlParameter("@location", to)));
using (var table = TM.GetResult <DataTable>(currentThread)) {
if (table.Rows.Count == 0)
{
// Error - no nodes on goal location's boundary
yield return(TM.Return(currentThread));
}
foreach (DataRow row in table.Rows)
{
var node = new Node(row);
if (!goalNodes.ContainsKey(node.Partition.Value))
{
goalNodes[node.Partition.Value] = new SortedSet <Node>(new NodeComparer());
}
goalNodes[node.Partition.Value].Add(node);
}
}
// Get the possible partition paths
Stack <PartitionPass> partitions = new Stack <PartitionPass>();
partitions.Push(new PartitionPass(-1, _start, 0));
yield return(TM.Await(currentThread, GetPartitionPaths(TM, partitions, goalNodes)));
var passes = TM.GetResult <List <List <PartitionPass> > >(currentThread);
SortedSet <TargetPath> queue = new SortedSet <TargetPath>(new TargetPathComparer());
Dictionary <int, ShortestPath> shortestPaths = new Dictionary <int, ShortestPath>();
shortestPaths[_start.Id] = new ShortestPath();
foreach (var pass in passes)
{
queue.Add(new TargetPath(_start, 0, pass, queue, _settings));
}
while (true)
{
var currentPath = queue.First();
_pathFindingDone = Math.Max(_pathFindingDone, currentPath.DistSoFar * currentPath.DistSoFar / (currentPath.BestDist * currentPath.BestDist));
if (goalNodes.Any(kvp => kvp.Value.Contains(currentPath.CurrentNode)))
{
var currentNode = currentPath.CurrentNode;
var prevNode = shortestPaths[currentNode.Id].PrevNode;
_paths.Add(new DirectionPath(currentNode.Lat, currentNode.Lon, null, true, null, currentNode.Altitude, currentNode.Location, currentNode.Outside, shortestPaths[currentNode.Id].Id, shortestPaths[currentNode.Id].Forward));
while (currentNode.Id != _start.Id)
{
_paths.Insert(0, new DirectionPath(prevNode.Lat, prevNode.Lon, null, false, null, prevNode.Altitude, prevNode.Location, prevNode.Outside, shortestPaths[currentNode.Id].Id, shortestPaths[currentNode.Id].Forward));
currentNode = prevNode;
prevNode = shortestPaths[currentNode.Id].PrevNode;
}
_paths.First().Flag = true;
yield return(TM.Return(currentThread));
}
yield return(TM.MakeDbCall(currentThread, Program.ConnectionString, "spGetPathsFromNode",
new SqlParameter("@node", currentPath.CurrentNode.Id),
new SqlParameter("@part", currentPath.PartitionsLeft.First().Partition)));
using (var table = TM.GetResult <DataTable>(currentThread)) {
foreach (DataRow row in table.Rows)
{
var path = new PathToNode(row, currentPath.CurrentNode.Id, currentPath.PartitionsLeft.First().Partition);
double pathDist = _settings.WeightedDist(currentPath.CurrentNode, path.Node);
if (shortestPaths.ContainsKey(path.Node.Id))
{
if (shortestPaths[path.Node.Id].TotalDist > currentPath.DistSoFar + pathDist)
{
shortestPaths[path.Node.Id].SetPrevNode(currentPath.CurrentNode, pathDist, path.Path.Id, path.Forward, shortestPaths);
}
else if (shortestPaths[path.Node.Id].TotalDist < currentPath.DistSoFar + pathDist)
{
continue;
}
// TODO
}
else
{
shortestPaths[path.Node.Id] = new ShortestPath(currentPath.CurrentNode, pathDist, path.Path.Id, path.Forward, shortestPaths);
// TODO
}
if (path.Node.Id == currentPath.PartitionsLeft.First().Target.Id)
{
queue.Add(new TargetPath(path.Node, shortestPaths[path.Node.Id].TotalDist, currentPath.PartitionsLeft.Skip(1).ToList(), queue, _settings));
}
else
{
queue.Add(new TargetPath(path.Node, shortestPaths[path.Node.Id].TotalDist, currentPath.PartitionsLeft, queue, _settings));
}
}
}
queue.Remove(currentPath);
}
}