Tudat/Astrodynamics/BasicAstrodynamics/attitudeElementConversions.cpp
//! Convert exponential map to quaternions.
Convert exponential map to quaternions.
Convert exponential map to quaternions .
Eigen::Vector4d convertExponentialMapToQuaternionElements( const Eigen::Vector4d& exponentialMapElements )
{
    const double singularityTolerance = 20.0 * std::numeric_limits< double >::epsilon( );
    Eigen::Vector4d convertedQuaternionElements;
    Eigen::Vector3d exponentialMapVector = exponentialMapElements.segment( e1ExponentialMapIndex, 3 );
    double exponentialMapMagnitude = exponentialMapVector.norm( );
    convertedQuaternionElements( etaQuaternionIndex ) = std::cos( 0.5 * exponentialMapMagnitude );
    if ( std::fabs( exponentialMapMagnitude ) < singularityTolerance )
    {
        convertedQuaternionElements.segment( epsilon1QuaternionIndex, 3 ) =
                exponentialMapVector * ( 0.5 - exponentialMapMagnitude * exponentialMapMagnitude / 48.0 );
    }
    else
    {
        convertedQuaternionElements.segment( epsilon1QuaternionIndex, 3 ) =
                exponentialMapVector / exponentialMapMagnitude * std::sin( 0.5 * exponentialMapMagnitude );
    }
    double conversionSign = ( int( exponentialMapElements( shadowFlagExponentialMapIndex ) ) == 1 ) ?
                - 1.0 : 1.0; 
    return conversionSign * convertedQuaternionElements;
}
Eigen :: Vector4d convertExponentialMapToQuaternionElements ( const Eigen :: Vector4d & exponentialMapElements ) { const double singularityTolerance = 20.0 * std :: numeric_limits < double > :: epsilon ( ) ; Eigen :: Vector4d convertedQuaternionElements ; Eigen :: Vector3d exponentialMapVector = exponentialMapElements . segment ( e1ExponentialMapIndex , 3 ) ; double exponentialMapMagnitude = exponentialMapVector . norm ( ) ; convertedQuaternionElements ( etaQuaternionIndex ) = std :: cos ( 0.5 * exponentialMapMagnitude ) ; if ( std :: fabs ( exponentialMapMagnitude ) < singularityTolerance ) { convertedQuaternionElements . segment ( epsilon1QuaternionIndex , 3 ) = exponentialMapVector * ( 0.5 - exponentialMapMagnitude * exponentialMapMagnitude / 48.0 ) ; } else { convertedQuaternionElements . segment ( epsilon1QuaternionIndex , 3 ) = exponentialMapVector / exponentialMapMagnitude * std :: sin ( 0.5 * exponentialMapMagnitude ) ; } double conversionSign = ( int ( exponentialMapElements ( shadowFlagExponentialMapIndex ) ) == 1 ) ? - 1.0 : 1.0 ; return conversionSign * convertedQuaternionElements ; }
Convert exponential map to quaternions.
Convert exponential map to quaternions .
// Define the tolerance of a singularity // Declare eventual output vector // Extract exponential map // Convert exponential map to quaternions // Change sign based on shadow flag // converion is slightly different for SEM and EM // Give output