private void ReadTaxiNode(string line) { string[] tokens = line.Split(_splitters, StringSplitOptions.RemoveEmptyEntries); uint id = uint.Parse(tokens[4]); _nodeDict[id] = new TaxiNode(id, tokens[1], tokens[2]) { Name = string.Join(" ", tokens.Skip(5)) }; }
public Parking(Airport airport) : base() { _airport = airport; AlternateAfterPushBack = null; PushBackLatitude = 0; PushBackLongitude = 0; MaxSize = XPlaneAircraftCategory.F; Operation = OperationType.Airline; Operators = Enumerable.Empty <string>(); }
public TaxiNode(uint id, string latitude, string longitude) : base() { Id = id; IncomingEdges = new List <TaxiEdge>(); DistanceToTarget = double.MaxValue; NextNodeToTarget = null; OverrideToTarget = null; LatitudeString = latitude; LongitudeString = longitude; }
public TaxiEdge(TaxiNode startNode, TaxiNode endNode, bool isRunway, XPlaneAircraftCategory maxSize, string linkName) { StartNode = startNode; EndNode = endNode; IsRunway = isRunway; MaxCategory = maxSize; LinkName = linkName; ActiveForRunways = new List <string>(); ActiveZone = false; ReverseEdge = null; }
public Runway(string designator, double latitude, double longitude, double displacement) : base(latitude, longitude) { NearestNode = null; DisplacedNode = null; EntryGroups = new Dictionary <TaxiNode, List <EntryPoint> >(); ExitGroups = new Dictionary <TaxiNode, List <ExitPoint> >(); Designator = designator; Displacement = displacement; DisplacedLatitude = 0; DisplacedLongitude = 0; OppositeEnd = null; }
/// <summary> /// Find the chain of TaxiNodes that represent this runway /// </summary> /// <param name="taxiNodes"></param> /// <param name="taxiEdges"></param> /// <returns></returns> private List <TaxiNode> FindNodeChain(IEnumerable <TaxiNode> taxiNodes, IEnumerable <TaxiEdge> taxiEdges) { List <TaxiNode> nodes = new List <TaxiNode>(); // Start with the node nearest to the runway lat/lon TaxiNode currentNode = NearestNode; nodes.Add(currentNode); ulong previousNodeId = 0; do { // Now find an edge that is marked as 'runway' and that starts at the current node, but does not lead to the previous node IEnumerable <TaxiEdge> edgesToNext = taxiEdges.Where(e => e.IsRunway && (e.StartNode.Id == currentNode.Id && e.EndNode.Id != previousNodeId)); if (edgesToNext.Count() == 0) { break; } TaxiEdge edgeToNext = edgesToNext.First(); if (edgesToNext.Count() > 1) { double maxDeviation = double.MaxValue; foreach (TaxiEdge candidate in edgesToNext) { double deviation = VortexMath.TurnAngle(this.Bearing, VortexMath.BearingRadians(currentNode, candidate.EndNode)); if (deviation < maxDeviation) { edgeToNext = candidate; maxDeviation = deviation; } } } // Keep the current Id as the previous Id previousNodeId = currentNode.Id; // And get the new current node currentNode = taxiNodes.Single(n => n.Id == edgeToNext.EndNode.Id); if (currentNode != null) { nodes.Add(currentNode); } }while (currentNode != null); return(nodes); }
private void FindExits() { ExitGroups.Clear(); TaxiNode groupStartNode = null; // First, group all nodes foreach (TaxiNode node in RunwayNodes) { foreach (TaxiEdge edge in node.IncomingEdges) { if (edge.IsRunway) { continue; } if (edge.ReverseEdge == null) { continue; } TaxiEdge actualEdge = edge.ReverseEdge; double exitAngle = VortexMath.TurnAngle(actualEdge.Bearing, Bearing); if (Math.Abs(exitAngle) > VortexMath.Deg135Rad) { continue; } if (groupStartNode == null) { groupStartNode = node; ExitGroups.Add(node, new List <ExitPoint>()); } double landingLengthUsed = VortexMath.DistanceKM(DisplacedNode, node); // 'distance used' actually if (VortexMath.DistanceKM(groupStartNode, node) < 0.200) { ExitGroups[groupStartNode].Add(new ExitPoint() { OffRunwayNode = actualEdge.EndNode, OnRunwayNode = node, LandingLengthUsed = landingLengthUsed, TurnAngle = exitAngle }); } else { // add to new group groupStartNode = node; ExitGroups.Add(node, new List <ExitPoint>()); ExitGroups[groupStartNode].Add(new ExitPoint() { OffRunwayNode = actualEdge.EndNode, OnRunwayNode = node, LandingLengthUsed = landingLengthUsed, TurnAngle = exitAngle }); } } } if (ExitGroups.Count == 0) { return; // todo: add warning } // Then pick groups based upon distance List <ExitPoint> minimumExit = null; List <ExitPoint> mediumExit = null; List <ExitPoint> longExit = null; List <ExitPoint> maxExit = null; foreach (KeyValuePair <TaxiNode, List <ExitPoint> > exitGroup in ExitGroups.OrderBy(eg => eg.Value.First().LandingLengthUsed)) { if (minimumExit == null || minimumExit.First().LandingLengthUsed < VortexMath.Feet4000Km) { minimumExit = exitGroup.Value; } else if (mediumExit == null || mediumExit.First().LandingLengthUsed < VortexMath.Feet5000Km) { mediumExit = exitGroup.Value; } else if (longExit == null || longExit.First().LandingLengthUsed < VortexMath.Feet6500Km) { longExit = exitGroup.Value; } else { maxExit = exitGroup.Value; } } ExitGroups.Clear(); if (minimumExit != null) { ExitGroups.Add(minimumExit.First().OnRunwayNode, minimumExit); } if (mediumExit != null) { ExitGroups.Add(mediumExit.First().OnRunwayNode, mediumExit); } if (longExit != null) { ExitGroups.Add(longExit.First().OnRunwayNode, longExit); } if (maxExit != null) { ExitGroups.Add(maxExit.First().OnRunwayNode, maxExit); } foreach (var result in ExitGroups) { Logger.Log($"{Designator} Group: {result.Key.Id}"); ExitPoint right = result.Value.Where(ep => ep.TurnAngle > 0).OrderBy(ep => ep.TurnAngle).FirstOrDefault(); ExitPoint left = result.Value.Where(ep => ep.TurnAngle < 0).OrderByDescending(ep => ep.TurnAngle).FirstOrDefault(); ExitGroups[result.Key].Clear(); if (right != null) { Logger.Log($" Right Exit: {right.OnRunwayNode.Id}->{right.OffRunwayNode.Id} {right.TurnAngle * VortexMath.Rad2Deg:0.0} {right.LandingLengthUsed * VortexMath.KmToFoot:0}ft"); ExitGroups[result.Key].Add(right); } if (left != null) { Logger.Log($" Left Exit: {left.OnRunwayNode.Id}->{left.OffRunwayNode.Id} {left.TurnAngle * VortexMath.Rad2Deg:0.0} {left.LandingLengthUsed * VortexMath.KmToFoot:0}ft"); ExitGroups[result.Key].Add(left); } } }
private void FindEntries() { EntryGroups.Clear(); TaxiNode groupStartNode = null; bool foundGroups = false; // Nodes are ordered, longest remaining runway to runway end foreach (TaxiNode node in RunwayNodes) { double takeoffLengthRemaining = VortexMath.DistanceKM(node, OppositeEnd); if (takeoffLengthRemaining < VortexMath.Feet4000Km) { break; } foreach (TaxiEdge edge in node.IncomingEdges) { if (edge.IsRunway) { continue; } double entryAngle = VortexMath.TurnAngle(edge.Bearing, Bearing); if (Math.Abs(entryAngle) > VortexMath.Deg120Rad) { continue; } if (groupStartNode == null) { groupStartNode = node; EntryGroups.Add(node, new List <EntryPoint>()); } // Next can be simplified to 1 if (>= 0.250) / else with the actual add after the if else // for now this shows better what is going on if (VortexMath.DistanceKM(groupStartNode, node) < 0.200) { EntryGroups[groupStartNode].Add(new EntryPoint() { OffRunwayNode = edge.StartNode, OnRunwayNode = node, TakeoffLengthRemaining = takeoffLengthRemaining, TurnAngle = entryAngle }); } else if (EntryGroups.Count < 2) { // add to new group groupStartNode = node; EntryGroups.Add(node, new List <EntryPoint>()); EntryGroups[groupStartNode].Add(new EntryPoint() { OffRunwayNode = edge.StartNode, OnRunwayNode = node, TakeoffLengthRemaining = takeoffLengthRemaining, TurnAngle = entryAngle }); } else { foundGroups = true; break; } } if (foundGroups) { break; } } bool gotLeft = false; bool gotRight = false; bool useIntersections = Settings.UseIntersectionTakeOffs; double maxShiftKm = (double)Settings.MaxIntersectionShift * VortexMath.Foot2Km; // Find at least one enrty from the left and one from the right // If using intersections is allowed, add the intersections as option // as long as they are not too far from the runway start foreach (var result in EntryGroups) { Logger.Log($"{Designator} Group: {result.Key.Id}"); EntryPoint right = result.Value.Where(ep => ep.TurnAngle < 0).OrderByDescending(ep => ep.TurnAngle).FirstOrDefault(); EntryPoint left = result.Value.Where(ep => ep.TurnAngle > 0).OrderBy(ep => ep.TurnAngle).FirstOrDefault(); EntryGroups[result.Key].Clear(); if (right != null) { // No entry from the right, or using intersections is allowed if (!gotRight || useIntersections) { // No entry from the right, or current intersections is not too far from runway start if (!gotRight || (this.Length - maxShiftKm) < right.TakeoffLengthRemaining) { Logger.Log($" Right Entry: {right.OffRunwayNode.Id}->{right.OnRunwayNode.Id} {right.TurnAngle * VortexMath.Rad2Deg:0.0} {right.TakeoffLengthRemaining * VortexMath.KmToFoot:0}ft"); EntryGroups[result.Key].Add(right); gotRight = true; } } } if (left != null) { // No entry from the left, or using intersections is allowed if (!gotLeft || useIntersections) { // No entry from the left, or current intersections is not too far from runway start if (!gotLeft || (this.Length - maxShiftKm) < left.TakeoffLengthRemaining) { Logger.Log($" Left Entry: {left.OffRunwayNode.Id}->{left.OnRunwayNode.Id} {left.TurnAngle * VortexMath.Rad2Deg:0.0} {left.TakeoffLengthRemaining * VortexMath.KmToFoot:0}ft"); EntryGroups[result.Key].Add(left); gotLeft = true; } } } } }
public bool Analyze(IEnumerable <TaxiNode> taxiNodes, IEnumerable <TaxiEdge> taxiEdges) { Logger.Log($"---------------------------------------------"); Logger.Log($"Analyzing Runway {Designator}"); Logger.Log($"---------------------------------------------"); // Find the taxi nodes closest to the start of the runway and the displaced start double shortestDistance = double.MaxValue; double shortestDisplacedDistance = double.MaxValue; IEnumerable <TaxiNode> runwayNodes = taxiEdges.Where(te => te.IsRunway).Select(te => te.StartNode).Concat(taxiEdges.Where(te => te.IsRunway).Select(te => te.EndNode)).Distinct(); foreach (TaxiNode node in runwayNodes) { // Start with a few sanity cehcks to prevent runways without nodes in the apt.dat fro picking up nodes from nearby runways double angleFromStart = VortexMath.AbsTurnAngle(Bearing, VortexMath.BearingRadians(this, node)); double d = VortexMath.DistanceKM(node.Latitude, node.Longitude, DisplacedLatitude, DisplacedLongitude); // A node more than 10m from the runway start should be near the centerline to be accepted if (d > 0.010 && angleFromStart > VortexMath.Deg005Rad) { continue; } // Ignore node that are farther away from the runway coordinates than the length of the runway if (d > Length * 1.1) { continue; } // Now see if this node is better than the best so far if (d < shortestDisplacedDistance) { shortestDisplacedDistance = d; DisplacedNode = node; } d = VortexMath.DistanceKM(this, node); if (d < shortestDistance) { shortestDistance = d; NearestNode = node; } } if (NearestNode == null) { // The KOKC runway 18 clause... Logger.Log($"No suitable nodes on the runway found."); AvailableForTakeOff = false; AvailableForLanding = false; return(false); } // Find the nodes that make up this runway: first find an edge connected to the nearest node IEnumerable <TaxiEdge> selectedEdges = taxiEdges.Where(te => te.IsRunway && (te.EndNode.Id == NearestNode.Id || te.StartNode.Id == NearestNode.Id)); if (selectedEdges.Count() == 0) { Logger.Log($"No runway edges found."); AvailableForTakeOff = false; AvailableForLanding = false; return(false); } // The name of the link gives the name of the runway, use it to retrieve all edges for this runway string edgeKey = selectedEdges.First().LinkName; selectedEdges = taxiEdges.Where(te => te.LinkName == edgeKey); RunwayNodes = FindNodeChain(taxiNodes, selectedEdges); if (AvailableForTakeOff) { FindEntries(); } else { Logger.Log("Not in use for take offs"); } if (AvailableForLanding) { FindExits(); } else { Logger.Log("Not in use for landing"); } return(true); }
public void DetermineTaxiOutLocation(IEnumerable <TaxiNode> taxiNodes) { double shortestDistance = double.MaxValue; double bestPushBackLatitude = 0; double bestPushBackLongitude = 0; TaxiNode firstAfterPush = null; TaxiNode alternateAfterPush = null; TaxiNode fallback = null; // For gates use the indicated bearings (push back), for others add 180 degrees for straight out // Then convert to -180...180 range double adjustedBearing = (LocationType == StartUpLocationType.Gate) ? Bearing : (Bearing + Math.PI); if (adjustedBearing > Math.PI) { adjustedBearing -= (VortexMath.PI2); } // Compute the distance (arbitrary units) from each taxi node to the start location foreach (TaxiNode node in taxiNodes) { node.TemporaryDistance = VortexMath.DistanceKM(node, this); } // Select the 25 nearest, then from those select only the ones that are in the 180 degree arc of the direction // we intend to move in from the startpoint // todo: make both 25 and 180 parameters IEnumerable <TaxiNode> selectedNodes = taxiNodes.OrderBy(v => v.TemporaryDistance).Take(25); fallback = selectedNodes.First(); if (fallback.TemporaryDistance < 0.0025) { // There is a atc taxi node really close to the parking, try to build pushback path from there if (fallback.IncomingEdges.Count == 1) { TaxiEdge theEdge = fallback.IncomingEdges.FirstOrDefault(); if (theEdge != null) { fallback = theEdge.StartNode; while (fallback.TemporaryDistance < 0.150 && fallback.IncomingEdges.Count <= 2) { TaxiEdge nextEdge = fallback.IncomingEdges.FirstOrDefault(e => e.StartNode != theEdge.EndNode); if (nextEdge == null) { break; } // This catches the cases at the end of an apron where the only // link is the actual taxipath already if (VortexMath.AbsTurnAngle(theEdge.Bearing, nextEdge.Bearing) > VortexMath.Deg060Rad) { break; } // todo: each node should be added to the parking as 'push back trajectory' fallback = nextEdge.StartNode; theEdge = nextEdge; } NearestNode = fallback; AlternateAfterPushBack = null; PushBackLatitude = fallback.Latitude; PushBackLongitude = fallback.Longitude; return; } } } selectedNodes = selectedNodes.Where(v => Math.Abs(adjustedBearing - VortexMath.BearingRadians(v, this)) < VortexMath.PI05); // For each qualifying node // Todo: check this part for tie downs foreach (TaxiNode v in selectedNodes) { // Look at each link coming into it from other nodes foreach (TaxiEdge incoming in v.IncomingEdges) { double pushBackLatitude = 0; double pushBackLongitude = 0; // Now find where the 'start point outgoing line' intersects with the taxi link we are currently checking if (!VortexMath.Intersection(Latitude, Longitude, adjustedBearing, incoming.StartNode.Latitude, incoming.StartNode.Longitude, incoming.Bearing, ref pushBackLatitude, ref pushBackLongitude)) { // If computation fails, try again but now with the link in the other direction. // Ignoring one way links here, I just want a push back target for now that's close to A link. if (!VortexMath.Intersection(Latitude, Longitude, adjustedBearing, incoming.StartNode.Latitude, incoming.StartNode.Longitude, incoming.Bearing + Math.PI, ref pushBackLatitude, ref pushBackLongitude)) { // Lines might be parallel, can't find intersection, skip continue; } } // Great Circles cross twice, if we found the one on the back of the earth, convert it to the // one on the airport // Todo: check might fail for airports on the -180/+180 longitude line if (Math.Abs(pushBackLongitude - Longitude) > 0.25 * Math.PI) { pushBackLatitude = -pushBackLatitude; pushBackLongitude += VortexMath.PI; if (pushBackLongitude > VortexMath.PI) { pushBackLongitude -= VortexMath.PI2; } } // To find the best spot we must know if the found intersection is actually // on the link or if it is somewhere outside the actual link. These are // still usefull in some cases bool foundTargetIsOutsideSegment = false; // Todo: check might fail for airports on the -180/+180 longitude line if (pushBackLatitude - incoming.StartNode.Latitude > 0) { if (v.Latitude - pushBackLatitude <= 0) { foundTargetIsOutsideSegment = true; } } else if (v.Latitude - pushBackLatitude > 0) { foundTargetIsOutsideSegment = true; } if (pushBackLongitude - incoming.StartNode.Longitude > 0) { if (v.Longitude - pushBackLongitude <= 0) { foundTargetIsOutsideSegment = true; } } else if (v.Longitude - pushBackLongitude > 0) { foundTargetIsOutsideSegment = true; } // Ignore links where the taxiout line intercepts at too sharp of an angle if it is // also outside the actual link. // todo: Maybe ignore these links right away, saves a lot of calculations double interceptAngleSharpness = Math.Abs(VortexMath.PI05 - Math.Abs((adjustedBearing - incoming.Bearing) % Math.PI)) / Math.PI; if (foundTargetIsOutsideSegment && interceptAngleSharpness > 0.4) { continue; } // for the found location keep track of the distance to it from the start point // also keep track of the distances to both nodes of the link we are inspecting now double pushDistance = 0.0; double distanceSource = VortexMath.DistancePyth(incoming.StartNode.Latitude, incoming.StartNode.Longitude, pushBackLatitude, pushBackLongitude); double distanceDest = VortexMath.DistancePyth(v.Latitude, v.Longitude, pushBackLatitude, pushBackLongitude); // If the found point is outside the link, add the distance to the nearest node of // the link times 2 as a penalty to the actual distance. This prevents pushback point // candidates that sneak up on the start because of a slight angle in remote link // from being accepted as best. TaxiNode nearestVertexIfPushBackOutsideSegment = null; if (foundTargetIsOutsideSegment) { if (distanceSource < distanceDest) { pushDistance = distanceSource * 2.0; nearestVertexIfPushBackOutsideSegment = incoming.StartNode; } else { pushDistance = distanceDest * 2.0; nearestVertexIfPushBackOutsideSegment = v; } } // How far is the candidate from the start point? pushDistance += VortexMath.DistancePyth(Latitude, Longitude, pushBackLatitude, pushBackLongitude); // See if it is a better candidate if (pushDistance < shortestDistance) { bestPushBackLatitude = pushBackLatitude; bestPushBackLongitude = pushBackLongitude; shortestDistance = pushDistance; // Setting things up for the path calculation that will follow later if (foundTargetIsOutsideSegment) { // The taxi out route will start with a push to the best candidate // Then move to the 'firstAfterPush' node and from there follow // the 'shortest' path to the runway firstAfterPush = nearestVertexIfPushBackOutsideSegment; alternateAfterPush = null; } else { // The taxi out route will start with a push to the best candidate // Then, if the second node in the find 'shortest' path is the alternate // the first point will be skipped. If the second point is not the alternate, // the 'firstAfterPush' will be the first indeed and after that the found // route will be followed. if (distanceSource < distanceDest) { firstAfterPush = incoming.StartNode; alternateAfterPush = v; } else { firstAfterPush = v; alternateAfterPush = incoming.StartNode; } } } } } // All candiates have been considered, post processing the winner: if (shortestDistance < double.MaxValue) { // If there is one, check if it is not too far away from the start. This catches cases where // a gate at the end of an apron with heading parallel to the apron entry would get a best // target on the taxiway outside the apron. double actualDistance = VortexMath.DistanceKM(Latitude, Longitude, bestPushBackLatitude, bestPushBackLongitude); if (actualDistance > 0.25) { // Fix this by pushing to the end point of the entry link // (If that is actually the nearest node to the parking, but alas... // this is the default WT3 behaviour anyway) NearestNode = selectedNodes.First(); AlternateAfterPushBack = null; PushBackLatitude = NearestNode.Latitude; PushBackLongitude = NearestNode.Longitude; } else { // Store the results in the startpoint PushBackLatitude = bestPushBackLatitude; PushBackLongitude = bestPushBackLongitude; NearestNode = firstAfterPush; AlternateAfterPushBack = alternateAfterPush; } } else { // Crude fallback to defautl WT behavoit if nothing was found. NearestNode = fallback; AlternateAfterPushBack = null; PushBackLatitude = NearestNode.Latitude; PushBackLongitude = NearestNode.Longitude; } }
private void ReadTaxiEdge(string line) { string[] tokens = line.Split(_splitters, StringSplitOptions.RemoveEmptyEntries); uint va = uint.Parse(tokens[1]); uint vb = uint.Parse(tokens[2]); bool isRunway = (tokens[4][0] != 't'); // taxiway_X or runway bool isTwoWay = (tokens[3][0] == 't'); // oneway or twoway XPlaneAircraftCategory maxSize; if (isRunway || tokens[4].Length < 9) { maxSize = (XPlaneAircraftCategory.Max - 1); } else { maxSize = (XPlaneAircraftCategory)(tokens[4][8] - 'A'); } string linkName = tokens.Length > 5 ? string.Join(" ", tokens.Skip(5)) : ""; TaxiNode startNode = _nodeDict[va]; TaxiNode endNode = _nodeDict[vb]; TaxiEdge outgoingEdge = _edges.SingleOrDefault(e => (e.StartNode.Id == va && e.EndNode.Id == vb)); if (outgoingEdge != null) { // todo: report warning outgoingEdge.MaxCategory = (XPlaneAircraftCategory)Math.Max((int)outgoingEdge.MaxCategory, (int)maxSize); } else { outgoingEdge = new TaxiEdge(startNode, endNode, isRunway, maxSize, linkName); _edges.Add(outgoingEdge); } TaxiEdge incomingEdge = null; if (isTwoWay) { incomingEdge = _edges.SingleOrDefault(e => (e.StartNode.Id == vb && e.EndNode.Id == va)); if (incomingEdge != null) { // todo: report warning incomingEdge.MaxCategory = (XPlaneAircraftCategory)Math.Max((int)incomingEdge.MaxCategory, (int)maxSize); } else { incomingEdge = new TaxiEdge(endNode, startNode, isRunway, maxSize, linkName); _edges.Add(incomingEdge); incomingEdge.ReverseEdge = outgoingEdge; outgoingEdge.ReverseEdge = incomingEdge; } } endNode.AddEdgeFrom(outgoingEdge); if (isTwoWay) { startNode.AddEdgeFrom(incomingEdge); } }
/// <summary> /// Dijkstra... goes through the full network finding shortest path from every node to the target so /// that afterwards we can cherrypick the starting nodes we are actually interested in. /// </summary> /// <param name="nodes">The node network</param> /// <param name="targetNode">Here do we go now</param> /// <param name="targetCategory">Minimum Cat (A-F) that needs to be supported by the route</param> private static void FindShortestPaths(IEnumerable <TaxiNode> nodes, TaxiNode targetNode, XPlaneAircraftCategory targetCategory) { List <TaxiNode> untouchedNodes = nodes.ToList(); List <TaxiNode> touchedNodes = new List <TaxiNode>(); // Reset previously found paths foreach (TaxiNode node in nodes) { node.DistanceToTarget = double.MaxValue; node.NextNodeToTarget = null; } // Setup the targetnode targetNode.DistanceToTarget = 0; targetNode.NextNodeToTarget = null; // Assign distances to all incoming edges of the target foreach (TaxiEdge incoming in targetNode.IncomingEdges) { // Skip taxiways that are too small if (targetCategory > incoming.MaxCategory) { continue; } // Mark the other side of the incoming edge as touched... if (untouchedNodes.Contains(incoming.StartNode) && !touchedNodes.Contains(incoming.StartNode)) { untouchedNodes.Remove(incoming.StartNode); touchedNodes.Add(incoming.StartNode); } // And set the properties of the path incoming.StartNode.DistanceToTarget = incoming.DistanceKM; incoming.StartNode.NextNodeToTarget = targetNode; incoming.StartNode.NameToTarget = incoming.LinkName; incoming.StartNode.PathIsRunway = incoming.IsRunway; incoming.StartNode.BearingToTarget = VortexMath.BearingRadians(incoming.StartNode, targetNode); } // Remove the target node completely, it's done. untouchedNodes.Remove(targetNode); // instantiate the comparer for taxinode list sorting ShortestPathComparer spc = new ShortestPathComparer(); // Now rinse and repeat will we still have 'touched nodes' (unprocessed nodes with a path to the target) while (touchedNodes.Count() > 0) { // Pick the next( touched but not finished) node to process, that is: the one which currently has the shortest path to the target touchedNodes.Sort(spc); TaxiNode currentNode = touchedNodes.First(); // And set the distances for the nodes with link towards the current node foreach (TaxiEdge incoming in currentNode.IncomingEdges) { // Skip taxiways that are too small if (targetCategory > incoming.MaxCategory) { continue; } // Avoid runways unless they are the only option. double distanceToCurrent = incoming.DistanceKM; if (incoming.IsRunway) { distanceToCurrent += 2.0; } // If the incoming link + the distance from the current node to the target is smaller // than the so far shortest distance from the node on the otherside of the incoming link to // the target... reroute the path from the otherside through the current node. if ((distanceToCurrent + currentNode.DistanceToTarget) < incoming.StartNode.DistanceToTarget) { if (untouchedNodes.Contains(incoming.StartNode) && !touchedNodes.Contains(incoming.StartNode)) { // The 'otherside' node is now ready to be processed untouchedNodes.Remove(incoming.StartNode); touchedNodes.Add(incoming.StartNode); } // Update the path properties incoming.StartNode.DistanceToTarget = (currentNode.DistanceToTarget + distanceToCurrent); incoming.StartNode.NextNodeToTarget = currentNode; incoming.StartNode.NameToTarget = incoming.LinkName; incoming.StartNode.PathIsRunway = incoming.IsRunway; incoming.StartNode.BearingToTarget = incoming.Bearing; } } // And the current is done. touchedNodes.Remove(currentNode); } }