Tudat/Astrodynamics/BasicAstrodynamics/attitudeElementConversions.cpp
//! Convert quaternions to exponential map.
Convert quaternions to exponential map.
Convert quaternions to exponential map .
Eigen::Vector4d convertQuaternionsToExponentialMapElements( const Eigen::Vector4d& quaternionElements )
{
    using mathematical_constants::PI;
    const double singularityTolerance = 20.0 * std::numeric_limits< double >::epsilon( );
    Eigen::Vector4d convertedExponentialMapElements;
    double exponentialMapMagnitude = 2.0 * std::acos( quaternionElements( etaQuaternionIndex ) );
    bool shadowFlag = std::fabs( exponentialMapMagnitude ) > PI;
    Eigen::Vector3d exponentialMapVector = quaternionElements.segment( epsilon1QuaternionIndex, 3 );
    if ( std::fabs( exponentialMapMagnitude ) < singularityTolerance )
    {
        exponentialMapVector *= 48.0 / ( 24.0 - exponentialMapMagnitude * exponentialMapMagnitude );
    }
    else
    {
        exponentialMapVector *= shadowFlag ?
                    - ( 2.0 * PI - exponentialMapMagnitude ) / std::sin( 0.5 * exponentialMapMagnitude ) : 
                    exponentialMapMagnitude / std::sin( 0.5 * exponentialMapMagnitude ); 
    }
    convertedExponentialMapElements.segment( e1ExponentialMapIndex, 3 ) = exponentialMapVector;
    convertedExponentialMapElements( shadowFlagExponentialMapIndex ) = shadowFlag ? 1.0 : 0.0; 
    return convertedExponentialMapElements;
}
Eigen :: Vector4d convertQuaternionsToExponentialMapElements ( const Eigen :: Vector4d & quaternionElements ) { using mathematical_constants :: PI ; const double singularityTolerance = 20.0 * std :: numeric_limits < double > :: epsilon ( ) ; Eigen :: Vector4d convertedExponentialMapElements ; double exponentialMapMagnitude = 2.0 * std :: acos ( quaternionElements ( etaQuaternionIndex ) ) ; bool shadowFlag = std :: fabs ( exponentialMapMagnitude ) > PI ; Eigen :: Vector3d exponentialMapVector = quaternionElements . segment ( epsilon1QuaternionIndex , 3 ) ; if ( std :: fabs ( exponentialMapMagnitude ) < singularityTolerance ) { exponentialMapVector *= 48.0 / ( 24.0 - exponentialMapMagnitude * exponentialMapMagnitude ) ; } else { exponentialMapVector *= shadowFlag ? - ( 2.0 * PI - exponentialMapMagnitude ) / std :: sin ( 0.5 * exponentialMapMagnitude ) : exponentialMapMagnitude / std :: sin ( 0.5 * exponentialMapMagnitude ) ; } convertedExponentialMapElements . segment ( e1ExponentialMapIndex , 3 ) = exponentialMapVector ; convertedExponentialMapElements ( shadowFlagExponentialMapIndex ) = shadowFlag ? 1.0 : 0.0 ; return convertedExponentialMapElements ; }
Convert quaternions to exponential map.
Convert quaternions to exponential map .
// Define the tolerance of a singularity // Declare eventual output vector // Convert quaternions to exponential map (or SEM) // shadow exponential map // exponential map // set flag // Give output