public static void Smooth(List <SteerPoint> steerPoints)
{
// todo: Could add a pass to merge points within 25 meters or so.
// Take (linear) middle between points, average speed, set 'maximum' runway ops on merged point
// Maybe after smoothing works best
for (int i = 1; i < steerPoints.Count() - 1; i++)
{
if (steerPoints[i] is PushbackPoint)
{
continue;
}
SteerPoint previous = steerPoints[i - 1];
SteerPoint current = steerPoints[i];
SteerPoint next = steerPoints[i + 1];
double incomingBearing = VortexMath.BearingRadians(previous, current);
double outgoingBearing = VortexMath.BearingRadians(current, next);
double turnAngle = VortexMath.AbsTurnAngle(incomingBearing, outgoingBearing);
if (!current.Protected && turnAngle < 2.5 * VortexMath.Deg2Rad && !(current is RunwayPoint))
{
if (previous.Name == next.Name && previous.Speed == next.Speed)
{
steerPoints.RemoveAt(i);
i--;
}
}
else
if (!current.Protected && turnAngle > VortexMath.PI025) // 45 degrees
{
double smoothingDistance = 0.050 * (turnAngle / VortexMath.PI); // 90 degrees = 0.5 PI / PI = 0.5 * 0.05 km = 25 meters
double currentLatitude = current.Latitude;
double currentLongitude = current.Longitude;
if (VortexMath.DistanceKM(previous.Latitude, previous.Longitude, currentLatitude, currentLongitude) > smoothingDistance)
{
// Shift the current point a bit back
VortexMath.PointFrom(currentLatitude, currentLongitude, incomingBearing + VortexMath.PI, smoothingDistance, ref current.Latitude, ref current.Longitude);
if (current.Speed > 14)
{
current.Speed /= 2;
}
}
else
{
// skip the current
steerPoints.RemoveAt(i);
i--;
}
double distanceToNext = VortexMath.DistanceKM(currentLatitude, currentLongitude, next.Latitude, next.Longitude);
if (distanceToNext > smoothingDistance)
{
// Insert an extra point
SteerPoint newPoint = next.Duplicate();
VortexMath.PointFrom(currentLatitude, currentLongitude, outgoingBearing, smoothingDistance, ref newPoint.Latitude, ref newPoint.Longitude);
// If room for additional speed up point, reduce acceleration on this point
if (distanceToNext > 2.5 * smoothingDistance)
{
newPoint.Speed = (newPoint.Speed * 2) / 3;
}
steerPoints.Insert(i + 1, newPoint);
i++;
}
if (distanceToNext > 2.5 * smoothingDistance)
{
// Insert an extra point
SteerPoint newPoint = next.Duplicate();
VortexMath.PointFrom(currentLatitude, currentLongitude, outgoingBearing, 2.5 * smoothingDistance, ref newPoint.Latitude, ref newPoint.Longitude);
steerPoints.Insert(i + 1, newPoint);
i++;
}
}
}
}
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);
}
