preliminary version gravity deformation propagation.

include/tudat/astro/aerodynamics/exponentialAtmosphere.h

@@ -40,9 +40,9 @@ namespace aerodynamics
  */
 enum BodiesWithPredefinedExponentialAtmospheres
 {
-    undefined_body = -1,
-    earth = 0,
-    mars = 1
+    undefined_body_ea = -1,
+    earth_ea = 0,
+    mars_ea = 1
 };
 
 //! Exponential atmosphere class.

include/tudat/astro/basic_astro/accelerationModelTypes.h

@@ -30,6 +30,8 @@
 #include "tudat/astro/propulsion/massRateFromThrust.h"
 #include "tudat/astro/electromagnetism/radiationPressureAcceleration.h"
 #include "tudat/astro/electromagnetism/yarkovskyAcceleration.h"
+#include "tudat/astro/basic_astro/gravityDeformationModel.h"
+#include "tudat/simulation/propagation_setup/gravityDeformationSettings.h"
 
 namespace tudat
 {
@@ -113,6 +115,10 @@ AvailableAcceleration getAccelerationModelType(
 AvailableMassRateModels getMassRateModelType(
         const std::shared_ptr< MassRateModel > massRateModel );
 
+//! Function to identify the type of a gravity deformation model.
+simulation_setup::GravityDeformationType getGravityDeformationModelType(
+        const std::shared_ptr< basic_astrodynamics::GravityDeformationModel > gravityDeformationModel );
+
 // Function to get all acceleration models of a given type from a list of models
 /*
  * Function to get all acceleration models of a given type from a list of models

include/tudat/astro/propagators/dynamicsStateDerivativeModel.h

@@ -145,6 +145,7 @@ class DynamicsStateDerivativeModel
      */
     StateType computeStateDerivative( const TimeType time, const StateType& state )
     {
+        // std::cout << "in compute state derivative" << std::endl;
 
         if( !( time == time ) )
         {
@@ -521,6 +522,8 @@ class DynamicsStateDerivativeModel
                 break;
             case custom_state:
                 break;
+            case gravity_deformation_state:
+                break;
             default:
                 throw std::runtime_error( "Error when updating state derivative model settings, did not recognize dynamics type" );
                 break;

include/tudat/astro/propagators/gravityDerivative.h

@@ -33,7 +33,7 @@ namespace propagators
  *  Class for computing the derivative of the gravity field of a set of bodies. 
  */
 template< typename StateScalarType = double, typename TimeType = double >
-class GravityDerivative: public propagators::SingleStateTypeDerivative< StateScalarType, TimeType >
+class GravityStateDerivative: public propagators::SingleStateTypeDerivative< StateScalarType, TimeType >
 {
 public:
 
@@ -44,7 +44,7 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
      * \param massRateModels Map of model per body that is to be used for the gravity deformation computation.
      * \param bodiesToIntegrate List of bodies for which the gravity coefficients are to be propagated.
      */
-    GravityDerivative(
+    GravityStateDerivative(
             const std::map< std::string, std::shared_ptr< basic_astrodynamics::GravityDeformationModel > >& gravityDeformationModels,
             const std::vector< std::string >& bodiesToIntegrate ):
         propagators::SingleStateTypeDerivative< StateScalarType, TimeType >( propagators::gravity_deformation_state ),
@@ -76,7 +76,7 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
      * \param massRateModels Map of models per body that are to be used for the gravity deformation computation.
      * \param bodiesToIntegrate List of bodies for which the gravity coefficients are to be propagated. 
      */
-    GravityDerivative(
+    GravityStateDerivative(
             const std::map< std::string, std::vector< std::shared_ptr< basic_astrodynamics::GravityDeformationModel > > >&
             gravityDeformationModels,
             const std::vector< std::string >& bodiesToIntegrate ):
@@ -97,11 +97,11 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
 
 
     //! Destructor
-    virtual ~GravityDerivative( ){ }
+    virtual ~GravityStateDerivative( ){ }
 
-    //! Calculates the state derivative of the system of equations for the gravity field deformations
+    //! Calculates the state derivative of the system of equations for the gravity field coefficients
     /*!
-     * Calculates the state derivative of the system of equations for the gravity field deformations
+     * Calculates the state derivative of the system of equations for the gravity field coefficients
      * The environment and gravity deformation models (updateStateDerivativeModel) must be
      * updated before calling this function.
      * \param time Time at which the state derivative is to be calculated.
@@ -125,8 +125,9 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
             stateDerivative( currentIndex, 0 ) = 0.0;
             for( unsigned int i = 0; i < gravityDeformationModelIterator_->second.size( ); i++ )
             {
-                stateDerivative( currentIndex, 0 ) += static_cast< StateScalarType >(
-                            gravityDeformationModelIterator_->second.at ( i )->getDeformation( ) );
+                // gravityDeformationModelIterator_->second.at ( i )->updateMembers( );
+                stateDerivative.block( currentIndex * 3, 0, 3, 1 ) +=
+                            gravityDeformationModelIterator_->second.at ( i )->getDeformation( );
                 currentIndex++;
             }
 
@@ -179,25 +180,60 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
             const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time,
             Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton )
     {
-        currentCartesianLocalSoluton = internalSolution;
+        unsigned int counter = 0;
+        for ( auto it : gravityDeformationModels_ )
+        {
+            Eigen::Vector3d transientCoefficients = internalSolution.block( counter * 3, 0, 3, 1 );
+            std::shared_ptr< basic_astrodynamics::MaxwellGravityDeformationModel > maxwellModel = std::dynamic_pointer_cast< basic_astrodynamics::MaxwellGravityDeformationModel >( it.second.at( 0 ) );
+            double timeDouble = double( time );
+            currentCartesianLocalSoluton.block( 3*counter, 0, 3, 1 ) = maxwellModel->computeCurrentNominalCoefficients( transientCoefficients );    
+            counter += 1;
+        }
+        // this->convertToOutputSolution( internalSolution, time, currentCartesianLocalSoluton );
     }
 
     //! Function included for compatibility purposes with base class, input and output representation is equal for gravity 
     //! deformation rate model. Function returns input outputSolution.
-    virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution(
+    Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution(
             const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& outputSolution, const TimeType& time )
     {
-        return outputSolution;
+        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > nominalCoefficients = outputSolution;
+
+        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > transientCoefficients = Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( nominalCoefficients.size( ), 1 );
+
+        unsigned int counter = 0;
+        for ( auto it : gravityDeformationModels_ )
+        {
+            std::shared_ptr< basic_astrodynamics::MaxwellGravityDeformationModel > maxwellModel = std::dynamic_pointer_cast< basic_astrodynamics::MaxwellGravityDeformationModel >( it.second.at( 0 ) );
+            double timeDouble = double( time );
+            transientCoefficients.block( 3*counter, 0, 3, 1 ) = maxwellModel->computeTransientCoefficients( nominalCoefficients, timeDouble );    
+            counter += 1;
+        }
+
+        // std::cout << "in convertFromOutputSolution : transientCoefficients " << transientCoefficients.transpose( ) << std::endl;
+        return transientCoefficients;
     }
 
     //! Function included for compatibility purposes with base class, input and output representation is equal for gravity 
     //! deformation model. Function returns  (by reference) input internalSolution.
     void convertToOutputSolution(
             const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution,
             const TimeType& time,
-            Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton )
+            Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSolution )
     {
-        currentCartesianLocalSoluton = internalSolution;
+
+        unsigned int counter = 0;
+        for ( auto it : gravityDeformationModels_ )
+        {
+            Eigen::Vector3d transientCoefficients = internalSolution.block( counter * 3, 0, 3, 1 );
+            std::shared_ptr< basic_astrodynamics::MaxwellGravityDeformationModel > maxwellModel = std::dynamic_pointer_cast< basic_astrodynamics::MaxwellGravityDeformationModel >( it.second.at( 0 ) );
+            double timeDouble = double( time );
+            currentCartesianLocalSolution.block( 3*counter, 0, 3, 1 ) = maxwellModel->computeNominalCoefficients( transientCoefficients, timeDouble );    
+            counter += 1;
+        }
+
+        // std::cout << "in convertToOutputSolution : nominalCoefficients " << currentCartesianLocalSolution.transpose( ) << std::endl;
+        // currentCartesianLocalSolution = nominalCoefficients;
     }
 
     //! Function to get the total size of the state of propagated gravity coefficients.
@@ -206,9 +242,9 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
      * Equal to number of bodies for which the gravity coefficients are propagated.
      * \return Size of propagated gravity coefficients.
      */
-    virtual int getConventionalStateSize( )
+    int getConventionalStateSize( )
     {
-        return bodiesToIntegrate_.size( );
+        return 3 * bodiesToIntegrate_.size( );
     }
 
     //! Get list of bodies for which the gravity coefficients are to be propagated.
@@ -256,6 +292,11 @@ class GravityDerivative: public propagators::SingleStateTypeDerivative< StateSca
         return gravityDeformationModels_;
     }
 
+    bool isStateToBePostProcessed( )
+    {
+        return true;
+    }
+
 private:
 
     void verifyInput( )

include/tudat/astro/propagators/integrateEquations.h

@@ -563,6 +563,7 @@ void integrateEquationsFromIntegrator(
 
                 // Perform integration step.
                 newState = integrator->performIntegrationStep( timeStep );
+                std::cout << currentTime << " - new state " << newState.transpose( ) << std::endl;
                 if( statePostProcessingFunction != nullptr )
                 {
                     statePostProcessingFunction( newState );

include/tudat/simulation/environment_setup/body.h

@@ -639,6 +639,7 @@ class Body {
     template<typename StateScalarType = double, typename TimeType = double>
     void setStateFromEphemeris(const TimeType &time)
     {
+        std::cout << "set state from ephemeris " << time << std::endl;
         if (!(static_cast<Time>(time) == timeOfCurrentState_))
         {
             if( bodyEphemeris_ == nullptr )
@@ -1020,6 +1021,22 @@ class Body {
         }
     }
 
+    //! Templated function to get the current state of the body from its ephemeris and
+    //! global-to-ephemeris-frame function.
+    /*!
+     * Templated function to get the current state of the body from its ephemeris and
+     * global-to-ephemeris-frame function.  It calls the setStateFromEphemeris state, resetting the currentState_ /
+     * currentLongState_ variables, and returning the state with the requested precision
+     * \param time Time at which to evaluate states.
+     * \return State at requested time
+     */
+    template<typename TimeType = double>
+    Eigen::Quaterniond getRotationToBaseFrameFromEphemeris( const TimeType time )
+    {
+        setCurrentRotationalStateToLocalFrameFromEphemeris< TimeType >( time );
+        return currentRotationToGlobalFrame_;
+    }
+
     //! Get current rotational state.
     /*!
      *  Get current rotational state, expressed as a quaternion from global to body-fixed frame
@@ -1310,6 +1327,39 @@ class Body {
         gravityFieldVariationSet_ = gravityFieldVariationSet;
     }
 
+    void setCurrentPropagatedGravityField(const Eigen::VectorXd gravityCoefficients )
+    {
+        double C20 = gravityCoefficients[ 0 ];
+        double C22 = gravityCoefficients[ 1 ];
+        double S22 = gravityCoefficients[ 2 ];
+        // if( gravityFieldModel_ == nullptr )
+        // {
+        //     gravityFieldModel_ = std::make_shared< SphericalHarmonicsGravityField >( const double gravitationalParameter,
+        //     const double referenceRadius,
+        //     const Eigen::MatrixXd& cosineCoefficients = Eigen::MatrixXd::Identity( 1, 1 ),
+        //     const Eigen::MatrixXd& sineCoefficients = Eigen::MatrixXd::Zero( 1, 1 ) );
+        // }
+        // else
+        // {
+            // ADD WARNING!
+            std::shared_ptr< gravitation::SphericalHarmonicsGravityField > sphericalHarmonicsModel = 
+                std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >( gravityFieldModel_ );
+            if ( sphericalHarmonicsModel == nullptr )
+            {
+                throw std::runtime_error( "Error when setting current propagated gravity field, should be a "
+                    " spherical harmonics expansion model" );
+            }
+            Eigen::MatrixXd cosineCoefficients = sphericalHarmonicsModel->getCosineCoefficients( );
+            Eigen::MatrixXd sineCoefficients = sphericalHarmonicsModel->getSineCoefficients( );
+            cosineCoefficients( 2, 0 ) = C20;
+            cosineCoefficients( 2, 2 ) = C22;
+            sineCoefficients( 2, 2 ) = S22;
+            sphericalHarmonicsModel->setCosineCoefficients( cosineCoefficients );
+            sphericalHarmonicsModel->setSineCoefficients( sineCoefficients );
+
+        // }
+    }
+
     //! Function to get the gravity field model of the body.
     /*!
      *  Function to get the gravity field model of the body.

include/tudat/simulation/environment_setup/createAtmosphereModel.h

@@ -262,7 +262,7 @@ class ExponentialAtmosphereSettings: public AtmosphereSettings
         AtmosphereSettings( exponential_atmosphere ),
         densityScaleHeight_( densityScaleHeight ), constantTemperature_( constantTemperature ),
         densityAtZeroAltitude_( densityAtZeroAltitude ), specificGasConstant_( specificGasConstant ),
-        ratioOfSpecificHeats_( ratioOfSpecificHeats ), bodyWithPredefinedExponentialAtmosphere_( undefined_body )
+        ratioOfSpecificHeats_( ratioOfSpecificHeats ), bodyWithPredefinedExponentialAtmosphere_( undefined_body_ea )
     { }
 
     //  Default constructor.
@@ -280,8 +280,8 @@ class ExponentialAtmosphereSettings: public AtmosphereSettings
         // Check that the body name inserted is available
         switch ( bodyWithPredefinedExponentialAtmosphere )
         {
-        case earth:
-        case mars:
+        case earth_ea:
+        case mars_ea:
             // all is good
             break;
         default:
@@ -913,11 +913,11 @@ inline std::shared_ptr< AtmosphereSettings > exponentialAtmosphereSettings(
     BodiesWithPredefinedExponentialAtmospheres bodyId;
     if( bodyName == "Earth" )
     {
-        bodyId = BodiesWithPredefinedExponentialAtmospheres::earth;
+        bodyId = BodiesWithPredefinedExponentialAtmospheres::earth_ea;
     }
     else if( bodyName == "Mars" )
     {
-        bodyId = BodiesWithPredefinedExponentialAtmospheres::mars;
+        bodyId = BodiesWithPredefinedExponentialAtmospheres::mars_ea;
     }
     else
     {

include/tudat/simulation/propagation_setup/createEnvironmentUpdater.h

@@ -84,6 +84,18 @@ createMassPropagationEnvironmentUpdaterSettings(
         const std::map< std::string, std::vector< std::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels,
         const simulation_setup::SystemOfBodies& bodies );
 
+//! Get list of required environment model update settings from gravity deformation models.
+/*!
+ * Get list of required environment model update settings from gravity deformation models.
+ * \param massRateModels List of gravity deformation models used in simulation.
+ * \param bodies List of body objects used in the simulations.
+ * \return List of required environment model update settings.
+ */
+std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >
+createGravityPropagationEnvironmentUpdaterSettings(
+        const std::map< std::string, std::vector< std::shared_ptr< basic_astrodynamics::GravityDeformationModel > > > gravityDeformationModels,
+        const simulation_setup::SystemOfBodies& bodies );
+
 //! Function to update environment to allow all required updates to be made
 /*!
  * Function to update environment to allow all required updates to be made. It checks whether a flight conditions object needs to
@@ -218,6 +230,13 @@ std::vector< std::string > > createEnvironmentUpdaterSettings(
     {
         break;
     }
+    case gravity_deformation_state:
+    {
+        environmentModelsToUpdate = createGravityPropagationEnvironmentUpdaterSettings(
+                    std::dynamic_pointer_cast< GravityDeformationPropagatorSettings< StateScalarType, TimeType > >(
+                        propagatorSettings )->getGravityDeformationModelsMap( ), bodies );
+        break;
+    }
     default:
     {
         throw std::runtime_error( "Error, cannot create environment updates for type " +
@@ -245,6 +264,15 @@ std::vector< std::string > > createEnvironmentUpdaterSettings(
         checkValidityOfRequiredEnvironmentUpdates( environmentModelsToUpdate, bodies );
     }
 
+    for ( auto it : environmentModelsToUpdate )
+        { 
+                for ( unsigned int i = 0 ; i < it.second.size( ) ; i++  )
+                {
+                        std::cout << it.first << " - " << it.second.at( i ) << std::endl;
+                }
+        }                
+        std::cout << "environmentModelsToUpdate: " << environmentModelsToUpdate.size( ) << std::endl;
+
     return environmentModelsToUpdate;
 
 }