private IEnumerable <SteerPoint> BuildSteerPoints(Parking currentParking, ResultRoute route) { LinkedNode link = route.RouteStart; TaxiNode nodeToWrite = route.StartNode; EntryPoint entryPoint = route.RunwayEntryPoint; List <SteerPoint> steerPoints = new List <SteerPoint> { new ParkingPoint(currentParking.Latitude, currentParking.Longitude, 3, $"{currentParking.Name}", currentParking.Bearing, false) }; // Write Pushback node, allowing room for turn double addLat = 0; double addLon = 0; // See if we need to skip the first route node if (currentParking.AlternateAfterPushBack != null && currentParking.AlternateAfterPushBack == route.RouteStart.Node) { // Our pushback point is better than the first point of the route nodeToWrite = currentParking.AlternateAfterPushBack; } // insert one more point here where the plane is pushed a little bit away from the next point if (currentParking.LocationType == StartUpLocationType.Gate) { if (nodeToWrite != null) { double nextPushBearing; if (VortexMath.DistanceKM(nodeToWrite.Latitude, nodeToWrite.Longitude, currentParking.PushBackLatitude, currentParking.PushBackLongitude) > 0.010) { // Push target is a virtual node nextPushBearing = VortexMath.BearingRadians(nodeToWrite.Latitude, nodeToWrite.Longitude, currentParking.PushBackLatitude, currentParking.PushBackLongitude); } else { // Push target is very close to the actual first node of the route nextPushBearing = (nodeToWrite.BearingToTarget + VortexMath.PI) % VortexMath.PI2; } double turn = VortexMath.TurnAngle(currentParking.Bearing + VortexMath.PI, nextPushBearing); double turnAbs = Math.Abs(turn); double factor = ((turnAbs) / VortexMath.PI); // 0...0.5.....1 factor = (factor * factor) + factor / 4; // 0...0.375...1.25 double distance = 0.040 * factor; // 0m...15m ...50m if (turnAbs < VortexMath.Deg135Rad) { // Try to trun the aircraft to the bearing it will need to go in after pushback // First point is on the pushback heading, but away from the actual target to allow the AC to turn VortexMath.PointFrom(currentParking.PushBackLatitude, currentParking.PushBackLongitude, currentParking.Bearing, distance, ref addLat, ref addLon); steerPoints.Add(new PushbackPoint(addLat, addLon, 2, $"{currentParking.Name}")); // Second point is on the (extended) line of the first link of the actual route VortexMath.PointFrom(currentParking.PushBackLatitude, currentParking.PushBackLongitude, nextPushBearing, distance, ref addLat, ref addLon); steerPoints.Add(new PushbackPoint(addLat, addLon, 2, $"{link.Edge.LinkName}")); // Third point is on the same line but a little bit extra backwards to get the nose in the intended heading VortexMath.PointFrom(currentParking.PushBackLatitude, currentParking.PushBackLongitude, nextPushBearing, distance + 0.015, ref addLat, ref addLon); steerPoints.Add(new SteerPoint(addLat, addLon, 8, $"{link.Edge.LinkName}", true)); } else { // Let's just turn it to a 90 degree angle with the first edge // First point is on the pushback heading, but away from the actual target to allow the AC to turn VortexMath.PointFrom(currentParking.PushBackLatitude, currentParking.PushBackLongitude, currentParking.Bearing, distance, ref addLat, ref addLon); steerPoints.Add(new PushbackPoint(addLat, addLon, 2, $"{currentParking.Name}")); // Second point is on the (extended) line of the first link of the actual route, but much closer then for the full turn VortexMath.PointFrom(currentParking.PushBackLatitude, currentParking.PushBackLongitude, nextPushBearing, distance / 2.0, ref addLat, ref addLon); steerPoints.Add(new PushbackPoint(addLat, addLon, 2, $"{link.Edge.LinkName}")); // Third point is on +/-90 degree angle from the first link VortexMath.PointFrom(addLat, addLon, (turn > 0) ? nextPushBearing + VortexMath.PI05 : nextPushBearing - VortexMath.PI05, 0.015, ref addLat, ref addLon); steerPoints.Add(new SteerPoint(addLat, addLon, 5, $"{link.Edge.LinkName}", true)); // Add a fourth point back on the intended line steerPoints.Add(new SteerPoint(currentParking.PushBackLatitude, currentParking.PushBackLongitude, 8, $"{link.Edge.LinkName}")); } } } else { // Tie down, hangar, misc: just add the 'pushback' point as first target, smoothing should take care of the rest steerPoints.Add(new SteerPoint(currentParking.PushBackLatitude, currentParking.PushBackLongitude, 8, $"{link.Edge.LinkName}")); } if (nodeToWrite != link.Node) { steerPoints.Add(new SteerPoint(nodeToWrite.Latitude, nodeToWrite.Longitude, 8, $"{link.Edge.LinkName}")); } while (link.Node != null) { bool activeZone = false; string activeFor = ""; if (link.Edge.ActiveZone) { activeZone = true; activeFor = link.Edge.ActiveForRunway(Runway.Designator); } else if (link.Next.Edge != null && link.Next.Edge.ActiveZone) { activeZone = true; activeFor = link.Next.Edge.ActiveForRunway(Runway.Designator); } else if (link.Next.Edge == null) { activeZone = true; activeFor = Runway.Designator; } if (activeZone) { steerPoints.Add(new RunwayPoint(link.Node.Latitude, link.Node.Longitude, 15, $"{link.Edge.LinkName}", activeFor)); } else { steerPoints.Add(new SteerPoint(link.Node.Latitude, link.Node.Longitude, 15, $"{link.Edge.LinkName}")); } link = link.Next; } steerPoints.Add(new RunwayPoint(entryPoint.OnRunwayNode, 8, Runway.Designator, Runway.Designator)); VortexMath.PointFrom(entryPoint.OnRunwayNode, Runway.Bearing, 0.022, ref addLat, ref addLon); steerPoints.Add(new RunwayPoint(addLat, addLon, 6, Runway.Designator, Runway.Designator)); RouteProcessor.Smooth(steerPoints); RouteProcessor.ProcessRunwayOperations(steerPoints); if (MaxOutPoints < steerPoints.Count) { MaxOutPoints = steerPoints.Count; } return(steerPoints); }
private IEnumerable <SteerPoint> BuildSteerPoints(ResultRoute route, TaxiNode runwayExitNode) { List <SteerPoint> steerPoints = new List <SteerPoint>(); // Route should start at the (displaced) threshold RunwayPoint threshold = new RunwayPoint(route.Runway.DisplacedNode, 55, $"{route.Runway.Designator} Threshold", route.RouteStart.Edge.ActiveForRunway(route.Runway.Designator)) { OnRunway = true, IsExiting = true }; steerPoints.Add(threshold); foreach (TaxiNode node in route.Runway.RunwayNodes) { int speed = (node == runwayExitNode) ? 35 : 55; steerPoints.Add(new RunwayPoint(node.Latitude, node.Longitude, speed, $"{route.Runway.Designator}", route.RouteStart.Edge.ActiveForRunway(route.Runway.Designator))); if (node == runwayExitNode) // Key of the dictionary is the last node on the runway centerline for this route { break; } } // This is the first node off the runway centerline steerPoints.Add(new RunwayPoint(route.StartNode, 30, route.RouteStart.Edge.LinkName, route.RouteStart.Edge.ActiveForRunway(route.Runway.Designator))); LinkedNode link = route.RouteStart; while (link.Node != null) { bool activeZone = false; string activeFor = ""; if (link.Edge.ActiveZone) { activeZone = true; activeFor = link.Edge.ActiveForRunway(""); } else if (link.Next.Edge != null && link.Next.Edge.ActiveZone) { activeZone = true; activeFor = link.Next.Edge.ActiveForRunway(""); } if (activeZone) { steerPoints.Add(new RunwayPoint(link.Node.Latitude, link.Node.Longitude, 15, $"{link.Edge.LinkName}", activeFor)); } else { steerPoints.Add(new SteerPoint(link.Node.Latitude, link.Node.Longitude, 15, $"{link.Edge.LinkName}")); } link = link.Next; } // remove last point if it takes us past the 'pushback point' if (steerPoints.Count > 1) { SteerPoint oneButLast = steerPoints.ElementAt(steerPoints.Count - 2); SteerPoint last = steerPoints.ElementAt(steerPoints.Count - 1); double lastBearing = VortexMath.BearingRadians(oneButLast, last); double bearingToPush = VortexMath.BearingRadians(last.Latitude, last.Longitude, Parking.PushBackLatitude, Parking.PushBackLongitude); double turnToPush = VortexMath.AbsTurnAngle(lastBearing, bearingToPush); if (turnToPush > VortexMath.Deg100Rad) { steerPoints.RemoveAt(steerPoints.Count - 1); } } // todo: how does this all work with freaky pushback points? // todo: tie downs steerPoints.Add(new SteerPoint(Parking.PushBackLatitude, Parking.PushBackLongitude, 5, Parking.Name)); steerPoints.Add(new ParkingPoint(Parking.Latitude, Parking.Longitude, 5, Parking.Name, Parking.Bearing, true)); //RouteProcessor.Smooth(steerPoints); RouteProcessor.ProcessRunwayOperations(steerPoints); if (MaxInPoints < steerPoints.Count) { MaxInPoints = steerPoints.Count; } return(steerPoints); }