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); } }