/// <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++; } }