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