src/AircraftTrajectories/Models/Optimisation/FlightSimulator.cs
/// <summary>
/// Check whether we reached the end of a turn
/// </summary>
Check whether we reached the end of a turn
Check whether we reached the end of a turn
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))
            {
                _deltaHeading = Interpolate(0, AngleDifference(_segmentStartHeading, targetHeading) + (Math.PI / 4), Setting(3));
            } else
            {
                _deltaHeading = Interpolate(-(AngleDifference(targetHeading, _segmentStartHeading) + (Math.PI / 4)), 0, Setting(3));
            }
            if (_segmentIndex == _numberOfSegments - 1)
            {
                currentPoint = new Point3D(_x, _y, 0, CoordinateUnit.metric);
                targetHeading = currentPoint.HeadingTo(_endPoint) * Math.PI / 180;
                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++;
            }
        }
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 ) ) { _deltaHeading = Interpolate ( 0 , AngleDifference ( _segmentStartHeading , targetHeading ) + ( Math . PI / 4 ) , Setting ( 3 ) ) ; } else { _deltaHeading = Interpolate ( - ( AngleDifference ( targetHeading , _segmentStartHeading ) + ( Math . PI / 4 ) ) , 0 , Setting ( 3 ) ) ; } if ( _segmentIndex == _numberOfSegments - 1 ) { currentPoint = new Point3D ( _x , _y , 0 , CoordinateUnit . metric ) ; targetHeading = currentPoint . HeadingTo ( _endPoint ) * Math . PI / 180 ; 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 ( " " + _segmentIndex + " " + _x + " " + _y + " " + _heading * 180 / Math . PI ) ; _horizontal_state = HORIZONTAL_STATE . STRAIGHT ; _segmentStartPoint = new Point3D ( _x , _y , 0 , CoordinateUnit . metric ) ; _heading = targetHeading ; _segmentIndex ++ ; } }
Check whether we reached the end of a turn
Check whether we reached the end of a turn
// right turn // left turn // Check whether we are in the last turn of the trajectory /*
                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));
                */