Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelModifiedRodriguesParameterElementConversions.cpp
Eigen::Vector6d convertUnifiedStateModelModifiedRodriguesParametersToKeplerianElements(
        const Eigen::Vector7d& unifiedStateModelElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    Eigen::Vector6d convertedKeplerianElements = Eigen::Vector6d::Zero( );

    // Define the tolerance of a singularity
    const double singularityTolerance = 20.0 * std::numeric_limits< double >::epsilon( );

    // Extract modified Rodrigues parameters
    Eigen::Vector3d modifiedRodriguesParametersVector = unifiedStateModelElements.segment( sigma1USM6Index, 3 );
    double modifiedRodriguesParametersMagnitude =
            modifiedRodriguesParametersVector.norm( ); // magnitude of modified Rodrigues parameters, also called sigma
    bool shadowFlag = int( unifiedStateModelElements( shadowFlagUSM6Index ) ) == 1;

    // Precompute often used variables
    // Note the for SMRP some variable names do not match their definitions
    double modifiedRodriguesParametersMagnitudeSquared =
            modifiedRodriguesParametersMagnitude * modifiedRodriguesParametersMagnitude;
    double onePlusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( 1.0 + modifiedRodriguesParametersMagnitudeSquared, 2 );
    double oneMinusModifiedRodriguesParametersMagnitudeSquared = shadowFlag ?
                ( modifiedRodriguesParametersMagnitudeSquared - 1.0 ) : // inverse definition for SMRP
                ( 1.0 - modifiedRodriguesParametersMagnitudeSquared );
    double oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( oneMinusModifiedRodriguesParametersMagnitudeSquared, 2 );

    // Compute right ascension of latitude (independent on shadowing, since variables have been precomputed)
    double denominator = 4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) +
            oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared; // denominator is never null
    double rightAscensionOfLatitude =
            std::atan2( 4.0 * modifiedRodriguesParametersVector( 2 ) *
                        ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) / denominator,
                        ( oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared -
                          4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) ) /
                        denominator ); // also known as lambda

    // Trigonometric values of right ascension of latitude
    double cosineLambda = std::cos( rightAscensionOfLatitude );
    double sineLambda = std::sin( rightAscensionOfLatitude );

    // Compute auxiliary parameters auxiliaryParameter1 and auxiliaryParameter2
    double auxiliaryParameter1 = unifiedStateModelElements( Rf1HodographUSM6Index ) * cosineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * sineLambda;
    double auxiliaryParameter2 = unifiedStateModelElements( CHodographUSM6Index ) -
            unifiedStateModelElements( Rf1HodographUSM6Index ) * sineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * cosineLambda;

    // Compute auxiliary R hodograph parameter
    double RHodographElement = std::sqrt( unifiedStateModelElements( Rf1HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf1HodographUSM6Index ) +
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) );

    // Compute eccentricity
    convertedKeplerianElements( eccentricityIndex ) =
            RHodographElement / unifiedStateModelElements( CHodographUSM6Index );

    // Compute semi-major axis or, in case of a parabolic orbit, the semi-latus rectum.
    if ( std::fabs( convertedKeplerianElements( eccentricityIndex ) - 1.0 ) < singularityTolerance )
        // parabolic orbit -> semi-major axis is not defined. Use semi-latus rectum instead.
    {
        convertedKeplerianElements( semiLatusRectumIndex ) = centralBodyGravitationalParameter /
                ( unifiedStateModelElements( CHodographUSM6Index ) *
                  unifiedStateModelElements( CHodographUSM6Index ) );
    }
    else
    {
        convertedKeplerianElements( semiMajorAxisIndex ) =
                centralBodyGravitationalParameter /
                ( std::pow( unifiedStateModelElements( CHodographUSM6Index ), 2 ) *
                  ( 1 - std::pow( convertedKeplerianElements( eccentricityIndex ), 2 ) ) );
    }

    // Compute inclination
    double arccosineArgument = ( 4.0 * ( std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 0 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 1 ), 2 ) ) +
                                 oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ) /
            onePlusModifiedRodriguesParametersMagnitudeSquaredSquared;
    if ( ( ( std::fabs( arccosineArgument ) - 1.0 ) > 0.0 ) ||
         ( std::abs( std::fabs( arccosineArgument ) - 1.0 ) < singularityTolerance ) )
    {
        // Make sure that the cosine does not exceed 1.0 in magnitude
        arccosineArgument = ( arccosineArgument > 0.0 ) ? 1.0 : - 1.0;
    }
    convertedKeplerianElements( inclinationIndex ) = std::acos( arccosineArgument );
    // this acos is always defined correctly because the inclination is always below PI rad

    // Find sine and cosine of longitude of ascending node separately
    double signDirectionCosineMatrix = shadowFlag ? - 1.0 : 1.0; // sign depends on shadow conditions
    double sineOmega = 8.0 * modifiedRodriguesParametersVector( 0 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 1 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    double cosineOmega = - 8.0 * modifiedRodriguesParametersVector( 1 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 0 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    denominator = std::sqrt( cosineOmega * cosineOmega +
                             sineOmega * sineOmega ); // overwrite

    // Compute longitude of ascending node
    if ( std::fabs( convertedKeplerianElements( inclinationIndex ) - PI ) < singularityTolerance )
        // pure-retrograde orbit -> inclination = PI
    {
        // Define the error message
        std::stringstream errorMessage;
        errorMessage << "Pure-retrograde orbit (inclination = PI).\n"
                     << "Unified state model elements cannot be transformed to Kepler elements." << std::endl;

        // Throw exception
        throw std::runtime_error( std::runtime_error( errorMessage.str( ) ) );
    }
    else if ( ( std::fabs( convertedKeplerianElements( inclinationIndex ) ) < singularityTolerance ) ||
        // pure-prograde orbit -> inclination = 0 -> RAAN is undefined, hence give zero
              ( std::fabs( denominator ) < singularityTolerance ) )
        // denominator is null, find a workaround
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
    }
    else
    {
        // Compute longitude of ascending node
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = std::atan2(
                    sineOmega / denominator, cosineOmega / denominator );

        // Round off small values of the right ascension of ascending node to zero
        if ( std::fabs( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
        }

        // Ensure the longitude of ascending node is positive
        while ( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 )
            // Because of the previous if statement, if the longitude of ascending node is smaller than 0, it will
            // always be smaller than -singularityTolerance
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) += 2.0 * PI;
        }
    }

    // Compute true anomaly and argument of periapsis
    if ( std::fabs( RHodographElement ) < singularityTolerance ) // circular orbit
    {
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; // by definition
        convertedKeplerianElements( trueAnomalyIndex ) =
                rightAscensionOfLatitude - convertedKeplerianElements( longitudeOfAscendingNodeIndex );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
            // Because of the previous if statement, if the true anomaly is smaller than zero, it will always be smaller than
            // -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }
    }
    else
    {
        convertedKeplerianElements( trueAnomalyIndex ) =
                std::atan2( ( auxiliaryParameter1 / RHodographElement ),
                            ( ( auxiliaryParameter2 - unifiedStateModelElements( CHodographUSM6Index ) )
                              / RHodographElement ) );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
            // Because of the previous if statement, if the true anomaly is smaller than zero, it will always
            // be smaller than -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }

        convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                rightAscensionOfLatitude -
                convertedKeplerianElements( longitudeOfAscendingNodeIndex ) -
                convertedKeplerianElements( trueAnomalyIndex );

        // Round off small omega to zero
        if ( std::fabs( convertedKeplerianElements( argumentOfPeriapsisIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;
        }

        // Ensure the argument of periapsis is positive
        while ( convertedKeplerianElements( argumentOfPeriapsisIndex ) < 0.0 )
            // Because of the previous if statement, if the argument of pericenter is smaller than zero,
            // it will be smaller than -singularityTolerance
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) += 2.0 * PI;
        }
    }

    // Give back result
    return convertedKeplerianElements;
}
//! Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements .
Eigen::Vector6d convertUnifiedStateModelModifiedRodriguesParametersToKeplerianElements(
        const Eigen::Vector7d& unifiedStateModelElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;
    Eigen::Vector6d convertedKeplerianElements = Eigen::Vector6d::Zero( );
    const double singularityTolerance = 20.0 * std::numeric_limits< double >::epsilon( );
    Eigen::Vector3d modifiedRodriguesParametersVector = unifiedStateModelElements.segment( sigma1USM6Index, 3 );
    double modifiedRodriguesParametersMagnitude =
            modifiedRodriguesParametersVector.norm( ); 
    bool shadowFlag = int( unifiedStateModelElements( shadowFlagUSM6Index ) ) == 1;
    double modifiedRodriguesParametersMagnitudeSquared =
            modifiedRodriguesParametersMagnitude * modifiedRodriguesParametersMagnitude;
    double onePlusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( 1.0 + modifiedRodriguesParametersMagnitudeSquared, 2 );
    double oneMinusModifiedRodriguesParametersMagnitudeSquared = shadowFlag ?
                ( modifiedRodriguesParametersMagnitudeSquared - 1.0 ) : 
                ( 1.0 - modifiedRodriguesParametersMagnitudeSquared );
    double oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( oneMinusModifiedRodriguesParametersMagnitudeSquared, 2 );
    double denominator = 4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) +
            oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared; 
    double rightAscensionOfLatitude =
            std::atan2( 4.0 * modifiedRodriguesParametersVector( 2 ) *
                        ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) / denominator,
                        ( oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared -
                          4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) ) /
                        denominator ); 
    double cosineLambda = std::cos( rightAscensionOfLatitude );
    double sineLambda = std::sin( rightAscensionOfLatitude );
    double auxiliaryParameter1 = unifiedStateModelElements( Rf1HodographUSM6Index ) * cosineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * sineLambda;
    double auxiliaryParameter2 = unifiedStateModelElements( CHodographUSM6Index ) -
            unifiedStateModelElements( Rf1HodographUSM6Index ) * sineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * cosineLambda;
    double RHodographElement = std::sqrt( unifiedStateModelElements( Rf1HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf1HodographUSM6Index ) +
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) );
    convertedKeplerianElements( eccentricityIndex ) =
            RHodographElement / unifiedStateModelElements( CHodographUSM6Index );
    if ( std::fabs( convertedKeplerianElements( eccentricityIndex ) - 1.0 ) < singularityTolerance )
    {
        convertedKeplerianElements( semiLatusRectumIndex ) = centralBodyGravitationalParameter /
                ( unifiedStateModelElements( CHodographUSM6Index ) *
                  unifiedStateModelElements( CHodographUSM6Index ) );
    }
    else
    {
        convertedKeplerianElements( semiMajorAxisIndex ) =
                centralBodyGravitationalParameter /
                ( std::pow( unifiedStateModelElements( CHodographUSM6Index ), 2 ) *
                  ( 1 - std::pow( convertedKeplerianElements( eccentricityIndex ), 2 ) ) );
    }
    double arccosineArgument = ( 4.0 * ( std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 0 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 1 ), 2 ) ) +
                                 oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ) /
            onePlusModifiedRodriguesParametersMagnitudeSquaredSquared;
    if ( ( ( std::fabs( arccosineArgument ) - 1.0 ) > 0.0 ) ||
         ( std::abs( std::fabs( arccosineArgument ) - 1.0 ) < singularityTolerance ) )
    {
        arccosineArgument = ( arccosineArgument > 0.0 ) ? 1.0 : - 1.0;
    }
    convertedKeplerianElements( inclinationIndex ) = std::acos( arccosineArgument );
    double signDirectionCosineMatrix = shadowFlag ? - 1.0 : 1.0; 
    double sineOmega = 8.0 * modifiedRodriguesParametersVector( 0 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 1 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    double cosineOmega = - 8.0 * modifiedRodriguesParametersVector( 1 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 0 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    denominator = std::sqrt( cosineOmega * cosineOmega +
                             sineOmega * sineOmega ); 
    if ( std::fabs( convertedKeplerianElements( inclinationIndex ) - PI ) < singularityTolerance )
    {
        std::stringstream errorMessage;
        errorMessage << "Pure-retrograde orbit (inclination = PI).\n"
                     << "Unified state model elements cannot be transformed to Kepler elements." << std::endl;
        throw std::runtime_error( std::runtime_error( errorMessage.str( ) ) );
    }
    else if ( ( std::fabs( convertedKeplerianElements( inclinationIndex ) ) < singularityTolerance ) ||
              ( std::fabs( denominator ) < singularityTolerance ) )
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
    }
    else
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = std::atan2(
                    sineOmega / denominator, cosineOmega / denominator );
        if ( std::fabs( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) += 2.0 * PI;
        }
    }
    if ( std::fabs( RHodographElement ) < singularityTolerance ) 
    {
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; 
        convertedKeplerianElements( trueAnomalyIndex ) =
                rightAscensionOfLatitude - convertedKeplerianElements( longitudeOfAscendingNodeIndex );
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }
    }
    else
    {
        convertedKeplerianElements( trueAnomalyIndex ) =
                std::atan2( ( auxiliaryParameter1 / RHodographElement ),
                            ( ( auxiliaryParameter2 - unifiedStateModelElements( CHodographUSM6Index ) )
                              / RHodographElement ) );
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }
        convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                rightAscensionOfLatitude -
                convertedKeplerianElements( longitudeOfAscendingNodeIndex ) -
                convertedKeplerianElements( trueAnomalyIndex );
        if ( std::fabs( convertedKeplerianElements( argumentOfPeriapsisIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( argumentOfPeriapsisIndex ) < 0.0 )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) += 2.0 * PI;
        }
    }
    return convertedKeplerianElements;
}
Eigen :: Vector6d convertUnifiedStateModelModifiedRodriguesParametersToKeplerianElements ( const Eigen :: Vector7d & unifiedStateModelElements , const double centralBodyGravitationalParameter ) { using mathematical_constants :: PI ; Eigen :: Vector6d convertedKeplerianElements = Eigen :: Vector6d :: Zero ( ) ; const double singularityTolerance = 20.0 * std :: numeric_limits < double > :: epsilon ( ) ; Eigen :: Vector3d modifiedRodriguesParametersVector = unifiedStateModelElements . segment ( sigma1USM6Index , 3 ) ; double modifiedRodriguesParametersMagnitude = modifiedRodriguesParametersVector . norm ( ) ; bool shadowFlag = int ( unifiedStateModelElements ( shadowFlagUSM6Index ) ) == 1 ; double modifiedRodriguesParametersMagnitudeSquared = modifiedRodriguesParametersMagnitude * modifiedRodriguesParametersMagnitude ; double onePlusModifiedRodriguesParametersMagnitudeSquaredSquared = std :: pow ( 1.0 + modifiedRodriguesParametersMagnitudeSquared , 2 ) ; double oneMinusModifiedRodriguesParametersMagnitudeSquared = shadowFlag ? ( modifiedRodriguesParametersMagnitudeSquared - 1.0 ) : ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) ; double oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared = std :: pow ( oneMinusModifiedRodriguesParametersMagnitudeSquared , 2 ) ; double denominator = 4.0 * std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) + oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ; double rightAscensionOfLatitude = std :: atan2 ( 4.0 * modifiedRodriguesParametersVector ( 2 ) * ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) / denominator , ( oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared - 4.0 * std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) ) / denominator ) ; double cosineLambda = std :: cos ( rightAscensionOfLatitude ) ; double sineLambda = std :: sin ( rightAscensionOfLatitude ) ; double auxiliaryParameter1 = unifiedStateModelElements ( Rf1HodographUSM6Index ) * cosineLambda + unifiedStateModelElements ( Rf2HodographUSM6Index ) * sineLambda ; double auxiliaryParameter2 = unifiedStateModelElements ( CHodographUSM6Index ) - unifiedStateModelElements ( Rf1HodographUSM6Index ) * sineLambda + unifiedStateModelElements ( Rf2HodographUSM6Index ) * cosineLambda ; double RHodographElement = std :: sqrt ( unifiedStateModelElements ( Rf1HodographUSM6Index ) * unifiedStateModelElements ( Rf1HodographUSM6Index ) + unifiedStateModelElements ( Rf2HodographUSM6Index ) * unifiedStateModelElements ( Rf2HodographUSM6Index ) ) ; convertedKeplerianElements ( eccentricityIndex ) = RHodographElement / unifiedStateModelElements ( CHodographUSM6Index ) ; if ( std :: fabs ( convertedKeplerianElements ( eccentricityIndex ) - 1.0 ) < singularityTolerance ) { convertedKeplerianElements ( semiLatusRectumIndex ) = centralBodyGravitationalParameter / ( unifiedStateModelElements ( CHodographUSM6Index ) * unifiedStateModelElements ( CHodographUSM6Index ) ) ; } else { convertedKeplerianElements ( semiMajorAxisIndex ) = centralBodyGravitationalParameter / ( std :: pow ( unifiedStateModelElements ( CHodographUSM6Index ) , 2 ) * ( 1 - std :: pow ( convertedKeplerianElements ( eccentricityIndex ) , 2 ) ) ) ; } double arccosineArgument = ( 4.0 * ( std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) - std :: pow ( modifiedRodriguesParametersVector ( 0 ) , 2 ) - std :: pow ( modifiedRodriguesParametersVector ( 1 ) , 2 ) ) + oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ) / onePlusModifiedRodriguesParametersMagnitudeSquaredSquared ; if ( ( ( std :: fabs ( arccosineArgument ) - 1.0 ) > 0.0 ) || ( std :: abs ( std :: fabs ( arccosineArgument ) - 1.0 ) < singularityTolerance ) ) { arccosineArgument = ( arccosineArgument > 0.0 ) ? 1.0 : - 1.0 ; } convertedKeplerianElements ( inclinationIndex ) = std :: acos ( arccosineArgument ) ; double signDirectionCosineMatrix = shadowFlag ? - 1.0 : 1.0 ; double sineOmega = 8.0 * modifiedRodriguesParametersVector ( 0 ) * modifiedRodriguesParametersVector ( 2 ) + signDirectionCosineMatrix * 4.0 * modifiedRodriguesParametersVector ( 1 ) * oneMinusModifiedRodriguesParametersMagnitudeSquared ; double cosineOmega = - 8.0 * modifiedRodriguesParametersVector ( 1 ) * modifiedRodriguesParametersVector ( 2 ) + signDirectionCosineMatrix * 4.0 * modifiedRodriguesParametersVector ( 0 ) * oneMinusModifiedRodriguesParametersMagnitudeSquared ; denominator = std :: sqrt ( cosineOmega * cosineOmega + sineOmega * sineOmega ) ; if ( std :: fabs ( convertedKeplerianElements ( inclinationIndex ) - PI ) < singularityTolerance ) { std :: stringstream errorMessage ; errorMessage << " \n " << " " << std :: endl ; throw std :: runtime_error ( std :: runtime_error ( errorMessage . str ( ) ) ) ; } else if ( ( std :: fabs ( convertedKeplerianElements ( inclinationIndex ) ) < singularityTolerance ) || ( std :: fabs ( denominator ) < singularityTolerance ) ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = 0.0 ; } else { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = std :: atan2 ( sineOmega / denominator , cosineOmega / denominator ) ; if ( std :: fabs ( convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = 0.0 ; } while ( convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) < 0.0 ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) += 2.0 * PI ; } } if ( std :: fabs ( RHodographElement ) < singularityTolerance ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) = 0.0 ; convertedKeplerianElements ( trueAnomalyIndex ) = rightAscensionOfLatitude - convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) ; if ( std :: fabs ( convertedKeplerianElements ( trueAnomalyIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( trueAnomalyIndex ) = 0.0 ; } while ( convertedKeplerianElements ( trueAnomalyIndex ) < 0.0 ) { convertedKeplerianElements ( trueAnomalyIndex ) += 2.0 * PI ; } } else { convertedKeplerianElements ( trueAnomalyIndex ) = std :: atan2 ( ( auxiliaryParameter1 / RHodographElement ) , ( ( auxiliaryParameter2 - unifiedStateModelElements ( CHodographUSM6Index ) ) / RHodographElement ) ) ; if ( std :: fabs ( convertedKeplerianElements ( trueAnomalyIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( trueAnomalyIndex ) = 0.0 ; } while ( convertedKeplerianElements ( trueAnomalyIndex ) < 0.0 ) { convertedKeplerianElements ( trueAnomalyIndex ) += 2.0 * PI ; } convertedKeplerianElements ( argumentOfPeriapsisIndex ) = rightAscensionOfLatitude - convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) - convertedKeplerianElements ( trueAnomalyIndex ) ; if ( std :: fabs ( convertedKeplerianElements ( argumentOfPeriapsisIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) = 0.0 ; } while ( convertedKeplerianElements ( argumentOfPeriapsisIndex ) < 0.0 ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) += 2.0 * PI ; } } return convertedKeplerianElements ; }
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements .
// Declaring eventual output vector. // Define the tolerance of a singularity // Extract modified Rodrigues parameters // magnitude of modified Rodrigues parameters, also called sigma // Precompute often used variables // Note the for SMRP some variable names do not match their definitions // inverse definition for SMRP // Compute right ascension of latitude (independent on shadowing, since variables have been precomputed) // denominator is never null // also known as lambda // Trigonometric values of right ascension of latitude // Compute auxiliary parameters auxiliaryParameter1 and auxiliaryParameter2 // Compute auxiliary R hodograph parameter // Compute eccentricity // Compute semi-major axis or, in case of a parabolic orbit, the semi-latus rectum. // parabolic orbit -> semi-major axis is not defined. Use semi-latus rectum instead. // Compute inclination // Make sure that the cosine does not exceed 1.0 in magnitude // this acos is always defined correctly because the inclination is always below PI rad // Find sine and cosine of longitude of ascending node separately // sign depends on shadow conditions // overwrite // Compute longitude of ascending node // pure-retrograde orbit -> inclination = PI // Define the error message // Throw exception // pure-prograde orbit -> inclination = 0 -> RAAN is undefined, hence give zero // denominator is null, find a workaround // Compute longitude of ascending node // Round off small values of the right ascension of ascending node to zero // Ensure the longitude of ascending node is positive // Because of the previous if statement, if the longitude of ascending node is smaller than 0, it will // always be smaller than -singularityTolerance // Compute true anomaly and argument of periapsis // circular orbit // by definition // Round off small theta to zero // Ensure the true anomaly is positive // Because of the previous if statement, if the true anomaly is smaller than zero, it will always be smaller than // -singularityTolerance // Round off small theta to zero // Ensure the true anomaly is positive // Because of the previous if statement, if the true anomaly is smaller than zero, it will always // be smaller than -singularityTolerance // Round off small omega to zero // Ensure the argument of periapsis is positive // Because of the previous if statement, if the argument of pericenter is smaller than zero, // it will be smaller than -singularityTolerance // Give back result