/// <summary>
/// Check whether we reached the end of a straight segment
/// </summary>
protected void CheckStraightEnd()
{
var currentPoint = new Point3D(_x, _y, 0, CoordinateUnit.metric);
// At the last straight segment, quit the optimisation when we reach the endpoint
if (_segmentIndex == _numberOfSegments)
{
if (currentPoint.DistanceTo(_segmentStartPoint) >= _endPoint.DistanceTo(_segmentStartPoint))
{
Log("Reached end of segment" + _segmentIndex + " (straight) X:" + _x + " Y:" + _y + " Heading:" + _heading * 180 / Math.PI);
_vertical_state = VERTICAL_STATE.END;
}
}
else
{
// At the second last straight segment,
// continue flying as long as we cannot reach the endpoint with a minimum radius turn in less than a 180degree turn
if (_segmentIndex == _numberOfSegments - 2 &&
currentPoint.DistanceTo(_endPoint) / 2 < _aircraft.MinimumTurnRadius(_aircraft.VMax))
{
Log("Continue X:" + _x);
return;
}
// Switch from flying straight to a turn if we reach beyond the given segment length
if (currentPoint.DistanceTo(_segmentStartPoint) >= Interpolate(0, MAX_SEGMENT_LENGTH, Setting(2)))
{
Log("Reached end of segment" + _segmentIndex + " (straight) X:" + _x + " Y:" + _y + " Heading:" + _heading * 180 / Math.PI);
_horizontal_state = HORIZONTAL_STATE.TURN;
_segmentStartHeading = _heading;
_segmentIndex++;
}
}
}
/// <summary>
/// Check whether we reached the end of a turn
/// </summary>
public void CheckEndOfTurn()
{
bool switchHorizontalState = false;
var currentPoint = new Point3D(_x, _y, 0, CoordinateUnit.metric);
var targetHeading = currentPoint.HeadingTo(_endPoint) * Math.PI / 180;
if (AngleDifference(targetHeading, _segmentStartHeading) > AngleDifference(_segmentStartHeading, targetHeading))
{
// right turn
_deltaHeading = Interpolate(0, AngleDifference(_segmentStartHeading, targetHeading) + (Math.PI / 4), Setting(3));
}
else
{
// left turn
_deltaHeading = Interpolate(-(AngleDifference(targetHeading, _segmentStartHeading) + (Math.PI / 4)), 0, Setting(3));
}
// Check whether we are in the last turn of the trajectory
if (_segmentIndex == _numberOfSegments - 1)
{
currentPoint = new Point3D(_x, _y, 0, CoordinateUnit.metric);
targetHeading = currentPoint.HeadingTo(_endPoint) * Math.PI / 180;
/*
* Console.WriteLine((_heading * 180 / Math.PI) + " to " + targetHeading * 180 / Math.PI + " seg:"+ _segmentStartHeading * 180 / Math.PI);
* Console.WriteLine(AngleDifference(_heading, _segmentStartHeading));
* Console.WriteLine(AngleDifference(_segmentStartHeading, targetHeading));
* Console.WriteLine(deltaHeading);
* Console.WriteLine(Setting(3));
*/
switchHorizontalState =
(_deltaHeading < 0 && AngleDifference(_heading, _segmentStartHeading) >= AngleDifference(targetHeading, _segmentStartHeading)) ||
(_deltaHeading > 0 && AngleDifference(_segmentStartHeading, _heading) >= AngleDifference(_segmentStartHeading, targetHeading));
}
else
{
targetHeading = (_segmentStartHeading + _deltaHeading) % (2 * Math.PI);
switchHorizontalState =
(_deltaHeading < 0 && AngleDifference(_heading, _segmentStartHeading) >= AngleDifference(targetHeading, _segmentStartHeading)) ||
(_deltaHeading > 0 && AngleDifference(_segmentStartHeading, _heading) >= AngleDifference(_segmentStartHeading, targetHeading));
}
if (switchHorizontalState || _deltaHeading == 0)
{
Log("Reached end of segment" + _segmentIndex + " (turn) X:" + _x + " Y:" + _y + " Heading:" + _heading * 180 / Math.PI);
_horizontal_state = HORIZONTAL_STATE.STRAIGHT;
_segmentStartPoint = new Point3D(_x, _y, 0, CoordinateUnit.metric);
_heading = targetHeading;
_segmentIndex++;
}
}
