diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h b/Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h index 4873c4bc9d..0318289e37 100644 --- a/Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h @@ -184,6 +184,7 @@ class AerodynamicAcceleration : public basic_astrodynamics::AccelerationModel< E return computeAerodynamicAcceleration( 0.5 * currentDensity_ * currentAirspeed_ * currentAirspeed_, currentReferenceArea_, currentForceCoefficients_, currentMass_ ); + } //! Update member variables used by the aerodynamic acceleration model. diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h b/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h index bd405ffa99..ee00aaf652 100644 --- a/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h @@ -41,6 +41,7 @@ #ifndef TUDAT_AERODYNAMIC_COEFFICIENT_INTERFACE_H #define TUDAT_AERODYNAMIC_COEFFICIENT_INTERFACE_H +#include #include #include diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt b/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt index bfae978711..cfeeaec5c1 100755 --- a/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt +++ b/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt @@ -76,6 +76,7 @@ set(BASICASTRODYNAMICS_HEADERS "${SRCROOT}${BASICASTRODYNAMICSDIR}/bodyShapeModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/oblateSpheroidBodyShapeModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/sphericalBodyShapeModel.h" + "${SRCROOT}${BASICASTRODYNAMICSDIR}/massRateModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/unifiedStateModelElementConversions.h" ) diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h index d5a109f71a..3a689daf90 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h @@ -46,7 +46,7 @@ #define TUDAT_ACCELERATION_MODEL_H #include -#include +#include #include @@ -103,6 +103,11 @@ class AccelerationModel */ virtual void updateMembers( const double currentTime = TUDAT_NAN ) = 0; + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ virtual void resetTime( const double currentTime = TUDAT_NAN ) { currentTime_ = currentTime; @@ -110,6 +115,7 @@ class AccelerationModel protected: + //! Previous time to which acceleration model was updated. double currentTime_; protected: @@ -137,14 +143,16 @@ typedef boost::shared_ptr< AccelerationModel2d > AccelerationModel2dPointer; * \tparam AccelerationDataType Data type used to represent accelerations * (default=Eigen::Vector3d). * \param accelerationModel Acceleration model that is to be evaluated. + * \param currentTime Time at which acceleration model is to be updated. * \return Acceleration that is obtained following the member update. */ template < typename AccelerationDataType > AccelerationDataType updateAndGetAcceleration( - boost::shared_ptr< AccelerationModel< AccelerationDataType > > accelerationModel ) + const boost::shared_ptr< AccelerationModel< AccelerationDataType > > accelerationModel, + const double currentTime = TUDAT_NAN ) { // Update members. - accelerationModel->updateMembers( ); + accelerationModel->updateMembers( currentTime ); // Evaluate and return acceleration. return accelerationModel->getAcceleration( ); @@ -152,14 +160,14 @@ AccelerationDataType updateAndGetAcceleration( //! Typedef defining a list of accelerations acting on a single body, key is the name of each //! body exerting a acceletation, value is a list of accelerations exerted by that body. -typedef std::map< std::string, std::vector< +typedef std::unordered_map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > > SingleBodyAccelerationMap; //! Typedef defining a list of accelerations acting on a set of bodies, key is the name of each //! body undergoing a acceletation, value is SingleBodyAccelerationMap, defining all accelerations //! acting on it. -typedef std::map< std::string, SingleBodyAccelerationMap > AccelerationMap; +typedef std::unordered_map< std::string, SingleBodyAccelerationMap > AccelerationMap; } // namespace basic_astrodynamics } // namespace tudat diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp index d7d8649a22..8e94439a5b 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp @@ -89,6 +89,27 @@ AvailableAcceleration getAccelerationModelType( } +//! Function to identify the type of a mass rate model. +AvailableMassRateModels getMassRateModelType( + const boost::shared_ptr< MassRateModel > massRateModel ) +{ + // Nominal type is undefined + AvailableMassRateModels massRateType = undefined_mass_rate_model; + + // Check for each mass rate mdoel type implemented as AvailableMassRateModels. + if( boost::dynamic_pointer_cast< basic_astrodynamics::CustomMassRateModel >( + massRateModel ) != NULL ) + { + massRateType = custom; + } + else + { + throw std::runtime_error( + "Error, mass rate model not identified when getting mass rate model type." ); + } + return massRateType; +} + } // namespace basic_astrodynamics diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h index 83b6520f65..2809a7eda6 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h @@ -40,6 +40,7 @@ #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" #include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" namespace tudat @@ -65,6 +66,17 @@ enum AvailableAcceleration third_body_spherical_harmonic_gravity }; +//! List of model types for body mass rates. +/*! +* List of model types for body mass rates available in simulations. Mass rate models not defined by this +* given enum cannot be used for automatic mass rate model setup. +*/ +enum AvailableMassRateModels +{ + undefined_mass_rate_model, + custom +}; + //! Function to identify the derived class type of an acceleration model. /*! * Function to identify the derived class type of an acceleration model. The type must be defined @@ -76,6 +88,16 @@ AvailableAcceleration getAccelerationModelType( const boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel ); +//! Function to identify the type of a mass rate model. +/*! + * Function to identify the type of a mass rate model. The type must be defined + * in the AvailableMassRateModels enum to be recognized by this function. + * \param massRateModel Mass rate model of which the type is to be identified. + * \return Type of the massRateModel, as identified by AvailableMassRateModels enum. + */ +AvailableMassRateModels getMassRateModelType( + const boost::shared_ptr< MassRateModel > massRateModel ); + } // namespace basic_astrodynamics diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h b/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h new file mode 100644 index 0000000000..5cb77d083e --- /dev/null +++ b/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h @@ -0,0 +1,136 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_MASSRATEMODEL_H +#define TUDAT_MASSRATEMODEL_H + +#include +#include +#include +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" + +namespace tudat +{ +namespace basic_astrodynamics +{ + +//! Base class for determining the rate of change of a body's mass, to be used in numerical integration. +/*! + * Base class for determining the rate of change of a body's mass, to be used in numerical integration. Specific types + * of mass rate models are to be implemented in derived classes + */ +class MassRateModel +{ +public: + + //! Constructor + MassRateModel( ): + currentTime_( TUDAT_NAN ), currentMassRate_( TUDAT_NAN ){ } + + //! Destructor + virtual ~MassRateModel( ) { } + + //! Function to retrieve current mass rate + /*! + * Function to retrieve current mass rate, as set by last call to updateMembers (implemented in derived class) + * \return Current mass rate + */ + virtual double getMassRate( ) + { + return currentMassRate_; + } + + //! Update member variables used by the mass rate model, and internally compute mass rate. + /*! + * Updates member variables used by the mass rate model, and internally compute mass rate. In the case of mass rate + * models containing varying parameters, function-pointers returning such a parameter (for instance + * the Cartesian state of a body) will be set as a member variable. + * This function evaluates such function-pointers and updates member variables to the 'current' + * values of these parameters. Only these current values, not the function-pointers are then + * used for the actual computation function. + * \param currentTime Time at which acceleration model is to be updated. + */ + virtual void updateMembers( const double currentTime = TUDAT_NAN ) = 0; + + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + virtual void resetTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + } + +protected: + + //! Previous time to which mass rate model was updated. + double currentTime_; + + //! Current mass rate, as set by last call to updateMembers (implemented in derived class) + double currentMassRate_; + +private: +}; + +//! Derived class for determining the rate of change of a body's mass, user-defined by a function pointer. +/*! + * Derived class for determining the rate of change of a body's mass, user-defined by a function pointer. + * This class can be used for any kind of mass rate model for which the user can define the dependency as a function + * of time. + */ +class CustomMassRateModel: public MassRateModel +{ +public: + + //! Constructor. + /*! + * Constructor + * \param massRateFunction Function returning mass rate as a function of time. + */ + CustomMassRateModel( + const boost::function< double( const double ) > massRateFunction ): + massRateFunction_( massRateFunction ){ } + + //! Destructor. + ~CustomMassRateModel( ){ } + + //! Update member variables used by the mass rate model and compute the mass rate + /*! + * Update member variables used by the mass rate model and compute the mass rate + * \param currentTime Time at which acceleration model is to be updated. + */ + void updateMembers( const double currentTime = TUDAT_NAN ) + { + // Check if update is needed. + if( !( currentTime_ == currentTime ) ) + { + currentMassRate_ = massRateFunction_( currentTime ); + } + } + +private: + + //! Function returning mass rate as a function of time. + boost::function< double( const double ) > massRateFunction_; + +}; + + +} // namespace basic_astrodynamics + +} // namespace tudat + +#endif // TUDAT_MASSRATEMODEL_H diff --git a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp index 80062f73b2..bac2c14b05 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp +++ b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp @@ -57,27 +57,5 @@ Eigen::Vector3d computeCannonBallRadiationPressureAcceleration( radiationPressure, vectorToSource, area, radiationPressureCoefficient ) / mass; } -//! Get radiation pressure acceleration. -Eigen::Vector3d CannonBallRadiationPressureAcceleration::getAcceleration( ) -{ - return computeCannonBallRadiationPressureAcceleration( - currentRadiationPressure_, currentVectorToSource_, currentArea_, - currentRadiationPressureCoefficient_, currentMass_ ); -} - -//! Update member variables used by the radiation pressure acceleration model. -void CannonBallRadiationPressureAcceleration::updateMembers( const double currentTime ) -{ - if( !( this->currentTime_ == currentTime ) ) - { - currentVectorToSource_ = ( sourcePositionFunction_( ) - - acceleratedBodyPositionFunction_( ) ).normalized( ); - currentRadiationPressure_ = radiationPressureFunction_( ); - currentRadiationPressureCoefficient_ = radiationPressureCoefficientFunction_( ); - currentArea_ = areaFunction_( ); - currentMass_ = massFunction_( ); - } -} - } // namespace electro_magnetism } // namespace tudat diff --git a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h index ca7c9d4dd9..77b90fbb26 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h +++ b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h @@ -172,7 +172,12 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel * \return Radiation pressure acceleration. * \sa computeCannonBallRadiationPressureAcceleration(). */ - Eigen::Vector3d getAcceleration( ); + Eigen::Vector3d getAcceleration( ) + { + return computeCannonBallRadiationPressureAcceleration( + currentRadiationPressure_, currentVectorToSource_, currentArea_, + currentRadiationPressureCoefficient_, currentMass_ ); + } //! Update member variables used by the radiation pressure acceleration model. /*! @@ -181,7 +186,28 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel * not the function-pointers are then used by the getAcceleration( ) function. * \param currentTime Time at which acceleration model is to be updated. */ - void updateMembers( const double currentTime = TUDAT_NAN ); + void updateMembers( const double currentTime = TUDAT_NAN ) + { + if( !( this->currentTime_ == currentTime ) ) + { + currentVectorToSource_ = ( sourcePositionFunction_( ) + - acceleratedBodyPositionFunction_( ) ).normalized( ); + currentRadiationPressure_ = radiationPressureFunction_( ); + currentRadiationPressureCoefficient_ = radiationPressureCoefficientFunction_( ); + currentArea_ = areaFunction_( ); + currentMass_ = massFunction_( ); + } + } + + //! Function to retrieve the function pointer returning mass of accelerated body. + /*! + * Function to retrieve the function pointer returning mass of accelerated body. + * \return Function pointer returning mass of accelerated body. + */ + boost::function< double( ) > getMassFunction( ) + { + return massFunction_; + } private: diff --git a/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h b/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h index 8d96d9ad13..b800bea79d 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h +++ b/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h @@ -175,6 +175,16 @@ class RadiationPressureInterface{ return radiationPressureCoefficient_; } + //! Function to reset the radiation pressure coefficient of the target body. + /*! + * Function to reset the radiation pressure coefficient of the target body. + * \param radiationPressureCoefficient The new radiation pressure coefficient of the target body. + */ + void resetRadiationPressureCoefficient( const double radiationPressureCoefficient ) + { + radiationPressureCoefficient_ = radiationPressureCoefficient; + } + //! Function to return the function returning the current total power (in W) emitted by the //! source body. /*! diff --git a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp index 7e422b9e01..21fd962a1f 100644 --- a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp +++ b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp @@ -67,7 +67,7 @@ BOOST_AUTO_TEST_CASE( testCompositeEphemeris ) double buffer = 5.0 * maximumTimeStep; // Create bodies needed in simulation - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( + NamedBodyMap bodyMap = createBodies( getDefaultBodySettings( bodyNames,initialEphemerisTime - buffer, finalEphemerisTime + buffer ) ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); diff --git a/Tudat/Astrodynamics/Gravitation/CMakeLists.txt b/Tudat/Astrodynamics/Gravitation/CMakeLists.txt index fbcd824a97..a64ca55237 100644 --- a/Tudat/Astrodynamics/Gravitation/CMakeLists.txt +++ b/Tudat/Astrodynamics/Gravitation/CMakeLists.txt @@ -120,7 +120,7 @@ target_link_libraries(test_SphericalHarmonicsGravityModel tudat_gravitation tuda add_executable(test_ThirdBodyPerturbation "${SRCROOT}${GRAVITATIONDIR}/UnitTests/unitTestThirdBodyPerturbation.cpp") setup_custom_test_program(test_ThirdBodyPerturbation "${SRCROOT}${GRAVITATIONDIR}") -target_link_libraries(test_ThirdBodyPerturbation tudat_gravitation ${Boost_LIBRARIES} ) +target_link_libraries(test_ThirdBodyPerturbation tudat_gravitation tudat_basic_mathematics ${Boost_LIBRARIES} ) if(USE_CSPICE) add_executable(test_GravityFieldVariations "${SRCROOT}${GRAVITATIONDIR}/UnitTests/unitTestGravityFieldVariations.cpp") diff --git a/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp index 46c172439c..ba8e0faf26 100644 --- a/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp @@ -57,6 +57,7 @@ #include "Tudat/Basics/testMacros.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" namespace tudat { @@ -97,7 +98,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo1 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 1 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -140,7 +142,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo2 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 2 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -183,7 +186,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo3 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 3 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -240,7 +244,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo4 ) gravitationalParameter, planetaryRadius, cosineCoefficients, - sineCoefficients ); + sineCoefficients, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 6, 6 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. diff --git a/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp index 35a57a119d..e47d18abc1 100644 --- a/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp @@ -60,10 +60,10 @@ Eigen::Vector3d computeGravitationalAcceleration( const double gravitationalParameterOfBodyExertingAcceleration, const Eigen::Vector3d& positionOfBodyExertingAcceleration ) { + double distance = ( positionOfBodySubjectToAcceleration - positionOfBodyExertingAcceleration ).norm( ); return -gravitationalParameterOfBodyExertingAcceleration * ( positionOfBodySubjectToAcceleration - positionOfBodyExertingAcceleration ) - / std::pow( ( positionOfBodySubjectToAcceleration - - positionOfBodyExertingAcceleration ).norm( ), 3.0 ); + / ( distance * distance * distance ); } //! Compute gravitational force. diff --git a/Tudat/Astrodynamics/Gravitation/centralGravityModel.h b/Tudat/Astrodynamics/Gravitation/centralGravityModel.h index 82c4558c50..c0a8ce77ba 100644 --- a/Tudat/Astrodynamics/Gravitation/centralGravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralGravityModel.h @@ -192,15 +192,21 @@ class CentralGravitationalAccelerationModel * \param aGravitationalParameter A (constant) gravitational parameter [m^2 s^-3]. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration (default = (0,0,0)). + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ CentralGravitationalAccelerationModel( const typename Base::StateFunction positionOfBodySubjectToAccelerationFunction, const double aGravitationalParameter, const typename Base::StateFunction positionOfBodyExertingAccelerationFunction - = boost::lambda::constant( StateMatrix::Zero( ) ) ) + = boost::lambda::constant( StateMatrix::Zero( ) ), + const bool isMutualAttractionUsed = 0 ) : Base( positionOfBodySubjectToAccelerationFunction, boost::lambda::constant( aGravitationalParameter ), - positionOfBodyExertingAccelerationFunction ) + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ) { this->updateMembers( ); } @@ -220,15 +226,21 @@ class CentralGravitationalAccelerationModel * parameter [m^2 s^-3]. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration (default = (0,0,0)). + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ CentralGravitationalAccelerationModel( const typename Base::StateFunction positionOfBodySubjectToAccelerationFunction, const boost::function< double( ) > aGravitationalParameterFunction, const typename Base::StateFunction positionOfBodyExertingAccelerationFunction - = boost::lambda::constant( StateMatrix::Zero( ) ) ) + = boost::lambda::constant( StateMatrix::Zero( ) ), + const bool isMutualAttractionUsed = 0 ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameterFunction, - positionOfBodyExertingAccelerationFunction ) + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ) { this->updateMembers( ); } diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h index 460f2ec9f2..f97e7e28e3 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h @@ -137,7 +137,7 @@ class CentralJ2GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, 0 ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ) { diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h index 9d02f5b92d..49dc922c9b 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h @@ -142,7 +142,7 @@ class CentralJ2J3GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, 0 ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ), j3GravityCoefficient( aJ3GravityCoefficient ) diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h index 67ecfd771c..804155b421 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h @@ -191,7 +191,7 @@ class CentralJ2J3J4GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, 0 ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ), j3GravityCoefficient( aJ3GravityCoefficient ), diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp index f038a951a7..c3a2cf0383 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp @@ -51,7 +51,7 @@ #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" - +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" namespace tudat { @@ -61,8 +61,11 @@ namespace gravitation //! Function to calculate the gravitational potential from a spherical harmonic field expansion. double calculateSphericalHarmonicGravitationalPotential( const Eigen::Vector3d& bodyFixedPosition, const double gravitationalParameter, - const double referenceRadius, const Eigen::MatrixXd& cosineCoefficients, - const Eigen::MatrixXd& sineCoefficients, const int minimumumDegree, + const double referenceRadius, + const Eigen::MatrixXd& cosineCoefficients, + const Eigen::MatrixXd& sineCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const int minimumumDegree, const int minimumumOrder ) { // Initialize (distance/reference radius)^n (n=ratioToPowerDegree) @@ -91,9 +94,12 @@ double calculateSphericalHarmonicGravitationalPotential( else { startDegree = minimumumDegree; - ratioToPowerDegree *= std::pow( radiusRatio, startDegree - 1 ); + ratioToPowerDegree *= basic_mathematics::raiseToIntegerPower< double >( radiusRatio, startDegree - 1 ); } + basic_mathematics::LegendreCache& legendreCacheReference = *sphericalHarmonicsCache->getLegendreCache( ); + legendreCacheReference.update( std::sin( latitude ) ); + // Iterate over all degrees for( int degree = startDegree; degree < cosineCoefficients.rows( ); degree++ ) { @@ -105,7 +111,7 @@ double calculateSphericalHarmonicGravitationalPotential( { // Calculate legendre polynomial (geodesy-normalized) at current degree and order legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, std::sin( latitude ) ); + degree, order, legendreCacheReference ); // Calculate contribution to potential from current degree and order singleDegreeTerm += legendrePolynomial * ( cosineCoefficients( degree, order ) * diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h index a36dc33d6b..16d7498573 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h @@ -79,6 +79,8 @@ namespace gravitation * \param referenceRadius Reference radius of spherical harmonic field expansion. * \param cosineCoefficients Cosine spherical harmonic coefficients (geodesy normalized). * \param sineCoefficients Sine spherical harmonic coefficients (geodesy normalized). + * \param sphericalHarmonicsCache Cache object containing current values of trigonometric funtions of latitude anf longitude, + * as well as legendre polynomials at current state. * \param minimumumDegree Maximum degree of spherical harmonic expansion. * \param minimumumOrder Maximum order of spherical harmonic expansion. @@ -88,6 +90,7 @@ double calculateSphericalHarmonicGravitationalPotential( const Eigen::Vector3d& bodyFixedPosition, const double gravitationalParameter, const double referenceRadius, const Eigen::MatrixXd& cosineCoefficients, const Eigen::MatrixXd& sineCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, const int minimumumDegree = 0, const int minimumumOrder = 0 ); //! Class to represent a spherical harmonic gravity field expansion. @@ -118,7 +121,11 @@ class SphericalHarmonicsGravityField: public GravityFieldModel : GravityFieldModel( gravitationalParameter ), referenceRadius_( referenceRadius ), cosineCoefficients_( cosineCoefficients ), sineCoefficients_( sineCoefficients ), fixedReferenceFrame_( fixedReferenceFrame ) - { } + { + sphericalHarmonicsCache_ = boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ); + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( cosineCoefficients_.rows( ) + 1, + cosineCoefficients_.cols( ) + 1 ); + } //! Virtual destructor. /*! @@ -263,6 +270,7 @@ class SphericalHarmonicsGravityField: public GravityFieldModel bodyFixedPosition, gravitationalParameter_, referenceRadius_, cosineCoefficients_.block( 0, 0, maximumDegree + 1, maximumOrder + 1 ), sineCoefficients_.block( 0, 0, maximumDegree + 1, maximumOrder + 1 ), + sphericalHarmonicsCache_, minimumDegree, minimumOrder ); } @@ -277,7 +285,7 @@ class SphericalHarmonicsGravityField: public GravityFieldModel Eigen::Vector3d getGradientOfPotential( const Eigen::Vector3d& bodyFixedPosition ) { return getGradientOfPotential( bodyFixedPosition, cosineCoefficients_.rows( ), - sineCoefficients_.cols( ) ); + sineCoefficients_.cols( ) ); } //! Get the gradient of the potential. @@ -289,13 +297,13 @@ class SphericalHarmonicsGravityField: public GravityFieldModel * \return Gradient of potential. */ Eigen::Vector3d getGradientOfPotential( const Eigen::Vector3d& bodyFixedPosition, - const double maximumDegree, - const double maximumOrder ) + const double maximumDegree, + const double maximumOrder ) { return computeGeodesyNormalizedGravitationalAccelerationSum( bodyFixedPosition, gravitationalParameter_, referenceRadius_, cosineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ), - sineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ) ); + sineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ), sphericalHarmonicsCache_ ); } //! Function to retrieve the tdentifier for body-fixed reference frame @@ -333,6 +341,9 @@ class SphericalHarmonicsGravityField: public GravityFieldModel * Identifier for body-fixed reference frame */ std::string fixedReferenceFrame_; + + //! Cache object for potential calculations. + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache_; }; } // namespace gravitation diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp index 88cbd4a66a..391218c665 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp @@ -64,7 +64,8 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( const double gravitationalParameter, const double equatorialRadius, const Eigen::MatrixXd& cosineHarmonicCoefficients, - const Eigen::MatrixXd& sineHarmonicCoefficients ) + const Eigen::MatrixXd& sineHarmonicCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) { // Set highest degree and order. const int highestDegree = cosineHarmonicCoefficients.rows( ); @@ -111,6 +112,13 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( // Compute longitude coordinate. sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 ); + double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ); + sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ), + sineOfAngle, + sphericalpositionOfBodySubjectToAcceleration( 2 ), + equatorialRadius ); + basic_mathematics::LegendreCache& legendreCacheReference = *( sphericalHarmonicsCache->getLegendreCache( ) ); + // Compute gradient premultiplier. const double preMultiplier = gravitationalParameter / equatorialRadius; @@ -121,35 +129,32 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( for ( int degree = 0; degree < highestDegree; degree++ ) { // Loop through all orders. - for ( int order = 0; order <= degree && order < highestOrder; order++ ) + for ( int order = 0; ( order <= degree ) && ( order < highestOrder ); order++ ) { // Compute geodesy-normalized Legendre polynomials. const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + degree, order, legendreCacheReference ); const double incrementedLegendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order + 1, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + degree, order + 1, legendreCacheReference ); // Compute geodesy-normalized Legendre polynomial derivative. const double legendrePolynomialDerivative = basic_mathematics::computeGeodesyLegendrePolynomialDerivative( degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ), + sineOfAngle, legendrePolynomial, incrementedLegendrePolynomial ); // Compute the potential gradient of a single spherical harmonic term. sphericalGradient += basic_mathematics::computePotentialGradient( sphericalpositionOfBodySubjectToAcceleration, - equatorialRadius, preMultiplier, degree, order, cosineHarmonicCoefficients( degree, order ), sineHarmonicCoefficients( degree, order ), legendrePolynomial, - legendrePolynomialDerivative ); + legendrePolynomialDerivative, sphericalHarmonicsCache ); } } @@ -167,7 +172,8 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( const int degree, const int order, const double cosineHarmonicCoefficient, - const double sineHarmonicCoefficient ) + const double sineHarmonicCoefficient, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) { // Declare spherical position vector. Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration; @@ -210,35 +216,41 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( // Compute longitude coordinate. sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 ); + + double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ); + sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ), + sineOfAngle, + sphericalpositionOfBodySubjectToAcceleration( 2 ), + equatorialRadius ); + basic_mathematics::LegendreCache& legendreCacheReference = *( sphericalHarmonicsCache->getLegendreCache( ) ); + // Compute gradient premultiplier. const double preMultiplier = gravitationalParameter / equatorialRadius; // Compute geodesy-normalized Legendre polynomials. const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + degree, order, legendreCacheReference ); const double incrementedLegendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order + 1, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + degree, order + 1, legendreCacheReference ); - // Compute geodesy-normalized Legendre polynomial derivative. + // Compute geodesy-normalized Legendre polynomial derivative.x const double legendrePolynomialDerivative = basic_mathematics::computeGeodesyLegendrePolynomialDerivative( degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ), legendrePolynomial, + sineOfAngle, legendrePolynomial, incrementedLegendrePolynomial ); // Compute the potential gradient resulting from the spherical harmonic term. const Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient( sphericalpositionOfBodySubjectToAcceleration, - equatorialRadius, preMultiplier, degree, order, cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial, - legendrePolynomialDerivative ); + legendrePolynomialDerivative, sphericalHarmonicsCache ); // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector), // and return resulting acceleration vector. diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h index c965473664..53692f9c1f 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h @@ -49,12 +49,14 @@ #include #include #include +#include #include #include #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h" +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" namespace tudat { @@ -88,15 +90,17 @@ namespace gravitation * position( 0 ) = x coordinate [m], * position( 1 ) = y coordinate [m], * position( 2 ) = z coordinate [m]. + * \param gravitationalParameter Gravitational parameter associated with the spherical harmonics + * [m^3 s^-2]. + * \param equatorialRadius Reference radius of the spherical harmonics [m]. * \param cosineHarmonicCoefficients Matrix with geodesy-normalized cosine harmonic * coefficients. The row index indicates the degree and the column index indicates the order * of coefficients. * \param sineHarmonicCoefficients Matrix with geodesy-normalized sine harmonic coefficients. * The row index indicates the degree and the column index indicates the order of * coefficients. The matrix must be equal in size to cosineHarmonicCoefficients. - * \param gravitationalParameter Gravitational parameter associated with the spherical harmonics - * [m^3 s^-2]. - * \param equatorialRadius Reference radius of the spherical harmonics [m]. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. * \return Cartesian acceleration vector resulting from the summation of all harmonic terms. * The order is important! * acceleration( 0 ) = x acceleration [m s^-2], @@ -108,7 +112,8 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( const double gravitationalParameter, const double equatorialRadius, const Eigen::MatrixXd& cosineHarmonicCoefficients, - const Eigen::MatrixXd& sineHarmonicCoefficients ); + const Eigen::MatrixXd& sineHarmonicCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); //! Compute gravitational acceleration due to single spherical harmonics term. /*! @@ -137,12 +142,14 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( * position( 2 ) = z coordinate [m]. * \param degree Degree of the harmonic term. * \param order Order of the harmonic term. - * \param cosineHarmonicCoefficient Geodesy-normalized cosine harmonic + * * \param cosineHarmonicCoefficient Geodesy-normalized cosine harmonic * coefficient. * \param sineHarmonicCoefficient Geodesy-normalized sine harmonic coefficient. * \param gravitationalParameter Gravitational parameter associated with the spherical harmonic * [m^3 s^-2]. * \param equatorialRadius Reference radius of the spherical harmonic [m]. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. * \return Cartesian acceleration vector resulting from the spherical harmonic term. * The order is important! * acceleration( 0 ) = x acceleration [m s^-2], @@ -156,7 +163,8 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( const int degree, const int order, const double cosineHarmonicCoefficient, - const double sineHarmonicCoefficient ); + const double sineHarmonicCoefficient, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); //! Template class for general spherical harmonics gravitational acceleration model. /*! @@ -205,6 +213,12 @@ class SphericalHarmonicsGravitationalAccelerationModel * body exerting gravitational acceleration (default = (0,0,0)). * \param rotationFromBodyFixedToIntegrationFrameFunction Function providing the rotation from * body-fixes from to the frame in which the numerical integration is performed. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. */ SphericalHarmonicsGravitationalAccelerationModel( const StateFunction positionOfBodySubjectToAccelerationFunction, @@ -216,17 +230,25 @@ class SphericalHarmonicsGravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ), const boost::function< Eigen::Quaterniond( ) > rotationFromBodyFixedToIntegrationFrameFunction = - boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ) ) + boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), + const bool isMutualAttractionUsed = 0, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ), equatorialRadius( anEquatorialRadius ), getCosineHarmonicsCoefficients( boost::lambda::constant(aCosineHarmonicCoefficientMatrix ) ), getSineHarmonicsCoefficients( boost::lambda::constant(aSineHarmonicCoefficientMatrix ) ), rotationFromBodyFixedToIntegrationFrameFunction_( - rotationFromBodyFixedToIntegrationFrameFunction ) + rotationFromBodyFixedToIntegrationFrameFunction ), + sphericalHarmonicsCache_( sphericalHarmonicsCache ) { + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).rows( ) ), sphericalHarmonicsCache_->getMaximumDegree( ) ), + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).cols( ) ), sphericalHarmonicsCache_->getMaximumOrder( ) ) ); this->updateMembers( ); } @@ -251,6 +273,12 @@ class SphericalHarmonicsGravitationalAccelerationModel * body exerting gravitational acceleration (default = (0,0,0)). * \param rotationFromBodyFixedToIntegrationFrameFunction Function providing the rotation from * body-fixes from to the frame in which the numerical integration is performed. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. */ SphericalHarmonicsGravitationalAccelerationModel( const StateFunction positionOfBodySubjectToAccelerationFunction, @@ -262,15 +290,25 @@ class SphericalHarmonicsGravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ), const boost::function< Eigen::Quaterniond( ) > rotationFromBodyFixedToIntegrationFrameFunction = - boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ) ) + boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), + const bool isMutualAttractionUsed = 0, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache = boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameterFunction, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ), equatorialRadius( anEquatorialRadius ), getCosineHarmonicsCoefficients( cosineHarmonicCoefficientsFunction ), getSineHarmonicsCoefficients( sineHarmonicCoefficientsFunction ), - rotationFromBodyFixedToIntegrationFrameFunction_( rotationFromBodyFixedToIntegrationFrameFunction ) + rotationFromBodyFixedToIntegrationFrameFunction_( rotationFromBodyFixedToIntegrationFrameFunction ), + + sphericalHarmonicsCache_( sphericalHarmonicsCache ) { + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).rows( ) ), sphericalHarmonicsCache_->getMaximumDegree( ) ), + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).cols( ) ), sphericalHarmonicsCache_->getMaximumOrder( ) ) ); + + this->updateMembers( ); } @@ -300,6 +338,16 @@ class SphericalHarmonicsGravitationalAccelerationModel } } + //! Function to retrieve the spherical harmonics cache for this acceleration. + /*! + * Function to retrieve the spherical harmonics cache for this acceleration. + * \return Spherical harmonics cache for this acceleration + */ + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > getSphericalHarmonicsCache( ) + { + return sphericalHarmonicsCache_; + } + protected: private: @@ -342,6 +390,8 @@ class SphericalHarmonicsGravitationalAccelerationModel //! Current rotation from body-fixed frame to integration frame. Eigen::Quaterniond rotationToIntegrationFrame_; + //! Spherical harmonics cache for this acceleration + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache_; }; //! Typedef for SphericalHarmonicsGravitationalAccelerationModelXd. @@ -361,13 +411,18 @@ template< typename CoefficientMatrixType > Eigen::Vector3d SphericalHarmonicsGravitationalAccelerationModel< CoefficientMatrixType > ::getAcceleration( ) { - return rotationToIntegrationFrame_ * computeGeodesyNormalizedGravitationalAccelerationSum( - rotationToIntegrationFrame_.inverse( ) * - ( this->positionOfBodySubjectToAcceleration - this->positionOfBodyExertingAcceleration ), - gravitationalParameter, equatorialRadius, cosineHarmonicCoefficients, sineHarmonicCoefficients ); + return rotationToIntegrationFrame_ * + computeGeodesyNormalizedGravitationalAccelerationSum( + rotationToIntegrationFrame_.inverse( ) * ( + this->positionOfBodySubjectToAcceleration - this->positionOfBodyExertingAcceleration ), + gravitationalParameter, + equatorialRadius, + cosineHarmonicCoefficients, + sineHarmonicCoefficients, sphericalHarmonicsCache_ ); } } // namespace gravitation + } // namespace tudat #endif // TUDAT_SPHERICAL_HARMONICS_GRAVITY_MODEL_H diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h index ba07f8f811..8c7efe26c6 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h @@ -75,14 +75,20 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * \param aGravitationalParameter Gravitational parameter of body exerting gravitational acceleration. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ SphericalHarmonicsGravitationalAccelerationModelBase( const StateFunction positionOfBodySubjectToAccelerationFunction, const double aGravitationalParameter, - const StateFunction positionOfBodyExertingAccelerationFunction ) + const StateFunction positionOfBodyExertingAccelerationFunction, + const bool isMutualAttractionUsed ) : subjectPositionFunction( positionOfBodySubjectToAccelerationFunction ), gravitationalParameterFunction( boost::lambda::constant( aGravitationalParameter ) ), - sourcePositionFunction( positionOfBodyExertingAccelerationFunction ) + sourcePositionFunction( positionOfBodyExertingAccelerationFunction ), + isMutualAttractionUsed_( isMutualAttractionUsed ) { } //! Default constructor taking position of body subject to acceleration, variable @@ -99,14 +105,20 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * of body exerting gravitational acceleration. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ SphericalHarmonicsGravitationalAccelerationModelBase( const StateFunction positionOfBodySubjectToAccelerationFunction, const boost::function< double( ) > aGravitationalParameterFunction, - const StateFunction positionOfBodyExertingAccelerationFunction ) + const StateFunction positionOfBodyExertingAccelerationFunction, + const bool isMutualAttractionUsed ) : subjectPositionFunction( positionOfBodySubjectToAccelerationFunction ), gravitationalParameterFunction( aGravitationalParameterFunction ), - sourcePositionFunction( positionOfBodyExertingAccelerationFunction ) + sourcePositionFunction( positionOfBodyExertingAccelerationFunction ), + isMutualAttractionUsed_( isMutualAttractionUsed ) { } //! Virtual destructor. @@ -123,12 +135,45 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * \return True; this should be modified to return a flag indicating if the update was * successful. */ - bool updateBaseMembers( ) + void updateBaseMembers( ) { this->gravitationalParameter = this->gravitationalParameterFunction( ); this->positionOfBodySubjectToAcceleration = this->subjectPositionFunction( ); this->positionOfBodyExertingAcceleration = this->sourcePositionFunction( ); - return true; + } + + //! Function to return the function returning the relevant gravitational parameter. + /*! + * Function to return the function returning the relevant gravitational parameter. + * \return Function returning the gravitational parameter used in the computations. + */ + boost::function< double( ) > getGravitationalParameterFunction( ) + { return gravitationalParameterFunction; } + + //! Function to return the function returning position of body exerting acceleration. + /*! + * Function to return the function returning position of body exerting acceleration. + * \return Function returning position of body exerting acceleration. + */ + boost::function< StateMatrix( ) > getStateFunctionOfBodyExertingAcceleration( ) + { return sourcePositionFunction; } + + //! Function to return the function returning position of body subject to acceleration. + /*! + * Function to return the function returning position of body subject to acceleration. + * \return Function returning position of body subject to acceleration. + */ + boost::function< StateMatrix( ) > getStateFunctionOfBodyUndergoingAcceleration( ) + { return subjectPositionFunction; } + + //! Function to return whether the mutual attraction is used. + /*! + * Function to return whether the mutual attraction is used. + * \return Boolean defining whether the mutual attraction is used. + */ + bool getIsMutualAttractionUsed( ) + { + return isMutualAttractionUsed_; } protected: @@ -171,6 +216,16 @@ class SphericalHarmonicsGravitationalAccelerationModelBase */ const StateFunction sourcePositionFunction; + //! Variable denoting whether mutual acceleration between bodies is included. + /*! + * Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + */ + bool isMutualAttractionUsed_; + + private: }; diff --git a/Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h b/Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h index fcccf5514c..f5a2c8a7e5 100644 --- a/Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h +++ b/Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h @@ -149,6 +149,17 @@ class ThirdBodyAcceleration: public basic_astrodynamics::AccelerationModel< Eige } } + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + void resetTime( const double currentTime = TUDAT_NAN ) + { + accelerationModelForBodyUndergoingAcceleration_->resetTime( currentTime ); + accelerationModelForCentralBody_->resetTime( currentTime ); + } + //! Function to return the direct acceleration model on body undergoing acceleration. /*! * Function to return the direct acceleration model on body undergoing acceleration. diff --git a/Tudat/Astrodynamics/Propagators/CMakeLists.txt b/Tudat/Astrodynamics/Propagators/CMakeLists.txt index 7221a097c1..b0957480ac 100644 --- a/Tudat/Astrodynamics/Propagators/CMakeLists.txt +++ b/Tudat/Astrodynamics/Propagators/CMakeLists.txt @@ -36,7 +36,9 @@ # Add source files. set(PROPAGATORS_SOURCES "${SRCROOT}${PROPAGATORSDIR}/createEnvironmentUpdater.cpp" + "${SRCROOT}${PROPAGATORSDIR}/createStateDerivativeModel.cpp" "${SRCROOT}${PROPAGATORSDIR}/setNumericallyIntegratedStates.cpp" + "${SRCROOT}${PROPAGATORSDIR}/propagationSettings.cpp" ) # Add header files. @@ -53,6 +55,7 @@ set(PROPAGATORS_HEADERS "${SRCROOT}${PROPAGATORSDIR}/dynamicsSimulator.h" "${SRCROOT}${PROPAGATORSDIR}/environmentUpdater.h" "${SRCROOT}${PROPAGATORSDIR}/integrateEquations.h" + "${SRCROOT}${PROPAGATORSDIR}/bodyMassStateDerivative.h" ) # Add static libraries. @@ -85,3 +88,21 @@ setup_custom_test_program(test_EnvironmentModelUpdater "${SRCROOT}${PROPAGATORSD tudat_basic_astrodynamics tudat_input_output tudat_basic_mathematics tudat_propagators tudat_spice_interface cspice ${Boost_LIBRARIES}) endif() + +add_executable(test_BodyMassPropagation + "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestBodyMassPropagation.cpp") +setup_custom_test_program(test_BodyMassPropagation "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_BodyMassPropagation tudat_simulation_setup tudat_propagators + tudat_aerodynamics tudat_gravitation tudat_mission_segments tudat_electro_magnetism + tudat_ephemerides tudat_numerical_integrators tudat_reference_frames tudat_basic_astrodynamics + tudat_input_output tudat_basic_mathematics tudat_propagators tudat_spice_interface + cspice ${Boost_LIBRARIES}) + +add_executable(test_MultiTypeStatePropagation + "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestMultiTypeStatePropagation.cpp") +setup_custom_test_program(test_MultiTypeStatePropagation "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_MultiTypeStatePropagation tudat_simulation_setup tudat_propagators + tudat_aerodynamics tudat_gravitation tudat_mission_segments tudat_electro_magnetism + tudat_ephemerides tudat_numerical_integrators tudat_reference_frames tudat_basic_astrodynamics + tudat_input_output tudat_basic_mathematics tudat_propagators tudat_spice_interface + cspice ${Boost_LIBRARIES}) diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp new file mode 100644 index 0000000000..efa72bbe05 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp @@ -0,0 +1,153 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" +#include "Tudat/SimulationSetup/body.h" +#include "Tudat/Astrodynamics/Propagators/dynamicsSimulator.h" + + +namespace tudat +{ + +namespace unit_tests +{ + +using namespace tudat::propagators; +using namespace tudat::simulation_setup; +using namespace tudat::numerical_integrators; + +BOOST_AUTO_TEST_SUITE( test_body_mass_propagation ) + +double getDummyMassRate1( + const NamedBodyMap bodyMap ) +{ + return ( bodyMap.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodyMap.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4; +} + +double getDummyMassRate2( + const NamedBodyMap bodyMap ) +{ + return ( 3.0 * bodyMap.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodyMap.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4; +} + +// Test mass rate of single body, linearly decreasing with time +BOOST_AUTO_TEST_CASE( testBodyMassPropagation ) +{ + // Crate bodyMap + NamedBodyMap bodyMap; + bodyMap[ "Vehicle" ] = boost::make_shared< Body >( ); + + // Create mass rate model. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::lambda::constant( -0.01 ) ); + + // Create settings for propagation + Eigen::VectorXd initialMass = Eigen::VectorXd( 1 ); + initialMass( 0 ) = 500.0; + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle" ), massRateModels, initialMass ); + + // Define numerical integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1000.0, 1.0 ); + + // Create dynamics simulation object. + SingleArcDynamicsSimulator< double, double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + // Test propagated solution. + std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( ); + stateIterator != integratedState.end( ); stateIterator++ ) + { + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), 500.0 - 0.01 * stateIterator->first, 1.0E-13 ); + } +} + +// Test coupled mass rate of two bodies. Model ius unphysical, but has an analytical solution, and allows the internal +// workings of the mass propagation to be more rigorously tested. +BOOST_AUTO_TEST_CASE( testTwoBodyMassPropagation ) +{ + // Crate bodyMap + NamedBodyMap bodyMap; + bodyMap[ "Vehicle1" ] = boost::make_shared< Body >( ); + bodyMap[ "Vehicle2" ] = boost::make_shared< Body >( ); + + // Create mass rate models. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle1" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::bind( &getDummyMassRate1, bodyMap ) ); + massRateModels[ "Vehicle2" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::bind( &getDummyMassRate2, bodyMap ) ); + bodyMap[ "Earth" ] = boost::make_shared< Body >( ); + bodyMap[ "Earth" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ) ) ); + bodyMap[ "Vehicle1" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ), "Earth" ) ); + bodyMap[ "Vehicle2" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ), "Earth" ) ); + + // Create settings for propagation + Eigen::VectorXd initialMass = Eigen::VectorXd( 2 ); + initialMass( 0 ) = 500.0; + initialMass( 1 ) = 1000.0; + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle1" )( "Vehicle2" ), massRateModels, initialMass ); + + // Define numerical integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1000.0, 1.0 ); + + // Create dynamics simulation object. + SingleArcDynamicsSimulator< double, double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, true ); + + // Test propagated solution. + std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( ); + stateIterator != integratedState.end( ); stateIterator++ ) + { + // Test directly + BOOST_CHECK_CLOSE_FRACTION( + stateIterator->second( 0 ), + 100.0 * ( -std::exp( -stateIterator->first / 1E4 ) + + 6.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 ); + BOOST_CHECK_CLOSE_FRACTION( + stateIterator->second( 1 ), + 100.0 * ( std::exp( -stateIterator->first / 1E4 ) + + 9.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 ); + + // Test reset mass solution of vehicles. + bodyMap[ "Vehicle1" ]->updateMass( stateIterator->first ); + bodyMap[ "Vehicle2" ]->updateMass( stateIterator->first ); + + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), bodyMap[ "Vehicle1" ]->getBodyMass( ), + std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 1 ), bodyMap[ "Vehicle2" ]->getBodyMass( ), + std::numeric_limits< double >::epsilon( ) ); + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp index 5f17cfd07f..f70376acb7 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp @@ -90,7 +90,7 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies ) bodySettings[ "Moon" ]->ephemerisSettings->resetFrameOrigin( "Earth" ); - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( bodySettings ); + NamedBodyMap bodyMap = createBodies( bodySettings ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); @@ -325,7 +325,8 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies ) //! Test to ensure that a point-mass acceleration on a body produces a Kepler orbit (to within //! numerical error bounds). -BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) +template< typename TimeType, typename StateScalarType > +void testCowellPropagationOfKeplerOrbit( ) { //Load spice kernels. std::string kernelsPath = input_output::getSpiceKernelPath( ); @@ -351,13 +352,19 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + if( std::is_same< long double, StateScalarType >::value ) + { + boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( + bodySettings[ "Moon" ]->ephemerisSettings )->setUseLongDoubleStates( 1 ); + } + // Change ephemeris settings of Moon and Earth to make test results analysis more transparent. boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings > ( bodySettings[ "Moon" ]->ephemerisSettings )->resetFrameOrigin( "Earth" ); bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >( basic_mathematics::Vector6d::Zero( ), "SSB", "ECLIPJ2000" ); - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( bodySettings ); + NamedBodyMap bodyMap = createBodies( bodySettings ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); // Set accelerations between bodies that are to be taken into account. @@ -374,15 +381,15 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) unsigned int numberOfNumericalBodies = bodiesToPropagate.size( ); // Define settings for numerical integrator. - boost::shared_ptr< IntegratorSettings< > > integratorSettings = - boost::make_shared< IntegratorSettings< > > + boost::shared_ptr< IntegratorSettings< TimeType > > integratorSettings = + boost::make_shared< IntegratorSettings< TimeType > > ( rungeKutta4, initialEphemerisTime, finalEphemerisTime, 120.0 ); // Run test where Moon gravity is/is not taken into account. for( unsigned testCase = 0; testCase < 2; testCase++ ) { // Get initial kepler elements - double effectiveGravitationalParameter; + StateScalarType effectiveGravitationalParameter; if( testCase == 0 ) { effectiveGravitationalParameter @@ -414,40 +421,43 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) } // Create system initial state. - Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToPropagate.size( ) * 6 ); + Eigen::Matrix< StateScalarType, 6, 1 > systemInitialState = + Eigen::Matrix< StateScalarType, 6, 1 >( bodiesToPropagate.size( ) * 6 ); for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ ) { systemInitialState.segment( i * 6 , 6 ) = spice_interface::getBodyCartesianStateAtEpoch( - bodiesToPropagate[ i ], "Earth", "ECLIPJ2000", "NONE", initialEphemerisTime ); + bodiesToPropagate[ i ], "Earth", "ECLIPJ2000", "NONE", initialEphemerisTime ). + template cast< StateScalarType >( ); } // Create acceleration models and propagation settings. AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); - boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = - boost::make_shared< TranslationalStatePropagatorSettings< double > > + boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< StateScalarType > > ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState ); // Create dynamics simulation object. - SingleArcDynamicsSimulator< > dynamicsSimulator( + SingleArcDynamicsSimulator< StateScalarType, TimeType > dynamicsSimulator( bodyMap, integratorSettings, propagatorSettings, true, false ); - basic_mathematics::Vector6d initialKeplerElements = - orbital_element_conversions::convertCartesianToKeplerianElements( - basic_mathematics::Vector6d( systemInitialState ), effectiveGravitationalParameter ); + Eigen::Matrix< StateScalarType, 6, 1 > initialKeplerElements = + orbital_element_conversions::convertCartesianToKeplerianElements< StateScalarType >( + Eigen::Matrix< StateScalarType, 6, 1 >( systemInitialState ), effectiveGravitationalParameter ); // Compare numerical state and kepler orbit at each time step. boost::shared_ptr< Ephemeris > moonEphemeris = bodyMap.at( "Moon" )->getEphemeris( ); double currentTime = initialEphemerisTime + buffer; while( currentTime < finalEphemerisTime - buffer ) { - basic_mathematics::Vector6d stateDifference - = orbital_element_conversions::convertKeplerianToCartesianElements( - propagateKeplerOrbit( initialKeplerElements, currentTime - initialEphemerisTime, + Eigen::VectorXd stateDifference + = ( orbital_element_conversions::convertKeplerianToCartesianElements( + propagateKeplerOrbit< StateScalarType >( initialKeplerElements, currentTime - initialEphemerisTime, effectiveGravitationalParameter ), effectiveGravitationalParameter ) - - moonEphemeris->getCartesianStateFromEphemeris( currentTime ); + - moonEphemeris->template getTemplatedStateFromEphemeris< StateScalarType >( currentTime ) ). + template cast< double >( ); for( int i = 0; i < 3; i++ ) { BOOST_CHECK_SMALL( stateDifference( i ), 1E-3 ); @@ -458,6 +468,12 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) } } } +BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) +{ + testCowellPropagationOfKeplerOrbit< double, double >( ); + testCowellPropagationOfKeplerOrbit< double, long double >( ); + +} BOOST_AUTO_TEST_SUITE_END( ) diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp index 527726bb72..ff592fc4bb 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp @@ -103,7 +103,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) std::vector< std::string > propagatedBodyList; std::vector< std::string > centralBodyList; - std::map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; + std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; double testTime = 0.0; { @@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), @@ -260,7 +260,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); } @@ -371,7 +371,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); } @@ -424,10 +424,10 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) // Define test time and state. double testTime = 2.0 * 86400.0; - std::map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; + std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; Eigen::VectorXd testState = 1.1 * bodyMap[ "Vehicle" ]->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ) - + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ); + getCartesianStateFromEphemeris( testTime ) + + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ); integratedStateToSet[ transational_state ] = testState; { @@ -484,10 +484,8 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); - - } { @@ -608,7 +606,7 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) std::numeric_limits< double >::epsilon( ) ); updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp new file mode 100644 index 0000000000..737e87f5d1 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp @@ -0,0 +1,239 @@ +#define BOOST_TEST_MAIN + + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + + +namespace tudat +{ +namespace unit_tests +{ + +using namespace mathematical_constants; + +BOOST_AUTO_TEST_SUITE( test_hybrid_state_derivative_model ) + +std::map< double, Eigen::VectorXd > propagateKeplerOrbitAndMassState( + const int simulationCase ) +{ + using namespace simulation_setup; + using namespace propagators; + using namespace numerical_integrators; + using namespace orbital_element_conversions; + using namespace basic_mathematics; + using namespace gravitation; + using namespace numerical_integrators; + using namespace unit_conversions; + + // Load Spice kernels. + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00009.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de-403-masses.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de421.bsp" ); + + // Set simulation end epoch. + const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY; + + // Set numerical integration fixed step size. + const double fixedStepSize = 60.0; + + // Define body settings for simulation. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings; + bodySettings[ "Earth" ] = boost::make_shared< BodySettings >( ); + bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >( + basic_mathematics::Vector6d::Zero( ), "SSB", "J2000" ); + bodySettings[ "Earth" ]->gravityFieldSettings = boost::make_shared< GravityFieldSettings >( central_spice ); + + // Create Earth object + NamedBodyMap bodyMap = createBodies( bodySettings ); + + // Create spacecraft object. + bodyMap[ "Asterix" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Asterix" ]->setEphemeris( boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator + < double, Vector6d > >( ), "Earth", "J2000" ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define propagation settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix; + accelerationsOfAsterix[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationMap[ "Asterix" ] = accelerationsOfAsterix; + bodiesToPropagate.push_back( "Asterix" ); + centralBodies.push_back( "Earth" ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + + // Set Keplerian elements for Asterix. + Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1; + asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 ); + asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = convertDegreesToRadians( 235.7 ); + asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = convertDegreesToRadians( 23.4 ); + asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 ); + + // Convert Asterix state from Keplerian elements to Cartesian elements. + double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements( + asterixInitialStateInKeplerianElements, + earthGravitationalParameter ); + + + boost::shared_ptr< PropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState ); + + // Create mass rate model and mass propagation settings + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::lambda::constant( -0.01 ) ); + Eigen::VectorXd initialMass = Eigen::VectorXd( 1 ); + initialMass( 0 ) = 500.0; + boost::shared_ptr< PropagatorSettings< double > > massPropagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Asterix" ), massRateModels, initialMass ); + + // Create total propagator settings, depending on current case. + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings; + if( ( simulationCase % 3 ) == 0 ) + { + propagatorSettings = translationalPropagatorSettings; + } + else if( ( simulationCase % 3 ) == 1 ) + { + propagatorSettings = massPropagatorSettings; + } + else if( ( simulationCase % 3 ) == 2 ) + { + std::vector< boost::shared_ptr< PropagatorSettings< double > > > propagatorSettingsList; + propagatorSettingsList.push_back( translationalPropagatorSettings ); + propagatorSettingsList.push_back( massPropagatorSettings ); + + propagatorSettings = boost::make_shared< MultiTypePropagatorSettings< double > >( + propagatorSettingsList ); + } + + + + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, simulationEndEpoch, fixedStepSize ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, true ); + + // Return propagated dynamics (if simulationCase < 3) or interpolated dynamics (else) + if( simulationCase < 3 ) + { + return dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + } + else + { + std::map< double, Eigen::VectorXd > returnMap; + double currentTime = 4.0 * fixedStepSize; + + double interpolationTimeStep = 32.1; + + // Interpolate dynamics for range of times. + while( currentTime < simulationEndEpoch - 4.0 * fixedStepSize ) + { + + // Interpolate propagated dynamics type + if( simulationCase == 3 ) + { + returnMap[ currentTime ] = bodyMap[ "Asterix" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTime ); + } + else if( simulationCase == 4 ) + { + returnMap[ currentTime ] = Eigen::VectorXd::Zero( 1 ); + returnMap[ currentTime ]( 0 ) = bodyMap[ "Asterix" ]->getBodyMassFunction( )( currentTime ); + } + else if( simulationCase == 5 ) + { + returnMap[ currentTime ] = Eigen::VectorXd::Zero( 7 ); + returnMap[ currentTime ].segment( 0, 6 ) = bodyMap[ "Asterix" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTime ); + returnMap[ currentTime ]( 6 ) = bodyMap[ "Asterix" ]->getBodyMassFunction( )( currentTime ); + + } + + // Increment time (non-resonant with integration time step). + currentTime += interpolationTimeStep; + } + + return returnMap; + } + +} +//! Test if conversion from Keplerian elements to Cartesian elements is working correctly. +BOOST_AUTO_TEST_CASE( testHybridStateDerivativeModel ) +{ + // Compare separate and multitype (independent) propagation directly from propagation (useCase = 0) and + // interpolated from reset dynamics (useCase = 1) + for( int useCase = 0; useCase < 2; useCase++ ) + { + int simulationCaseToAdd = ( useCase == 0 ) ? ( 0 ) : ( 3 ); + + // Propagate dynamics for translational, mass and combined state. + std::map< double, Eigen::VectorXd > translationalState = propagateKeplerOrbitAndMassState( 0 + simulationCaseToAdd ); + std::map< double, Eigen::VectorXd > massState = propagateKeplerOrbitAndMassState( 1 + simulationCaseToAdd ); + std::map< double, Eigen::VectorXd > combinedState = propagateKeplerOrbitAndMassState( 2 + simulationCaseToAdd ); + + std::map< double, Eigen::VectorXd >::const_iterator stateIterator = translationalState.begin( ); + std::map< double, Eigen::VectorXd >::const_iterator massIterator = massState.begin( ); + std::map< double, Eigen::VectorXd >::const_iterator combinedIterator = combinedState.begin( ); + + // Compare separate and multitype dynamics of each type. + for( unsigned int i = 0; i < translationalState.size( ); i++ ) + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( stateIterator->second, combinedIterator->second.segment( 0, 6 ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( massIterator->second, combinedIterator->second.segment( 6, 1 ), + std::numeric_limits< double >::epsilon( ) ); + stateIterator++; + massIterator++; + combinedIterator++; + } + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h b/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h new file mode 100644 index 0000000000..ea3f0720ce --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h @@ -0,0 +1,177 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_BODYMASSSTATEDERIVATIVE_H +#define TUDAT_BODYMASSSTATEDERIVATIVE_H + +#include +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" + + +namespace tudat +{ + +namespace propagators +{ + +//! Class for computing the derivative of the mass of a set of bodies. +/*! + * Class for computing the derivative of the mass of a set of bodies. Note that the local and global states are equal + * for mass propagation, both represent the physical body mass (in kg). + */ +template< typename StateScalarType = double, typename TimeType = double > +class BodyMassStateDerivative: public propagators::SingleStateTypeDerivative< StateScalarType, TimeType > +{ +public: + + //! Constructor + /*! + * Constructor, sets the mass rate models and the bodies that are to be propagated + * \param massRateModels Map of models per body that are to be used for the mass rate computation. + * \param bodiesToIntegrate List of bodies for which the mass is to be propagated. Note that this vector have + * more entries than the massRateModels map, as a body's mass can be 'propagated' with no rate model (i.e. constant + * mass). + */ + BodyMassStateDerivative( + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > >& massRateModels, + const std::vector< std::string >& bodiesToIntegrate ): + propagators::SingleStateTypeDerivative< StateScalarType, TimeType >( + propagators::body_mass_state ), + massRateModels_( massRateModels ), bodiesToIntegrate_( bodiesToIntegrate ){ } + + //! Destructor + virtual ~BodyMassStateDerivative( ){ } + + //! Calculates the state derivative of the system of equations for the mass dynamics + /*! + * Calculates the state derivative of the system of equations for the mass dynamics + * The environment and acceleration models (updateStateDerivativeModel) must be + * updated before calling this function. + * \param time Time at which the state derivative is to be calculated. + * \param stateOfSystemToBeIntegrated Current masses of the bodies that are propagated + * \param stateDerivative Mass rates of the bodies for which the mass is propagated, in the same order as + * bodiesToIntegrate_ + */ + void calculateSystemStateDerivative( + const TimeType time, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) + { + stateDerivative.setZero( ); + + // Iterate over all mass rate models, retrieve value and put into corresponding entry. + int currentIndex = 0; + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + stateDerivative( currentIndex, 0 ) = static_cast< StateScalarType >( + massRateModelIterator_->second->getMassRate( ) ); + + currentIndex++; + + } + } + + //! Function to update the mass state derivative model to the current time. + /*! + * Function to update the mass state derivative model to the urrent time. + * cNote that this function only updates the state derivative model itself, the + * environment models must be updated before calling this function + * \param currentTime Time to which the mass state derivative is to be updated. + */ + void updateStateDerivativeModel( const TimeType currentTime ) + { + // Reset all mass rate times (to allow multiple evaluations at same time, e.g. stage 2 and 3 in RK4 integrator) + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + massRateModelIterator_->second->resetTime( TUDAT_NAN ); + } + + // Update local variables of mass rate model objects. + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + massRateModelIterator_->second->updateMembers( static_cast< double >( currentTime ) ); + } + } + + //! Function included for compatibility purposes with base class, local and global representation is equal for mass rate + //! model. Function returns (by reference) input internalSolution. + void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) + { + currentCartesianLocalSoluton = internalSolution; + } + + //! Function included for compatibility purposes with base class, input and output representation is equal for mass rate + //! model. Function returns input outputSolution. + virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& outputSolution, const TimeType& time ) + { + return outputSolution; + } + + //! Function included for compatibility purposes with base class, input and output representation is equal for mass rate + //! 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 ) + { + currentCartesianLocalSoluton = internalSolution; + } + + //! Function to get the total size of the state of propagated masses. + /*! + * Function to get the total size of the state of propagated masses. Equal to number of bodies for which the mass + * is propagated. + * \return Size of propagated mass state. + */ + virtual int getStateSize( ) + { + return bodiesToIntegrate_.size( ); + } + +private: + + //! Map of models per body that are to be used for the mass rate computation. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels_; + + //! Predefined iterator to save (de-)allocation time. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > >::const_iterator massRateModelIterator_; + + //! List of bodies for which the mass is to be propagated. + /*! + * List of bodies for which the mass is to be propagated. Note that this vector have + * more entries than the massRateModels map, as a body's mass can be 'propagated' with no rate model (i.e. constant + * mass). + */ + std::vector< std::string > bodiesToIntegrate_; + +}; + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_BODYMASSSTATEDERIVATIVE_H diff --git a/Tudat/Astrodynamics/Propagators/centralBodyData.h b/Tudat/Astrodynamics/Propagators/centralBodyData.h index f396b3f453..c2454449cd 100644 --- a/Tudat/Astrodynamics/Propagators/centralBodyData.h +++ b/Tudat/Astrodynamics/Propagators/centralBodyData.h @@ -162,6 +162,8 @@ class CentralBodyData updateOrder_[ currentUpdateIndex ] = numericalBodies_.at( i ); currentUpdateIndex++; } + + localInternalState_.resize( 6 * bodiesToIntegrate.size( ), 1 ); } @@ -174,33 +176,36 @@ class CentralBodyData * size of bodiesToIntegrate, with entries in the order of the bodies in the bodiesToIntegrate * vector. * \param time Current time (used for retrieving states from ephemerides) - * \param areInputStateLocal True if the internalState vector is given in the local frames of - * the integrated bodies, or the global frame. - * \return Vector of states of the reference frame origins for each body. + * \param referenceFrameOriginStates Vector of states of the reference frame origins for each body + * (returned by reference). + * \param areInputStateLocal True if the internalState vector is given in the local frames of the integrated + * bodies, or the global frame. */ - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > getReferenceFrameOriginInertialStates( - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > internalState, const TimeType time, + void getReferenceFrameOriginInertialStates( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalState, const TimeType time, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& referenceFrameOriginStates, const bool areInputStateLocal = true ) { - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > referenceFrameOriginStates_; - referenceFrameOriginStates_.resize( updateOrder_.size( ) ); + localInternalState_ = internalState; + if( referenceFrameOriginStates.size( ) != updateOrder_.size( ) )\ + { + referenceFrameOriginStates.resize( updateOrder_.size( ) ); + } // Update state in correct order. for( unsigned int i = 0; i < updateOrder_.size( ); i++ ) { - referenceFrameOriginStates_[ updateOrder_[ i ] ] = - getSingleReferenceFrameOriginInertialState( internalState, - time, updateOrder_[ i ] ); + getSingleReferenceFrameOriginInertialState( + localInternalState_, time, updateOrder_.at( i ), + referenceFrameOriginStates.at( updateOrder_.at( i ) )); // Modify current input state to global frame if input is local (in propagation frame). if( areInputStateLocal ) { - internalState.segment( 6 * updateOrder_[ i ], 6 ) - += referenceFrameOriginStates_[ updateOrder_[ i ] ]; + localInternalState_.segment( 6 * updateOrder_.at( i ), 6 ) += + referenceFrameOriginStates.at( updateOrder_.at( i ) ); } } - - return referenceFrameOriginStates_; } @@ -250,6 +255,9 @@ class CentralBodyData //! as propagation origin std::map< int, int > centralBodiesFromIntegration_; + //! State of propagated bodies, with the origins translated to the global origin + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > localInternalState_; + //! Function to get the global origin of the propagation center of a single body. /*! * Function to get the global origin of the propagation center of a single body, where the @@ -261,20 +269,19 @@ class CentralBodyData * \sa updateOrder * \param time Current time. * \param bodyIndex Index of the body for which the global origin state is to be retrieved - * \return Global origin state of the requested body. + * \param originState Global origin state of the requested body (returned by reference). */ - Eigen::Matrix< StateScalarType, 6, 1 > getSingleReferenceFrameOriginInertialState( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > internalSolution, + void getSingleReferenceFrameOriginInertialState( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType time, - const int bodyIndex ) + const int bodyIndex, + Eigen::Matrix< StateScalarType, 6, 1 >& originState ) { - Eigen::Matrix< StateScalarType, 6, 1 > originState - = Eigen::Matrix< StateScalarType, 6, 1 >::Zero( ); - // Check origin type. switch( bodyOriginType_[ bodyIndex ] ) { case inertial: + originState.setZero( ); break; case from_ephemeris: originState @@ -289,7 +296,6 @@ class CentralBodyData boost::lexical_cast< std::string >( bodyOriginType_[ bodyIndex ] ) ); break; } - return originState; } }; diff --git a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp b/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp index 92a723b74c..b643127133 100644 --- a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp +++ b/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp @@ -219,8 +219,11 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( if( thirdBodyAcceleration != NULL && translationalAccelerationModels.count( thirdBodyAcceleration->getCentralBodyName( ) ) == 0 ) { - singleAccelerationUpdateNeeds[ body_transational_state_update ].push_back( - thirdBodyAcceleration->getCentralBodyName( ) ); + if( translationalAccelerationModels.count( thirdBodyAcceleration->getCentralBodyName( ) ) == 0 ) + { + singleAccelerationUpdateNeeds[ body_transational_state_update ].push_back( + thirdBodyAcceleration->getCentralBodyName( ) ); + } } else if( thirdBodyAcceleration == NULL ) { @@ -278,6 +281,8 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( break; } default: + throw std::runtime_error( std::string( "Error when setting acceleration model update needs, model type not recognized: " ) + + boost::lexical_cast< std::string >( currentAccelerationModelType ) ); break; } @@ -295,10 +300,54 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( return environmentModelsToUpdate; } +//! Get list of required environment model update settings from mass rate models. +std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > +createMassPropagationEnvironmentUpdaterSettings( + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels, + const simulation_setup::NamedBodyMap& bodyMap ) +{ + using namespace basic_astrodynamics; + using namespace propagators; + + std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > > environmentModelsToUpdate; + std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > > singleRateModelUpdateNeeds; + + // Iterate over all bodies with mass rate model. + for( std::map< std::string, boost::shared_ptr< MassRateModel > >::const_iterator massRateModelIterator = + massRateModels.begin( ); massRateModelIterator != massRateModels.end( ); massRateModelIterator++ ) + { + singleRateModelUpdateNeeds.clear( ); + + // Identify mass rate type and set required environment update settings. + AvailableMassRateModels currentAccelerationModelType = + getMassRateModelType( massRateModelIterator->second ); + switch( currentAccelerationModelType ) + { + case custom: + break; + default: + throw std::runtime_error( std::string( "Error when setting mass rate model update needs, model type not recognized: " ) + + boost::lexical_cast< std::string >( currentAccelerationModelType ) ); + + } + + // Check whether requested updates are possible. + checkValidityOfRequiredEnvironmentUpdates( singleRateModelUpdateNeeds, bodyMap ); + + // Add requested updates of current acceleration model to + // full list of environment updates. + addEnvironmentUpdates( environmentModelsToUpdate, singleRateModelUpdateNeeds ); + } + + return environmentModelsToUpdate; + +} //! Function to create 'brute-force' update settings, in which each environment model is updated. std::map< propagators::EnvironmentModelsToUpdate, - std::vector< std::string > > createFullEnvironmentUpdaterSettings( +std::vector< std::string > > createFullEnvironmentUpdaterSettings( const simulation_setup::NamedBodyMap& bodyMap ) { using namespace basic_astrodynamics; diff --git a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h b/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h index cc1863c4a6..f57d3475ee 100644 --- a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h +++ b/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h @@ -62,6 +62,18 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( const basic_astrodynamics::AccelerationMap& translationalAccelerationModels, const simulation_setup::NamedBodyMap& bodyMap ); +//! Get list of required environment model update settings from mass rate models. +/*! + * Get list of required environment model update settings from mass rate models. + * \param massRateModels List of mass rate models used in simulation. + * \param bodyMap List of body objects used in the simulations. + * \return List of required environment model update settings. + */ +std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > +createMassPropagationEnvironmentUpdaterSettings( + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels, + const simulation_setup::NamedBodyMap& bodyMap ); + //! Get list of required environment model update settings from a list of propagation settings. /*! * Get list of required environment model update settings from a list of propagation settings. @@ -82,6 +94,42 @@ std::map< propagators::EnvironmentModelsToUpdate, // Check dynamics type switch( propagatorSettings->stateType_ ) { + case hybrid: + { + // Cast to derived type + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + // Iterate over all propagation settings in hybrid model + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > singleAccelerationUpdateNeeds; + + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + // Create current propagation model from settings list (must not be hybrid). + if( typeIterator->first != hybrid ) + { + singleAccelerationUpdateNeeds = createEnvironmentUpdaterSettings< StateScalarType >( + typeIterator->second.at( i ), bodyMap ); + + // Add single model environment model update settings to full list + checkValidityOfRequiredEnvironmentUpdates( singleAccelerationUpdateNeeds, bodyMap ); + addEnvironmentUpdates( environmentModelsToUpdate, singleAccelerationUpdateNeeds ); + } + else + { + throw std::runtime_error( + "Error when making environment updater type list, cannot handle hybrid propagator inside hybrid propagator" ); + } + } + } + break; + } + // Retrieve environment model settings for translational dynamics case transational_state: { environmentModelsToUpdate = createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( @@ -91,6 +139,15 @@ std::map< propagators::EnvironmentModelsToUpdate, bodyMap ); break; } + // Retrieve environment model settings for mass rate model + case body_mass_state: + { + environmentModelsToUpdate = createMassPropagationEnvironmentUpdaterSettings( + boost::dynamic_pointer_cast< + MassPropagatorSettings< StateScalarType > >( + propagatorSettings )->massRateModels_, bodyMap ); + break; + } default: { throw std::runtime_error( "Error, cannot create environment updates for type " + diff --git a/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.cpp b/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.cpp new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.cpp @@ -0,0 +1 @@ + diff --git a/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h b/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h index cf962a2cac..4553ffaea6 100644 --- a/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h +++ b/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h @@ -11,9 +11,12 @@ #ifndef TUDAT_CREATESTATEDERIVATIVEMODEL_H #define TUDAT_CREATESTATEDERIVATIVEMODEL_H +#include + #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" #include "Tudat/Astrodynamics/Propagators/propagationSettings.h" #include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h" +#include "Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h" #include "Tudat/SimulationSetup/body.h" namespace tudat @@ -132,6 +135,23 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > return stateDerivativeModel; } +//! Function to create a mass state derivative model. +/*! + * Function to create a mass state derivative model from propagation settings and environment. + * \param massPropagatorSettings Settings for the mass dynamics model. + * \param bodyMap List of body objects in the environment + * \return Mass state derivative model. + */ +template< typename StateScalarType = double, typename TimeType = double > +boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > createBodyMassStateDerivativeModel( + const boost::shared_ptr< MassPropagatorSettings< StateScalarType > > massPropagatorSettings, + const simulation_setup::NamedBodyMap& bodyMap ) +{ + return boost::make_shared< propagators::BodyMassStateDerivative< StateScalarType, TimeType > >( + massPropagatorSettings->massRateModels_, + massPropagatorSettings->bodiesWithMassToPropagate_ ); +} + //! Function to create a state derivative model. /*! * Function to create a state derivative model from propagation settings and the environment. @@ -170,6 +190,23 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > } break; } + case body_mass_state: + { + // Check input consistency. + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > massPropagatorSettings = + boost::dynamic_pointer_cast< MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( + "Error, expected mass propagation settings when making state derivative model" ); + } + else + { + stateDerivativeModel = createBodyMassStateDerivativeModel< StateScalarType, TimeType >( + massPropagatorSettings, bodyMap ); + } + break; + } default: throw std::runtime_error( "Error, could not process state type " @@ -200,6 +237,36 @@ std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, Time // Check type of state derivative model and call associated create function. switch( propagatorSettings->stateType_ ) { + // If hybrid, call create function separately for each entry. + case hybrid: + { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + // Iterate over all propagation settings + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::iterator + propagatorIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + propagatorIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); propagatorIterator++ ) + { + for( unsigned int i = 0; i < propagatorIterator->second.size( ); i++ ) + { + // Call create function for current propagation settings. + if( propagatorIterator->first != hybrid ) + { + stateDerivativeModels.push_back( createStateDerivativeModel< StateScalarType, TimeType >( + propagatorIterator->second.at( i ), bodyMap ) ); + } + else + { + throw std::runtime_error( + "Error when making state derivative model, cannot process nested hybrid propagators" ); + } + } + } + break; + } + // If not hybrid, call create function for single object directly. default: stateDerivativeModels.push_back( createStateDerivativeModel< StateScalarType, TimeType >( propagatorSettings, bodyMap ) ); diff --git a/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h b/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h index 806906d2b2..2ce840acf6 100644 --- a/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h +++ b/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h @@ -165,7 +165,7 @@ class DynamicsSimulator * automatically use the integrated results to set ephemerides. */ DynamicsSimulator( - const simulation_setup::NamedBodyMap& bodyMap, + const simulation_setup::NamedBodyMap& bodyMap, const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, @@ -176,10 +176,10 @@ class DynamicsSimulator clearNumericalSolutions_( clearNumericalSolutions ), setIntegratedResult_( setIntegratedResult ) { - frameManager_ = boost::make_shared< ephemerides::ReferenceFrameManager >( bodyMap ); - if( setIntegratedResult ) + if( setIntegratedResult_ ) { + frameManager_ = boost::make_shared< ephemerides::ReferenceFrameManager >( bodyMap ); integratedStateProcessors_ = createIntegratedStateProcessors< TimeType, StateScalarType >( propagatorSettings_, bodyMap_, frameManager_ ); } @@ -193,6 +193,9 @@ class DynamicsSimulator boost::bind( &DynamicsStateDerivativeModel < TimeType, StateScalarType >::computeStateDerivative, dynamicsStateDerivative_, _1, _2 ); + doubleStateDerivativeFunction_ = + boost::bind( &DynamicsStateDerivativeModel< TimeType, StateScalarType >::computeStateDoubleDerivative, + dynamicsStateDerivative_, _1, _2 ); } //! Virtual destructor @@ -236,6 +239,18 @@ class DynamicsSimulator return stateDerivativeFunction_; } + //! Function to get the function that performs a single state derivative function evaluation with double precision. + /*! + * Function to get the function that performs a single state derivative function evaluation with double precision, + * regardless of template arguments. + * \return Function that performs a single state derivative function evaluation with double precision. + */ + boost::function< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > + ( const double, const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& ) > getDoubleStateDerivativeFunction( ) + { + return doubleStateDerivativeFunction_; + } + //! Function to get the settings for the propagator. /*! * Function to get the settings for the propagator. @@ -321,6 +336,13 @@ class DynamicsSimulator ( const TimeType, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& ) > stateDerivativeFunction_; + //! Function that performs a single state derivative function evaluation with double precision. + /*! + * Function that performs a single state derivative function evaluation with double precision + * \sa stateDerivativeFunction_ + */ + boost::function< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > + ( const double, const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& ) > doubleStateDerivativeFunction_; //! Map of bodies (with names) of all bodies in integration. simulation_setup::NamedBodyMap bodyMap_; @@ -422,7 +444,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim equationsOfMotionNumericalSolution_.clear( ); - dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1 ); + dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1, 0 ); // Integrate equations of motion numerically. equationsOfMotionNumericalSolution_ = @@ -484,7 +506,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim equationsOfMotionNumericalSolution_.clear( ); } - for( std::map< std::string, boost::shared_ptr< simulation_setup::Body > >::const_iterator + for( simulation_setup::NamedBodyMap::const_iterator bodyIterator = bodyMap_.begin( ); bodyIterator != bodyMap_.end( ); bodyIterator++ ) { diff --git a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h index 21048f4236..f3f47e3ddb 100644 --- a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h +++ b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h @@ -25,6 +25,7 @@ #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" #include "Tudat/Astrodynamics/Propagators/environmentUpdater.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" namespace tudat { @@ -105,6 +106,11 @@ class DynamicsStateDerivativeModel // Set current model in member map. stateDerivativeModels_[ stateDerivativeModels.at( i )->getIntegratedStateType( ) ].push_back( stateDerivativeModels.at( i ) ); + + + currentStatesPerTypeInConventionalRepresentation_[ stateDerivativeModels.at( i )->getIntegratedStateType( ) ] = + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( + stateTypeSize_.at( stateDerivativeModels.at( i )->getIntegratedStateType( ) ), 1 ); } } @@ -123,23 +129,24 @@ class DynamicsStateDerivativeModel StateType computeStateDerivative( const TimeType time, const StateType& state ) { // Initialize state derivative - StateType stateDerivative = StateType::Zero( state.rows( ), state.cols( ) ); + if( stateDerivative_.rows( ) != state.rows( ) || stateDerivative_.cols( ) != state.cols( ) ) + { + stateDerivative_.resize( state.rows( ), state.cols( ) ); + } // If dynamical equations are integrated, update the environment with the current state. if( evaluateDynamicsEquations_ ) { - std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - statesPerTypeInConventionalRepresentation = - convertCurrentStateToGlobalRepresentationPerType( state, time ); - environmentUpdater_->updateEnvironment( time, statesPerTypeInConventionalRepresentation, + convertCurrentStateToGlobalRepresentationPerType( state, time, evaluateVariationalEquations_ ); + environmentUpdater_->updateEnvironment( time, currentStatesPerTypeInConventionalRepresentation_, integratedStatesFromEnvironment_ ); } else { environmentUpdater_->updateEnvironment( - time, std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ), - integratedStatesFromEnvironment_ ); + time, std::unordered_map< + IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ), + integratedStatesFromEnvironment_ ); } // If dynamical equations are integrated, evaluate dynamics state derivatives. @@ -161,21 +168,29 @@ class DynamicsStateDerivativeModel currentIndices = stateIndices_.at( stateDerivativeModelsIterator_->first ).at( i ); stateDerivativeModelsIterator_->second.at( i )->calculateSystemStateDerivative( - time, state.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) ); - - stateDerivative.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) = - stateDerivativeModelsIterator_->second.at( i )->calculateSystemStateDerivative( - time, state.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) ); + time, state.block( currentIndices.first, dynamicsStartColumn_, currentIndices.second, 1 ), + stateDerivative_.block( currentIndices.first, dynamicsStartColumn_, currentIndices.second, 1 ) ); + } } } - return stateDerivative; + return stateDerivative_; } + //! Function to calculate the system state derivative with double precision, regardless of template arguments + /*! + * Function to calculate the system state derivative with double precision, regardless of template arguments + * \sa computeStateDerivative + * \param time Current time. + * \param state Current complete state. + * \return Calculated state derivative. + */ + Eigen::MatrixXd computeStateDoubleDerivative( + const double time, const Eigen::MatrixXd& state ) + { + return computeStateDerivative( static_cast< TimeType >( time ), state.template cast< StateScalarType >( ) ).template cast< double >( ); + } //! Function to convert the state in the conventional form to the propagator-specific form. /*! @@ -242,12 +257,11 @@ class DynamicsStateDerivativeModel stateDerivativeModelsIterator_->first ); for( unsigned int i = 0; i < stateDerivativeModelsIterator_->second.size( ); i++ ) { - outputState.segment( currentStateIndices.at( i ).first, - currentStateIndices.at( i ).second ) = - stateDerivativeModelsIterator_->second.at( i )->convertToOutputSolution( + stateDerivativeModelsIterator_->second.at( i )->convertToOutputSolution( internalSolution.segment( - currentStateIndices.at( i ).first, - currentStateIndices.at( i ).second ), time ); + currentStateIndices.at( i ).first, currentStateIndices.at( i ).second ), time, + outputState.block( currentStateIndices.at( i ).first, 0, + currentStateIndices.at( i ).second, 1 ) ); } } return outputState; @@ -287,16 +301,26 @@ class DynamicsStateDerivativeModel * Function to set which segments of the full state to propagate, i.e. whether to propagate the * variational/dynamical equations, and which types of the dynamics to propagate. * \param stateTypesToNotIntegrate Types of dynamics to propagate - * \param evaluateDynamicsEquations Boolean to denote whether the dynamical equations are to be - * propagated or not + * \param evaluateDynamicsEquations Boolean to denote whether the dynamical equations are to be propagated or not + * \param evaluateVariationalEquations Boolean to denote whether the variational equations are to be propagated or not */ void setPropagationSettings( const std::vector< IntegratedStateType >& stateTypesToNotIntegrate, - const bool evaluateDynamicsEquations ) + const bool evaluateDynamicsEquations, + const bool evaluateVariationalEquations ) { integratedStatesFromEnvironment_ = stateTypesToNotIntegrate; evaluateDynamicsEquations_ = evaluateDynamicsEquations; - dynamicsStartColumn_ = 0; + evaluateVariationalEquations_ = evaluateVariationalEquations; + + if( evaluateVariationalEquations_ ) + { + throw std::runtime_error( "Error, variational equations not yet implemented" ); + } + else + { + dynamicsStartColumn_ = 0; + } } //! Function to get complete list of state derivative models, sorted per state type. @@ -304,7 +328,7 @@ class DynamicsStateDerivativeModel * Function to get complete list of state derivative models, sorted per state type. * \return Complete list of state derivative models, sorted per state type. */ - std::map< IntegratedStateType, std::vector< boost::shared_ptr + std::unordered_map< IntegratedStateType, std::vector< boost::shared_ptr < SingleStateTypeDerivative< StateScalarType, TimeType > > > > getStateDerivativeModels( ) { return stateDerivativeModels_; @@ -324,23 +348,31 @@ class DynamicsStateDerivativeModel //! Function to convert the to the conventional form in the global frame per dynamics type. /*! - * Function to convert the propagator-specific form of the state to the conventional form in the - * global frame, split by dynamics type. The conventional form is one that is typically used to - * represent the current state in the environment (e.g. Body class). For translational dynamics - * this is the Cartesian position and velocity). The inertial frame is typically the barycenter - * with J2000/ECLIPJ2000 orientation, but may differ depending on simulation settings. + * Function to convert the propagator-specific form of the state to the conventional form in the global frame, split + * by dynamics type. This function updates the currentStatesPerTypeInConventionalRepresentation_ to the current state + * and time. + * The conventional form is one that is typically used to represent the current state in the environment + * (e.g. Body class). For translational dynamics this is the Cartesian position and velocity). + * The inertial frame is typically the barycenter with J2000/ECLIPJ2000 orientation, but may differ depending on + * simulation settings * \param state State in propagator-specific form (i.e. form that is used in numerical integration). * \param time Current time at which the state is valid. - * \return State (state), converted to the 'conventional form' in inertial coordinates, - * that can for instance be set directly in the body object. + * \param stateIncludesVariationalState Boolean defining whether the stae includes the state transition/sensitivity + * matrices */ - std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - convertCurrentStateToGlobalRepresentationPerType( const StateType& state, const TimeType& time ) + void convertCurrentStateToGlobalRepresentationPerType( + const StateType& state, const TimeType& time, const bool stateIncludesVariationalState ) { int startColumn = 0; - std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > splitConventionalStates; + if( stateIncludesVariationalState ) + { + throw std::runtime_error( "Error, variational equations not yet implemented" ); + } + else + { + startColumn = 0; + } std::pair< int, int > currentIndices; @@ -348,10 +380,7 @@ class DynamicsStateDerivativeModel for( stateDerivativeModelsIterator_ = stateDerivativeModels_.begin( ); stateDerivativeModelsIterator_ != stateDerivativeModels_.end( ); stateDerivativeModelsIterator_++ ) - { - splitConventionalStates[ stateDerivativeModelsIterator_->first ] = - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( - stateTypeSize_.at( stateDerivativeModelsIterator_->first ), 1 ); + { int currentStateTypeSize = 0; // Iterate over all state derivative models of current type @@ -361,16 +390,15 @@ class DynamicsStateDerivativeModel currentIndices = stateIndices_.at( stateDerivativeModelsIterator_->first ).at( i ); // Set current block in split state (in global form) - splitConventionalStates[ stateDerivativeModelsIterator_->first ].block( - currentStateTypeSize, 0, currentIndices.second, 1 ) = - stateDerivativeModelsIterator_->second.at( i ) - ->convertCurrentStateToGlobalRepresentation( - state.block( currentIndices.first, startColumn, - currentIndices.second, 1 ), time ); + stateDerivativeModelsIterator_->second.at( i )->convertCurrentStateToGlobalRepresentation( + state.block( currentIndices.first, startColumn, currentIndices.second, 1 ), time, + currentStatesPerTypeInConventionalRepresentation_.at( + stateDerivativeModelsIterator_->first ).block( + currentStateTypeSize, 0, currentIndices.second, 1 ) ); + currentStateTypeSize += currentIndices.second; } } - return splitConventionalStates; } //! Object used to update the environment to the current state and time. @@ -387,14 +415,12 @@ class DynamicsStateDerivativeModel std::map< IntegratedStateType, int > stateTypeStartIndex_; //! Complete list of state derivative models, sorted per state type. - std::map< IntegratedStateType, - std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > - stateDerivativeModels_; + std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > stateDerivativeModels_; //! Predefined iterator for computational efficiency. - typename std::map< IntegratedStateType, std::vector< boost::shared_ptr - < SingleStateTypeDerivative< StateScalarType, TimeType > > > >::iterator - stateDerivativeModelsIterator_; + typename std::unordered_map< IntegratedStateType, std::vector< boost::shared_ptr + < SingleStateTypeDerivative< StateScalarType, TimeType > > > >::iterator stateDerivativeModelsIterator_; //! Total length of state vector. int totalStateSize_; @@ -406,11 +432,20 @@ class DynamicsStateDerivativeModel //! Boolean denoting whether the equations of motion are to be propagated or not. bool evaluateDynamicsEquations_; + //! Boolean denoting whether the variational equations are to be propagated or not. + bool evaluateVariationalEquations_; + //! Start index in propagated matrix of the equations of motion (=0 if variational equations are //! not propagated). int dynamicsStartColumn_; + //! Current state derivative, as computed by computeStateDerivative. + StateType stateDerivative_; + //! Current state in 'conventional' representation, computed from current propagated state by + //! convertCurrentStateToGlobalRepresentationPerType + std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > + currentStatesPerTypeInConventionalRepresentation_; }; diff --git a/Tudat/Astrodynamics/Propagators/environmentUpdater.h b/Tudat/Astrodynamics/Propagators/environmentUpdater.h index 52a7f02302..ebd5db7af5 100644 --- a/Tudat/Astrodynamics/Propagators/environmentUpdater.h +++ b/Tudat/Astrodynamics/Propagators/environmentUpdater.h @@ -96,7 +96,7 @@ class EnvironmentUpdater */ void updateEnvironment( const TimeType currentTime, - const std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& + const std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& integratedStatesToSet, const std::vector< IntegratedStateType >& setIntegratedStatesFromEnvironment = std::vector< IntegratedStateType >( ) ) @@ -136,7 +136,7 @@ class EnvironmentUpdater { for( unsigned int i = 0; i < updateFunctionIterator->second.size( ); i++ ) { - updateFunctionIterator->second.at( i ).second( ); + updateFunctionIterator->second[ i ].second( ); } } @@ -148,7 +148,7 @@ class EnvironmentUpdater { for( unsigned int i = 0; i < updateTimeIterator->second.size( ); i++ ) { - updateTimeIterator->second.at( i ).second( static_cast< double >( currentTime ) ); + updateTimeIterator->second[ i ].second( static_cast< double >( currentTime ) ); } } } @@ -164,35 +164,43 @@ class EnvironmentUpdater * \param integratedStatesToSet Integrated states which are to be set in environment. */ void setIntegratedStatesInEnvironment( - const std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& + const std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& integratedStatesToSet ) { // Iterate over state types and set states in environment - for( typename std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator - integratedStateIterator = integratedStatesToSet.begin( ); - integratedStateIterator != integratedStatesToSet.end( ); - integratedStateIterator++ ) + for( integratedStateIterator_ = integratedStatesToSet.begin( ); + integratedStateIterator_ != integratedStatesToSet.end( ); + integratedStateIterator_++ ) { - - switch( integratedStateIterator->first ) + switch( integratedStateIterator_->first ) { case transational_state: { // Set translational states for bodies provided as input. - std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = - integratedStates_.at( transational_state ); - for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) + for( unsigned int i = 0; i < integratedStates_[ transational_state ].size( ); i++ ) { - bodyList_[ bodiesWithIntegratedStates[ i ].first ] - ->template setTemplatedState< StateScalarType >( - integratedStateIterator->second.segment( i * 6, 6 ) ); + bodyList_[ integratedStates_[ transational_state ][ i ].first ]->template + setTemplatedState< StateScalarType >( + integratedStateIterator_->second.segment( i * 6, 6 ) ); } break; }; + case body_mass_state: + { + // Set mass for bodies provided as input. + std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedMass = + integratedStates_.at( body_mass_state ); + + for( unsigned int i = 0; i < bodiesWithIntegratedMass.size( ); i++ ) + { + bodyList_[ bodiesWithIntegratedMass[ i ].first ] + ->setConstantBodyMass( integratedStateIterator_->second( i ) ); + } + break; + }; default: throw std::runtime_error( "Error, could not find integrated state settings for " + - boost::lexical_cast< std::string >( integratedStateIterator->first ) ); + boost::lexical_cast< std::string >( integratedStateIterator_->first ) ); } } } @@ -217,7 +225,7 @@ class EnvironmentUpdater { // Iterate over all integrated translational states. std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = - integratedStates_.at( transational_state ); + integratedStates_[ transational_state ]; for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) { bodyList_[ bodiesWithIntegratedStates[ i ].first ]-> @@ -226,6 +234,19 @@ class EnvironmentUpdater } break; } + case body_mass_state: + { + // Iterate over all integrated masses. + std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = + integratedStates_.at( body_mass_state ); + for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) + { + bodyList_[ bodiesWithIntegratedStates[ i ].first ]-> + updateMass( currentTime ); + + } + break; + } default: throw std::runtime_error( "Error, could not find state settings for " + boost::lexical_cast< std::string >( statesToSet.at( i ) ) ); @@ -269,7 +290,7 @@ class EnvironmentUpdater { bool addUpdate = 1; - // Check if translational state is propagated + // Check if mass is propagated if( integratedStates_.count( transational_state ) > 0 ) { // Check if current body is propagated @@ -325,11 +346,32 @@ class EnvironmentUpdater } case body_mass_update: + { + bool addUpdate = 1; + + // Check if translational state is propagated + if( integratedStates_.count( body_mass_state ) > 0 ) + { + // Check if current body is propagated + std::pair< std::string, std::string > bodyToCheck + = std::make_pair( currentBodies.at( i ), "" ); + std::vector< std::pair< std::string, std::string > > integratedBodyMasses + = integratedStates_.at( body_mass_state ); + if( std::find( integratedBodyMasses.begin( ), + integratedBodyMasses.end( ), + bodyToCheck ) != integratedBodyMasses.end( ) ) + { + addUpdate = 0; + } + } + + if( addUpdate ) { updateTimeFunctionList_[ body_mass_update ].push_back( - std::make_pair( currentBodies.at( i ), - boost::bind( &simulation_setup::Body::updateMass, - bodyList_.at( currentBodies.at( i ) ), _1 ) ) ); + std::make_pair( currentBodies.at( i ), + boost::bind( &simulation_setup::Body::updateMass, + bodyList_.at( currentBodies.at( i ) ), _1 ) ) ); + } break; } case spherical_harmonic_gravity_field_update: @@ -338,16 +380,16 @@ class EnvironmentUpdater // Check if body has time-dependent sh field boost::shared_ptr< gravitation::TimeDependentSphericalHarmonicsGravityField > gravityField = boost::dynamic_pointer_cast - < gravitation::TimeDependentSphericalHarmonicsGravityField > - ( bodyList_.at( currentBodies.at( i ) )->getGravityFieldModel( ) ); + < gravitation::TimeDependentSphericalHarmonicsGravityField > + ( bodyList_.at( currentBodies.at( i ) )->getGravityFieldModel( ) ); if( gravityField != NULL ) { updateTimeFunctionList_[ spherical_harmonic_gravity_field_update ].push_back( std::make_pair( currentBodies.at( i ), boost::bind( &gravitation - ::TimeDependentSphericalHarmonicsGravityField - ::update, + ::TimeDependentSphericalHarmonicsGravityField + ::update, gravityField, _1 ) ) ); } // If no sh field at all, throw eeror. @@ -399,7 +441,7 @@ class EnvironmentUpdater } else if( radiationPressureInterfaces.size( ) > 1 ) { - std::cerr<<"Request radiation pressure update of "< > > > updateTimeFunctionList_; - //! Predefined iterator for computational efficiency. - std::map< EnvironmentModelsToUpdate, - std::vector< std::pair< std::string, boost::function< void( const double ) > > > >::iterator - updateTimeIterator; + //! Predefined environment model iterator for computational efficiency. + std::map< EnvironmentModelsToUpdate, std::vector< std::pair< std::string, boost::function< void( const double ) > > > > + ::iterator updateTimeIterator; + + + //! Predefined state history iterator for computational efficiency. + typename std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator + integratedStateIterator_; }; diff --git a/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h b/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h index 0b843ceebb..b9d10fc31f 100644 --- a/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h +++ b/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h @@ -19,51 +19,86 @@ namespace tudat namespace propagators { +//! Class for computing the state derivative of translational motion of N bodies, using a Cowell propagator. template< typename StateScalarType = double, typename TimeType = double > class NBodyCowellStateDerivative: public NBodyStateDerivative< StateScalarType, TimeType > { public: + + //! Constructor + /*! + * Constructor + * \param accelerationModelsPerBody A map containing the list of accelerations acting on each + * body, identifying the body being acted on and the body acted on by an acceleration. The map + * has as key a string denoting the name of the body the list of accelerations, provided as the + * value corresponding to a key, is acting on. This map-value is again a map with string as + * key, denoting the body exerting the acceleration, and as value a pointer to an acceleration + * model. + * \param centralBodyData Object responsible for providing the current integration origins from + * the global origins. + * \param bodiesToIntegrate List of names of bodies that are to be integrated numerically. + */ NBodyCowellStateDerivative( const basic_astrodynamics::AccelerationMap& accelerationModelsPerBody, const boost::shared_ptr< CentralBodyData< StateScalarType, TimeType > > centralBodyData, const std::vector< std::string >& bodiesToIntegrate ): NBodyStateDerivative< StateScalarType, TimeType >( accelerationModelsPerBody, centralBodyData, cowell, bodiesToIntegrate ){ } + //! Destructor ~NBodyCowellStateDerivative( ){ } //! Calculates the state derivative of the translational motion of the system. /*! - * Calculates the state derivative (velocity+acceleration of each body) of the translational - * motion of the system at the given time and position/velocity of bodies. - * \param time Time (TDB seconds since J2000) at which the system is to be updated. - * \param stateOfSystemToBeIntegrated List of 6 * bodiesToBeIntegratedNumerically_.size( ), - * containing Caartesian position/velocity of the bodies being integrated. The order of the - * values is defined by the order of bodies in bodiesToBeIntegratedNumerically_ - * \return Current state derivative (velocity+acceleration) of system of bodies integrated numerically. + * Calculates the state derivative (velocity+acceleration of each body) of the translational motion of the system + * at the given time and position/velocity of bodies. + * \param time Time (TDB seconds since J2000) at which the system is to be updated. + * \param stateOfSystemToBeIntegrated List of 6 * bodiesToBeIntegratedNumerically_.size( ), containing Caartesian + * position/velocity of the bodies being integrated. The order of the values is defined by the order of bodies in + * bodiesToBeIntegratedNumerically_ + * \param stateDerivative Current state derivative (velocity+acceleration) of system of bodies integrated numerically + * (returned by reference). */ - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > calculateSystemStateDerivative( - const TimeType time, - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated ) + void calculateSystemStateDerivative( + const TimeType time, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) { - return ( this->sumStateDerivativeContributions( - stateOfSystemToBeIntegrated.template cast< double >( ), - time ) ).template cast< StateScalarType >( ); + stateDerivative.setZero( ); + this->sumStateDerivativeContributions( stateOfSystemToBeIntegrated, stateDerivative ); } + //! Function to convert the state in the conventional form to the propagator-specific form. + /*! + * Function to convert the state in the conventional form to the propagator-specific form. For the Cowell propagator, + * the two are equivalent, and this function returns the input state. + * \param cartesianSolution State in 'conventional form' + * \param time Current time at which the state is valid (not used in this class). + * \return State (outputSolution), converted to the 'propagator-specific form' (which is equal to outputSolution). + */ Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, - Eigen::Dynamic >& cartesianSolution, const TimeType& time ) + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& cartesianSolution, const TimeType& time ) { return cartesianSolution; } - Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > - convertToOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, - Eigen::Dynamic >& internalSolution, const TimeType& time ) + //! Function to convert the propagator-specific form of the state to the conventional form. + /*! + * Function to convert the propagator-specific form of the state to the conventional form. For the Cowell propagator, + * the two are equivalent, and this function returns the input state. + * In contrast to the convertCurrentStateToGlobalRepresentation function, this + * function does not provide the state in the inertial frame, but instead provides it in the + * frame in which it is propagated. + * \param internalSolution State in propagator-specific form (i.e. form that is used in + * numerical integration, equal to conventional form for this class). + * \param time Current time at which the state is valid (not used in this class). + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form', + * which is equal to outputSolution for this class (returned by reference). + */ + void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) { - return internalSolution; + currentCartesianLocalSoluton = internalSolution; } }; diff --git a/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h index 11db5dc3c3..cc9130fa80 100644 --- a/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h +++ b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h @@ -75,7 +75,28 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State centralBodyData_( centralBodyData ), propagatorType_( propagatorType ), bodiesToBeIntegratedNumerically_( bodiesToIntegrate ) - { } + { + // Add empty acceleration map if body is to be propagated with no accelerations. + for( unsigned int i = 0; i < bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + if( accelerationModelsPerBody_.count( bodiesToBeIntegratedNumerically_.at( i ) ) == 0 ) + { + accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_.at( i ) ] = + basic_astrodynamics::SingleBodyAccelerationMap( ); + } + } + + // Correct order of propagated bodies. + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); + outerAccelerationIterator++ ) + { + std::vector< std::string >::iterator findIterator = + std::find( bodiesToBeIntegratedNumerically_.begin( ), bodiesToBeIntegratedNumerically_.end( ), outerAccelerationIterator->first ); + bodyOrder_.push_back( std::distance( bodiesToBeIntegratedNumerically_.begin( ), findIterator ) ); + } + + } //! Destructor virtual ~NBodyStateDerivative( ){ } @@ -89,39 +110,34 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State */ void updateStateDerivativeModel( const TimeType currentTime ) { - // Reser all acceleration times (to allow multiple evaluations at same time, e.g. stage 2 - // and 3 in RK4 integrator) - for( accelerationMapIterator = accelerationModelsPerBody_.begin( ); - accelerationMapIterator != accelerationModelsPerBody_.end( ); - accelerationMapIterator++ ) + // Reset all acceleration times (to allow multiple evaluations at same time, e.g. stage 2 and 3 in RK4 integrator) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); outerAccelerationIterator++ ) { // Iterate over all accelerations acting on body - for( innerAccelerationIterator = accelerationMapIterator->second.begin( ); - innerAccelerationIterator != accelerationMapIterator->second.end( ); - innerAccelerationIterator++ ) + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); innerAccelerationIterator++ ) { // Update accelerations for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - innerAccelerationIterator->second[ j ]->resetTime( TUDAT_NAN ); } } } // Iterate over all accelerations and update their internal state. - for( accelerationMapIterator = accelerationModelsPerBody_.begin( ); - accelerationMapIterator != accelerationModelsPerBody_.end( ); accelerationMapIterator++ ) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); outerAccelerationIterator++ ) { // Iterate over all accelerations acting on body - for( innerAccelerationIterator = accelerationMapIterator->second.begin( ); - innerAccelerationIterator != accelerationMapIterator->second.end( ); + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); innerAccelerationIterator++ ) { // Update accelerations for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - innerAccelerationIterator->second[ j ]->updateMembers( currentTime ); } } @@ -137,23 +153,22 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the Cartesian state in inertial coordinates. + * \param currentCartesianLocalSoluton State (internalSolution), converted to the Cartesian state in inertial coordinates + * (returned by reference). */ - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > convertCurrentStateToGlobalRepresentation( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, - const TimeType& time ) + void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) { - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > cartesianLocalSolution - = this->convertToOutputSolution( internalSolution, time ); + this->convertToOutputSolution( internalSolution, time, currentCartesianLocalSoluton ); + + centralBodyData_->getReferenceFrameOriginInertialStates( + currentCartesianLocalSoluton, time, centralBodyInertialStates_, true ); - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > centralBodyInertialStates - = centralBodyData_->getReferenceFrameOriginInertialStates( cartesianLocalSolution, - time, true ); - for( unsigned int i = 0; i < centralBodyInertialStates.size( ); i++ ) + for( unsigned int i = 0; i < centralBodyInertialStates_.size( ); i++ ) { - cartesianLocalSolution.segment( i * 6, 6 ) += centralBodyInertialStates[ i ]; + currentCartesianLocalSoluton.segment( i * 6, 6 ) += centralBodyInertialStates_[ i ]; } - return cartesianLocalSolution; } //! Function to get list of names of bodies that are to be integrated numerically. @@ -215,46 +230,43 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State * and acceleration models must have been updated to the current state before calling this * function. * \param stateOfSystemToBeIntegrated Current Cartesian state of the system. - * \param time Time at which the state derivative is to be computed - * \return State derivative of the system in Cartesian coordinates. + * \param stateDerivative State derivative of the system in Cartesian coordinates (returned by reference). */ - Eigen::VectorXd sumStateDerivativeContributions( - const Eigen::VectorXd& stateOfSystemToBeIntegrated, const TimeType time ) + void sumStateDerivativeContributions( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) { using namespace basic_astrodynamics; - // Declare and initialize to zero vector to be returned. - Eigen::VectorXd stateDerivative = Eigen::VectorXd::Zero( stateOfSystemToBeIntegrated.size( ) ); + int currentBodyIndex = TUDAT_NAN; + int currentAccelerationIndex = 0; // Iterate over all bodies with accelerations. - for( unsigned int i = 0; i < bodiesToBeIntegratedNumerically_.size( ); i++ ) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); + outerAccelerationIterator++ ) { - // If body undergoes acceleration, calculate and add accelerations. - if( accelerationModelsPerBody_.count( bodiesToBeIntegratedNumerically_[ i ] ) != 0 ) - { + currentBodyIndex = bodyOrder_[ currentAccelerationIndex ]; + // Iterate over all accelerations acting on body + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); + innerAccelerationIterator++ ) + { + for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - // Iterate over all accelerations acting on body - for( innerAccelerationIterator = - accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_[ i ] ].begin( ); - innerAccelerationIterator != - accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_[ i ] ].end( ); - innerAccelerationIterator++ ) - { - for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) - { - // Calculate acceleration and add to state derivative. - stateDerivative.segment( i * 6 + 3, 3 ) += ( - innerAccelerationIterator->second[ j ]->getAcceleration( ) ); - } - } + // Calculate acceleration and add to state derivative. + stateDerivative.block( currentBodyIndex * 6 + 3, 0, 3, 1 ) += ( + innerAccelerationIterator->second[ j ]->getAcceleration( ) ). + template cast< StateScalarType >( ); } } + // Add body velocity as derivative of its position. - stateDerivative.segment( i * 6, 3 ) = stateOfSystemToBeIntegrated.segment( i * 6 + 3, 3 ); + stateDerivative.block( currentBodyIndex * 6, 0, 3, 1 ) = + ( stateOfSystemToBeIntegrated.segment( currentBodyIndex * 6 + 3, 3 ) ); + currentAccelerationIndex++; } - - return stateDerivative; } @@ -277,17 +289,23 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State //! List of names of bodies that are to be integrated numerically. std::vector< std::string > bodiesToBeIntegratedNumerically_; + std::vector< int > bodyOrder_; + //! Predefined iterator to save (de-)allocation time. - basic_astrodynamics::AccelerationMap::iterator accelerationMapIterator; + std::unordered_map< std::string, std::vector< + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > >::iterator innerAccelerationIterator; //! Predefined iterator to save (de-)allocation time. - std::map< std::string, std::vector< boost::shared_ptr< - basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > >::iterator - innerAccelerationIterator; + std::unordered_map< std::string, std::unordered_map< std::string, std::vector< + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > > >::iterator outerAccelerationIterator; + //! List of states of teh central bodies of the propagated bodies. + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > centralBodyInertialStates_; }; } // namespace propagators + } // namespace tudat + #endif // TUDAT_NBODYSTATEDERIVATIVE_H diff --git a/Tudat/Astrodynamics/Propagators/propagationSettings.cpp b/Tudat/Astrodynamics/Propagators/propagationSettings.cpp new file mode 100644 index 0000000000..4757ebdd18 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/propagationSettings.cpp @@ -0,0 +1,53 @@ +#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Get size of state for single propagated state of given type. +int getSingleIntegrationSize( const IntegratedStateType stateType ) +{ + int singleStateSize = 0; + switch( stateType ) + { + case transational_state: + singleStateSize = 6; + break; + case body_mass_state: + singleStateSize = 1; + break; + default: + std::string errorMessage = + "Did not recognize state type " + boost::lexical_cast< std::string >( stateType ) + "when getting size"; + throw std::runtime_error( errorMessage ); + } + return singleStateSize; +} + +//! Get order of differential equation for governing equations of dynamics of given type. +int getSingleIntegrationDifferentialEquationOrder( const IntegratedStateType stateType ) +{ + int singleStateSize = 0; + switch( stateType ) + { + case transational_state: + singleStateSize = 2; + break; + case body_mass_state: + singleStateSize = 1; + break; + default: + default: + std::string errorMessage = + "Did not recognize state type " + boost::lexical_cast< std::string >( stateType ) + "when getting order"; + throw std::runtime_error( errorMessage ); + } + return singleStateSize; +} + + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/propagationSettings.h b/Tudat/Astrodynamics/Propagators/propagationSettings.h index 5d5ef9a558..38550fade4 100644 --- a/Tudat/Astrodynamics/Propagators/propagationSettings.h +++ b/Tudat/Astrodynamics/Propagators/propagationSettings.h @@ -15,6 +15,7 @@ #include #include #include +#include #include @@ -22,6 +23,7 @@ #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" namespace tudat { @@ -33,15 +35,34 @@ namespace propagators //! Enum listing types of dynamics that can be numerically integrated enum IntegratedStateType { - transational_state + hybrid = 0, + transational_state = 1, + body_mass_state = 2 }; + //! Enum listing propagator types for translational dynamics that can be used. enum TranslationalPropagatorType { cowell = 0 }; +//! Get size of state for single propagated state of given type. +/*! + * Get size of state for single propagated state of given type (i.e. 6 for translational state). + * \param stateType Type of state + * \return Size of single state. + */ +int getSingleIntegrationSize( const IntegratedStateType stateType ); + +//! Get order of differential equation for governing equations of dynamics of given type. +/*! + * Get order of differential equation for governing equations of dynamics of given type (i.e. 2 for translational state). + * \param stateType Type of state + * \return Order of differential equations. + */ +int getSingleIntegrationDifferentialEquationOrder( const IntegratedStateType stateType ); + //! Base class for defining setting of a propagator /*! * Base class for defining setting of a propagator. This class is non-functional, and each state @@ -60,7 +81,7 @@ class PropagatorSettings */ PropagatorSettings( const IntegratedStateType stateType, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialBodyStates ): - stateType_( stateType ), initialStates_( initialBodyStates ){ } + stateType_( stateType ), initialStates_( initialBodyStates ), stateSize_( initialBodyStates.rows( ) ){ } //! Virtual destructor. virtual ~PropagatorSettings( ){ } @@ -87,6 +108,17 @@ class PropagatorSettings Eigen::Dynamic, 1 >& initialBodyStates ) { initialStates_ = initialBodyStates; + stateSize_ = initialStates_.rows( ); + } + + //! Get total size of the propagated state. + /*! + * Get total size of the propagated state. + * \return Total size of the propagated state. + */ + int getStateSize( ) + { + return stateSize_; } protected: @@ -94,6 +126,9 @@ class PropagatorSettings //! Initial state used as input for numerical integration Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialStates_; + //! Total size of the propagated state. + int stateSize_; + }; //! Class for defining settings for propagating translational dynamics. @@ -131,10 +166,7 @@ class TranslationalStatePropagatorSettings: public PropagatorSettings< StateScal centralBodies_( centralBodies ), accelerationsMap_( accelerationsMap ), bodiesToIntegrate_( bodiesToIntegrate ), propagator_( propagator ){ } - //! Virtual destructor - /*! - * Virtual destructor - */ + //! Destructor ~TranslationalStatePropagatorSettings( ){ } //! List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are propagated. @@ -158,6 +190,202 @@ class TranslationalStatePropagatorSettings: public PropagatorSettings< StateScal }; + +//! Class for defining settings for propagating the mass of a body +/*! + * Class for defining settings for propagating the mass of a body. The body masses are propagated in their natural + * form (i.e. no choice of equations of motion as is the case for translational dynamics)l + */ +template< typename StateScalarType > +class MassPropagatorSettings: public PropagatorSettings< StateScalarType > +{ +public: + + //! Constructor of mass state propagator settings + /*! + * Constructor of mass state propagator settings + * \param bodiesWithMassToPropagate List of bodies for which the mass is to be propagated. + * \param massRateModels List of mass rate models per propagated body. + * \param initialBodyMasses Initial masses used as input for numerical integration. + */ + MassPropagatorSettings( + const std::vector< std::string > bodiesWithMassToPropagate, + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyMasses ): + PropagatorSettings< StateScalarType >( body_mass_state, initialBodyMasses ), + bodiesWithMassToPropagate_( bodiesWithMassToPropagate ), massRateModels_( massRateModels ) + { } + + //! List of bodies for which the mass is to be propagated. + std::vector< std::string > bodiesWithMassToPropagate_; + + //! List of mass rate models per propagated body. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels_; +}; + +//! Function to retrieve the state size for a list of propagator settings. +/*! + * Function to retrieve the initial state for a list of propagator settings. + * \param propagatorSettingsList List of propagator settings (sorted by type as key). Map value provides list + * of propagator settings for given type. + * \return Vector of initial states, sorted in order of IntegratedStateType, and then in the order of the + * vector of PropagatorSettings of given type. + */ +template< typename StateScalarType > +int getMultiTypePropagatorStateSize( + const std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >& + propagatorSettingsList ) +{ + int stateSize = 0; + + // Iterate over all propagation settings and add size to list + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = propagatorSettingsList.begin( ); typeIterator != propagatorSettingsList.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + stateSize += typeIterator->second.at( i )->getStateSize( ); + } + } + return stateSize; +} + +//! Function to retrieve the initial state for a list of propagator settings. +/*! + * Function to retrieve the initial state for a list of propagator settings. + * \param propagatorSettingsList List of propagator settings (sorted by type as key). Map value provides list + * of propagator settings for given type. + * \return Vector of initial states, sorted in order of IntegratedStateType, and then in the order of the + * vector of PropagatorSettings of given type. + */ +template< typename StateScalarType > +Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > createCombinedInitialState( + const std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >& + propagatorSettingsList ) +{ + // Get total size of propagated state + int totalSize = getMultiTypePropagatorStateSize( propagatorSettingsList ); + + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > combinedInitialState = + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( totalSize, 1 ); + + // Iterate over all propagation settings and add to total list + int currentIndex = 0; + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > currentInitialState; + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = propagatorSettingsList.begin( ); typeIterator != propagatorSettingsList.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + currentInitialState = typeIterator->second.at( i )->getInitialStates( ); + combinedInitialState.segment( currentIndex, currentInitialState.rows( ) ) = currentInitialState; + currentIndex += currentInitialState.rows( ); + } + } + + return combinedInitialState; +} + +//! Class for defining settings for propagating multiple types of dynamics concurrently. +template< typename StateScalarType > +class MultiTypePropagatorSettings: public PropagatorSettings< StateScalarType > +{ +public: + + //! Constructor. + /*! + * Constructor + * \param propagatorSettingsMap List of propagator settings to use (state type as key). List of propagator settigns + * per type given as vector in map value. + */ + MultiTypePropagatorSettings( + const std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > > propagatorSettingsMap ): + PropagatorSettings< StateScalarType >( + hybrid, createCombinedInitialState< StateScalarType >( propagatorSettingsMap ) ), + propagatorSettingsMap_( propagatorSettingsMap ) + { } + + //! Constructor. + /*! + * Constructor + * \param propagatorSettingsVector Vector of propagator settings to use. + */ + MultiTypePropagatorSettings( + const std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > propagatorSettingsVector ): + PropagatorSettings< StateScalarType >( + hybrid, Eigen::VectorXd::Zero( 0 ) ) + { + for( unsigned int i = 0; i < propagatorSettingsVector.size( ); i++ ) + { + propagatorSettingsMap_[ propagatorSettingsVector.at( i )->stateType_ ].push_back( + propagatorSettingsVector.at( i ) ); + } + + this->initialStates_ = createCombinedInitialState< StateScalarType >( propagatorSettingsMap_ ); + } + + //! Destructor + ~MultiTypePropagatorSettings( ){ } + + //! Function to reset the initial state used as input for numerical integration + /*! + * Function to reset the initial state used as input for numerical integration + * \param initialBodyStates New initial state used as input for numerical integration, sorted in order of + * IntegratedStateType, and then in the order of the vector of PropagatorSettings of given type. + */ + void resetInitialStates( const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyStates ) + { + // Iterate over all propagator settings. + int currentStartIndex = 0; + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::iterator + propagatorIterator = propagatorSettingsMap_.begin( ); propagatorIterator != propagatorSettingsMap_.end( ); + propagatorIterator++ ) + { + for( unsigned int i = 0; i < propagatorIterator->second.size( ); i++ ) + { + // Get current state size + int currentParameterSize = propagatorIterator->second.at( i )->getInitialStates( ).rows( ); + + // Check consistency + if( currentParameterSize + currentStartIndex > initialBodyStates.rows( ) ) + { + throw std::runtime_error( + "Error when resetting multi-type state, sizes are incompatible " ); + } + + // Reset state for current settings + propagatorIterator->second.at( i )->resetInitialStates( + initialBodyStates.block( currentStartIndex, 0, currentParameterSize, 1 ) ); + currentStartIndex += currentParameterSize; + + } + } + + // Check consistency + if( currentStartIndex != initialBodyStates.rows( ) ) + { + std::string errorMessage = "Error when resetting multi-type state, total size is incompatible "+ + boost::lexical_cast< std::string >( currentStartIndex ) + + boost::lexical_cast< std::string >( initialBodyStates.rows( ) ); + throw std::runtime_error( errorMessage ); + } + } + + //! List of propagator settings to use + /*! + * List of propagator settings to use (state type as key). List of propagator settigns + * per type given as vector in map value. + */ + + std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > > + propagatorSettingsMap_; + +}; + template< typename StateScalarType > //! Function to retrieve the list of integrated state types and reference ids /*! @@ -176,9 +404,53 @@ std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string // Identify propagator type switch( propagatorSettings->stateType_ ) { - case transational_state: + case hybrid: { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > singleTypeIntegratedStateList; + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + if( typeIterator->first != hybrid ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + singleTypeIntegratedStateList = getIntegratedTypeAndBodyList< StateScalarType >( + typeIterator->second.at( i ) ); + + if( singleTypeIntegratedStateList.begin( )->first != typeIterator->first + || singleTypeIntegratedStateList.size( ) != 1 ) + { + std::cerr<<"Error when making integrated state list for hybrid propagator, inconsistency encountered "<< + singleTypeIntegratedStateList.begin( )->first<<" "<first<<" "<< + singleTypeIntegratedStateList.size( )<<" "<< + singleTypeIntegratedStateList.begin( )->second.size( )<first ].size( ); j++ ) + { + integratedStateList[ typeIterator->first ].push_back( + singleTypeIntegratedStateList.begin( )->second.at( j ) ); + } + + } + } + } + else + { + std::cerr<<"Error when making integrated state list, cannot handle hybrid propagator inside hybrid propagator"< > translationalPropagatorSettings = boost::dynamic_pointer_cast< TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); @@ -198,8 +470,29 @@ std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string break; } + case body_mass_state: + { + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > + massPropagatorSettings = boost::dynamic_pointer_cast< + MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( "Error getting integrated state type list, mass state input inconsistent" ); + } + + // Retrieve list of integrated bodies in correct formatting. + std::vector< std::pair< std::string, std::string > > integratedBodies; + for( unsigned int i = 0; i < massPropagatorSettings->bodiesWithMassToPropagate_.size( ); i++ ) + { + integratedBodies.push_back( std::make_pair( + massPropagatorSettings->bodiesWithMassToPropagate_.at( i ), "" ) ); + } + integratedStateList[ body_mass_state ] = integratedBodies; + + break; + } default: - throw std::runtime_error( "Error, could not process integrated state type " + + throw std::runtime_error( "Error, could not process integrated state type ingetIntegratedTypeAndBodyList " + boost::lexical_cast< std::string >( propagatorSettings->stateType_ ) ); } @@ -210,4 +503,23 @@ std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string } // namespace tudat +namespace std +{ + +//! Hash for IntegratedStateType enum. +template< > +struct hash< tudat::propagators::IntegratedStateType > +{ + typedef tudat::propagators::IntegratedStateType argument_type; + typedef size_t result_type; + + result_type operator () (const argument_type& x) const + { + using type = typename std::underlying_type::type; + return std::hash< type >( )( static_cast< type >( x ) ); + } +}; + +} + #endif // TUDAT_PROPAGATIONSETTINGS_H diff --git a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp b/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp index 095f2fcc2a..5f2b664673 100644 --- a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp +++ b/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp @@ -17,7 +17,6 @@ namespace tudat namespace propagators { - //! Function to create an interpolator for the new translational state of a body. template< > boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 6, 1 > > > diff --git a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h b/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h index 6761178415..a698ad82d3 100644 --- a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h +++ b/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h @@ -74,6 +74,7 @@ void resetIntegratedEphemerisOfBody( * integrated states Additionally, it changes the origin of the reference frame in which the states * are given, by using the integrationToEphemerisFrameFunction input variable. * \param bodyIndex Index of integrated body for which the state is to be retrieved + * \param startIndex Index in entries of equationsOfMotionNumericalSolution where the translational states start. * \param equationsOfMotionNumericalSolution Full numerical solution of numerical integrator, * already converted to Cartesian states (w.r.t. the integration origin of the body of bodyIndex) * \param integrationToEphemerisFrameFunction Function to provide the state of the ephemeris origin @@ -83,6 +84,7 @@ void resetIntegratedEphemerisOfBody( template< typename TimeType, typename StateScalarType > std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSolutionToEphemerisInput( const int bodyIndex, + const int startIndex, const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > @@ -98,7 +100,7 @@ std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSol bodyIterator = equationsOfMotionNumericalSolution.begin( ); bodyIterator != equationsOfMotionNumericalSolution.end( ); bodyIterator++ ) { - ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( 6 * bodyIndex, 0, 6, 1 ); + ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( startIndex + 6 * bodyIndex, 0, 6, 1 ); } } // Else, extract indices and add required translation from integrationToEphemerisFrameFunction @@ -109,7 +111,7 @@ std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSol bodyIterator != equationsOfMotionNumericalSolution.end( ); bodyIterator++ ) { - ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( 6 * bodyIndex, 0, 6, 1 ) - + ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( startIndex + 6 * bodyIndex, 0, 6, 1 ) - integrationToEphemerisFrameFunction( bodyIterator->first ); } } @@ -134,6 +136,7 @@ createStateInterpolator( * \param bodyMap List of bodies used in simulations. * \param bodiesToIntegrate List of names of bodies which are numericall integrated (in the order in * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndex Index in entries of equationsOfMotionNumericalSolution where the translational states start. * \param ephemerisUpdateOrder Order in which to update the ephemeris objects. * \param equationsOfMotionNumericalSolution Numerical solution of translational equations of * motion, in Cartesian elements w.r.t. integratation origins. @@ -144,14 +147,12 @@ template< typename TimeType, typename StateScalarType > void createAndSetInterpolatorsForEphemerides( const simulation_setup::NamedBodyMap& bodyMap, const std::vector< std::string >& bodiesToIntegrate, + const int startIndex, const std::vector< std::string >& ephemerisUpdateOrder, - const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& - equationsOfMotionNumericalSolution, - const std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, + const std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& integrationToEphemerisFrameFunctions = - std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) + std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) { using namespace tudat::interpolators; @@ -180,11 +181,12 @@ void createAndSetInterpolatorsForEphemerides( // Create and reset interpolator. boost::shared_ptr< OneDimensionalInterpolator< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > > ephemerisInterpolator = - createStateInterpolator( convertNumericalSolutionToEphemerisInput( - bodyIndex, equationsOfMotionNumericalSolution, - integrationToEphemerisFrameFunction ) ); - resetIntegratedEphemerisOfBody( bodyMap, ephemerisInterpolator, - bodiesToIntegrate.at( bodyIndex ) ); + createStateInterpolator( + convertNumericalSolutionToEphemerisInput( + bodyIndex, startIndex, equationsOfMotionNumericalSolution, integrationToEphemerisFrameFunction ) ); + + resetIntegratedEphemerisOfBody( + bodyMap, ephemerisInterpolator, bodiesToIntegrate.at( bodyIndex ) ); } } @@ -195,6 +197,8 @@ void createAndSetInterpolatorsForEphemerides( * \param bodyMap List of bodies used in simulations. * \param bodiesToIntegrate List of names of bodies which are numerically integrated (in the order in * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndexAndSize Pair with start index and total (contiguous) size of integrated states in entries of + * equationsOfMotionNumericalSolution * \param ephemerisUpdateOrder Order in which to update the ephemeris objects (empty if arbitrary). * \param equationsOfMotionNumericalSolution Numerical solution of translational equations of * motion, in Cartesian elements w.r.t. integratation origins. @@ -204,14 +208,13 @@ void createAndSetInterpolatorsForEphemerides( template< typename TimeType, typename StateScalarType > void resetIntegratedEphemerides( const simulation_setup::NamedBodyMap& bodyMap, - const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& - equationsOfMotionNumericalSolution, + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, const std::vector< std::string >& bodiesToIntegrate, + const std::pair< int, int > startIndexAndSize, std::vector< std::string > ephemerisUpdateOrder = std::vector< std::string >( ), - const std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& - integrationToEphemerisFrameFunctions = std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) + const std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& + integrationToEphemerisFrameFunctions = + std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) { // Set update order arbitrarily if no order is provided. if( ephemerisUpdateOrder.size( ) == 0 ) @@ -224,10 +227,67 @@ void resetIntegratedEphemerides( throw std::runtime_error( "Error when resetting ephemerides, input vectors have inconsistent size" ); } + if( equationsOfMotionNumericalSolution.begin( )->second.rows( ) < startIndexAndSize.first + startIndexAndSize.second ) + { + throw std::runtime_error( "Error when resetting ephemerides, input solution inconsistent with start index and size." ); + } + + if( startIndexAndSize.second != 6 * bodiesToIntegrate.size( ) ) + { + throw std::runtime_error( "Error when resetting ephemerides, number of bodies inconsistent with input size." ); + + } + // Create interpolators from numerical integration results (states) at discrete times. createAndSetInterpolatorsForEphemerides( - bodyMap, bodiesToIntegrate, ephemerisUpdateOrder, equationsOfMotionNumericalSolution, - integrationToEphemerisFrameFunctions ); + bodyMap, bodiesToIntegrate, startIndexAndSize.first, ephemerisUpdateOrder, + equationsOfMotionNumericalSolution, integrationToEphemerisFrameFunctions ); +} + +//! Resets the mass models of the integrated bodies from the numerical integration results. +/*! + * Resets the mass models of the integrated bodies from the numerical integration results. + * \param bodyMap List of bodies used in simulations. + * \param equationsOfMotionNumericalSolution Numerical solution of the body masses. + * \param bodiesToIntegrate List of names of bodies for which mass is numerically integrated (in the order in + * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndexAndSize Pair with start index and total (contiguous) size of integrated states in entries of + * equationsOfMotionNumericalSolution + */ +template< typename TimeType, typename StateScalarType > +void resetIntegratedBodyMass( + const simulation_setup::NamedBodyMap& bodyMap, + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, + const std::vector< std::string >& bodiesToIntegrate , + const std::pair< int, int > startIndexAndSize ) +{ + if( startIndexAndSize.second != bodiesToIntegrate.size( ) ) + { + throw std::runtime_error( "Error when resetting body masses, number of bodies inconsistent with input size." ); + } + + // Iterate over all bodies for which mass is propagated. + for( unsigned int i = 0; i < bodiesToIntegrate.size( ); i++ ) + { + std::map< double, double > currentBodyMassMap; + + // Create mass map with double entries. + for( typename std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator + stateIterator = equationsOfMotionNumericalSolution.begin( ); + stateIterator != equationsOfMotionNumericalSolution.end( ); stateIterator++ ) + { + currentBodyMassMap[ static_cast< double >( stateIterator->first ) ] = + static_cast< double >( stateIterator->second( startIndexAndSize.first + i ) ); + } + + typedef interpolators::OneDimensionalInterpolator< double, double > LocalInterpolator; + + // Create and set interpolator. + bodyMap.at( bodiesToIntegrate.at( i ) )->setBodyMassFunction( boost::bind( + static_cast< double( LocalInterpolator::* )( const double ) > + ( &LocalInterpolator::interpolate ), + boost::make_shared< interpolators::LagrangeInterpolatorDouble >( currentBodyMassMap, 6 ), _1 ) ); + } } //! Function to determine in which order the ephemerides are to be updated @@ -331,6 +391,8 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso bodiesToIntegrate_, frameManager ); } + ~TranslationalStateIntegratedStateProcessor( ){ } + //! Function processing translational state in the full numericalSolution /*! * Function that processes the entries of the translational state in the full numericalSolution, @@ -344,7 +406,7 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& numericalSolution ) { resetIntegratedEphemerides< TimeType, StateScalarType >( - bodyMap_, numericalSolution, bodiesToIntegrate_, ephemerisUpdateOrder_, + bodyMap_, numericalSolution, bodiesToIntegrate_, this->startIndexAndSize_, ephemerisUpdateOrder_, integrationToEphemerisFrameFunctions_ ); } @@ -369,6 +431,42 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso integrationToEphemerisFrameFunctions_; }; +template< typename TimeType, typename StateScalarType > +class BodyMassIntegratedStateProcessor: public IntegratedStateProcessor< TimeType, StateScalarType > +{ +public: + + BodyMassIntegratedStateProcessor( + const int startIndex, + const simulation_setup::NamedBodyMap& bodyMap, + const std::vector< std::string >& bodiesToIntegrate ): + IntegratedStateProcessor< TimeType, StateScalarType >( + body_mass_state, std::make_pair( startIndex, bodiesToIntegrate.size( ) ) ), + bodyMap_( bodyMap ), bodiesToIntegrate_( bodiesToIntegrate ) + { } + + ~BodyMassIntegratedStateProcessor( ){ } + + void processIntegratedStates( + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& numericalSolution ) + { + resetIntegratedBodyMass( bodyMap_, numericalSolution, bodiesToIntegrate_, this->startIndexAndSize_ ); + } + +private: + + //! List of bodies used in simulations. + simulation_setup::NamedBodyMap bodyMap_; + + //! List of bodies for which the body mass is numerically integrated. + /*! + * List of bodies for which the body mass is numerically integrated. Order in this + * vector is the same as the order in state vector. + */ + std::vector< std::string > bodiesToIntegrate_; +}; + + //! Function checking feasibility of resetting the translational dynamics /*! * Function to check the feasibility of resetting the translational dynamics of a set of @@ -466,6 +564,50 @@ createIntegratedStateProcessors( // Check dynamics type. switch( propagatorSettings->stateType_ ) { + case hybrid: + { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + std::map< IntegratedStateType, std::vector< boost::shared_ptr< IntegratedStateProcessor< TimeType, StateScalarType > > > > + singleTypeIntegratedStateProcessors; + int currentStartIndex = 0; + for( typename std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + if( typeIterator->first != hybrid ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + singleTypeIntegratedStateProcessors = createIntegratedStateProcessors< TimeType, StateScalarType >( + typeIterator->second.at( i ), bodyMap, frameManager, currentStartIndex ); + + if( singleTypeIntegratedStateProcessors.size( ) != 1 ) + { + throw std::runtime_error( "Error when making hybrid integrated result processors, multiple types found" ); + } + else + { + if( singleTypeIntegratedStateProcessors.begin( )->second.size( ) != 1 ) + { + throw std::runtime_error( "Error when making hybrid integrated result processors, multiple processors of single type found" ); + } + } + + currentStartIndex += typeIterator->second.at( i )->getStateSize( ); + integratedStateProcessors[ singleTypeIntegratedStateProcessors.begin( )->first ].push_back( + singleTypeIntegratedStateProcessors.begin( )->second.at( 0 ) ); + } + } + else + { + throw std::runtime_error( "Error when making integrated state processors, cannot handle hybrid propagator inside hybrid propagator" ); + } + + } + + break; + } case transational_state: { @@ -480,13 +622,31 @@ createIntegratedStateProcessors( checkTranslationalStatesFeasibility< TimeType, StateScalarType >( translationalPropagatorSettings->bodiesToIntegrate_, bodyMap ); - // Create state propagator settings + // Create state processors integratedStateProcessors[ transational_state ].push_back( boost::make_shared< TranslationalStateIntegratedStateProcessor< TimeType, StateScalarType > >( startIndex, bodyMap, translationalPropagatorSettings->bodiesToIntegrate_, translationalPropagatorSettings->centralBodies_, frameManager ) ); break; } + case body_mass_state: + { + + // Check input feasibility + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > + massPropagatorSettings = boost::dynamic_pointer_cast + < MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( "Error, input type is inconsistent in createIntegratedStateProcessors" ); + } + + // Create mass processors. + integratedStateProcessors[ body_mass_state ].push_back( + boost::make_shared< BodyMassIntegratedStateProcessor< TimeType, StateScalarType > >( + startIndex, bodyMap, massPropagatorSettings->bodiesWithMassToPropagate_ ) ); + break; + } default: throw std::runtime_error( "Error, could not process integrated state type " + boost::lexical_cast< std::string >( propagatorSettings->stateType_ ) ); diff --git a/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h index 125809348a..8fc906a7cd 100644 --- a/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h +++ b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h @@ -53,14 +53,15 @@ class SingleStateTypeDerivative * updated before calling this function. It returns the state derivative in teh form required * for the specific type of propagator used (defined by derived class). * \param time Time at which the state derivative is to be calculated. - * \param stateOfSystemToBeIntegrated Current state of the system, in the form that the - * equations are propagated (i.e. directly from numerical integrator) - * \return Derivative of the state of the system, in the form that the equations are propagated - * (i.e. to be piped directly to numerical integrator) + * \param stateOfSystemToBeIntegrated Current state of the system, in the form that the equations are propagated (i.e. + * directly from numerical integrator) + * \param stateDerivative Derivative of the state of the system, in the form that the equations are propagated + * (i.e. to be piped directly to numerical integrator), returned by reference. */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > calculateSystemStateDerivative( + virtual void calculateSystemStateDerivative( const TimeType time, - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated ) = 0; + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) = 0; //! Function to update the state derivative model to the current time. /*! @@ -82,11 +83,12 @@ class SingleStateTypeDerivative * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the 'conventional form' in inertial - * coordinates, that can for instance be set directly in the body object. + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form' in inertial + * coordinates, that can for instance be set directly in the body object (returned by reference). */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > convertCurrentStateToGlobalRepresentation( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time ) = 0; + virtual void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) = 0; //! Function to convert the state in the conventional form to the propagator-specific form. @@ -113,10 +115,12 @@ class SingleStateTypeDerivative * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the 'conventional form' + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form' (returned by + * reference). */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertToOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time ) = 0; + virtual void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) = 0; //! Function to return the size of the state handled by the object /*! diff --git a/Tudat/Basics/utilities.h b/Tudat/Basics/utilities.h new file mode 100644 index 0000000000..ce41b79572 --- /dev/null +++ b/Tudat/Basics/utilities.h @@ -0,0 +1,89 @@ +#ifndef UTILITIES_H +#define UTILITIES_H + +#include + +#include + +namespace tudat +{ + +namespace utilities +{ + +//! Function to create a vector from the values of a map +/*! + * Function to create a vector from the values of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator. + * The map keys are not used for the return vector. + * \param inputMap Original map from which the vector is to be created + * \return Vector created from the map values + */ +template< typename VectorArgument, typename KeyType > +std::vector< VectorArgument > createVectorFromMapValues( const std::map< KeyType, VectorArgument >& inputMap ) +{ + // Create and size return vector. + std::vector< VectorArgument > outputVector; + outputVector.resize( inputMap.size( ) ); + + // Iterate over all map entries and create vector + int currentIndex = 0; + for( typename std::map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( ); + mapIterator != inputMap.end( ); mapIterator++ ) + { + outputVector[ currentIndex ] = mapIterator->second; + currentIndex++; + } + + return outputVector; + +} + +//! Function to create a vector from the keys of a map +/*! + * Function to create a vector from the keys of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator. + * The map values are not used for the return vector. + * \param inputMap Original map from which the vector is to be created + * \return Vector created from the map keys + */ +template< typename VectorArgument, typename KeyType > +std::vector< KeyType > createVectorFromMapKeys( const std::map< KeyType, VectorArgument >& inputMap ) +{ + // Create and size return vector. + std::vector< KeyType > outputVector; + outputVector.resize( inputMap.size( ) ); + + // Iterate over all map entries and create vector + int currentIndex = 0; + for( typename std::map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( ); + mapIterator != inputMap.end( ); mapIterator++ ) + { + outputVector[ currentIndex ] = mapIterator->first; + currentIndex++; + } + + return outputVector; +} + +template< typename S, typename T > +void createVectorBlockMatrixHistory( + const std::map< S, Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > >& matrixHistory, + std::map< S, Eigen::Matrix< T, Eigen::Dynamic, 1 > >& blockMatrixHistory, + const std::pair< int, int > startIndices, const int segmentSize ) +{ + blockMatrixHistory.clear( ); + + // Loop over integration output and put results in corresponding data structures. + for( typename std::map< S, Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > >::const_iterator matrixIterator = matrixHistory.begin( ); + matrixIterator != matrixHistory.end( ); matrixIterator++ ) + { + // Set numerically integrated states of bodies. + blockMatrixHistory[ matrixIterator->first ] = + matrixIterator->second.block( startIndices.first, startIndices.second, segmentSize, 1 ); + } +} + +} + +} + +#endif // UTILITIES_H diff --git a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp index fa9a62634b..5c88449cd8 100644 --- a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp +++ b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp @@ -44,6 +44,7 @@ #include #include +#include #include @@ -74,6 +75,7 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) { // Declare test values vector. Vector12d computedTestValues; + Vector12d computedTestValuesDirect; // Define degree and order vectors. const Vector12i degree @@ -81,14 +83,21 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) const Vector12i order = ( Eigen::VectorXi( 12 ) << 0, 0, 1, 0, 1, 2, 0, 1, 2, 3, 0, 150 ).finished( ); + basic_mathematics::LegendreCache legendreCache = basic_mathematics::LegendreCache( 150, 150, 0 ); + // Define polynomial parameter. const double polynomialParameter = 0.5; + legendreCache.update( polynomialParameter ); // Loop through degrees and orders. for ( int index = 0; index < degree.size( ); index++ ) { // Compute test value of Legendre polynomial. computedTestValues( index ) = basic_mathematics::computeLegendrePolynomial( + degree( index ), + order( index ), + legendreCache ); + computedTestValuesDirect( index ) = basic_mathematics::computeLegendrePolynomial( degree( index ), order( index ), polynomialParameter ); @@ -118,12 +127,16 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValues, 1.0e-14 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValuesDirect, 1.0e-14 ); } BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) { // Declare test values vector. Vector10d computedTestValues; + Vector10d computedTestValuesDirect; + + basic_mathematics::LegendreCache legendreCache = basic_mathematics::LegendreCache( 4, 4, 1 ); // Define degree and order vectors. const Vector10i degree = ( Eigen::VectorXi( 10 ) << 0, 1, 1, 2, 2, 2, 3, 3, 3, 3 ).finished( ); @@ -132,14 +145,17 @@ BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) // Define polynomial parameter. const double polynomialParameter = 0.5; + legendreCache.update( polynomialParameter ); // Loop through degrees and orders. for ( int index = 0; index < degree.size( ); index++ ) { // Compute test value of Legendre polynomial. computedTestValues( index ) = basic_mathematics::computeGeodesyLegendrePolynomial( degree( index ), - order( index ), - polynomialParameter ); + order( index ), legendreCache ); + computedTestValuesDirect( index ) = basic_mathematics::computeGeodesyLegendrePolynomial( + degree( index ), + order( index ), polynomialParameter ); } // Set expected values. These values have been obtained from the MATLAB subfunction @@ -160,6 +176,8 @@ BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValuesDirect, 1.0e-15 ); + } BOOST_AUTO_TEST_CASE( test_LegendrePolynomialDerivative ) diff --git a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp index 30b6a35c7d..5b750a41ee 100644 --- a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp +++ b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp @@ -128,11 +128,34 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) -2.717133139910520 ).finished( ); + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 4, 4 ); + sphericalHarmonicsCache->update( + sphericalPosition( 0 ), std::sin( sphericalPosition( 1 ) ), sphericalPosition( 2 ), referenceRadius ); + // Compute to be tested potential gradient. Eigen::MatrixXd testPotentialGradient( 10, 3 ); + Eigen::MatrixXd testPotentialGradient2( 10, 3 ); + Eigen::MatrixXd testPotentialGradient3( 10, 3 ); + + // Compute potential gradients (using each of the three functions). for ( int index = 0; index < degree.size( ); index++ ) { - Eigen::Vector3d placeholder = basic_mathematics::computePotentialGradient( + testPotentialGradient.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( + sphericalPosition( 0 ), + std::pow( referenceRadius / sphericalPosition( 0 ), degree( index ) + 1 ), + std::cos( static_cast< double >( order( index ) ) * sphericalPosition( 2 ) ), + std::sin( static_cast< double >( order( index ) ) * sphericalPosition( 2 ) ), + std::cos( sphericalPosition( 1 ) ), + preMultiplier, + degree( index ), + order( index ), + cosineHarmonicCoefficient( index ), + sineHarmonicCoefficient( index ), + legendrePolynomial( index ), + legendrePolynomialDerivative( index ) ).transpose( ); + + testPotentialGradient2.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( sphericalPosition, referenceRadius, preMultiplier, @@ -141,10 +164,19 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) cosineHarmonicCoefficient( index ), sineHarmonicCoefficient( index ), legendrePolynomial( index ), - legendrePolynomialDerivative( index ) ); + legendrePolynomialDerivative( index ) ).transpose( ); + + testPotentialGradient3.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( + sphericalPosition, + preMultiplier, + degree( index ), + order( index ), + cosineHarmonicCoefficient( index ), + sineHarmonicCoefficient( index ), + legendrePolynomial( index ), + legendrePolynomialDerivative( index ), sphericalHarmonicsCache ).transpose( ); + - // Transpose test values matrix. - testPotentialGradient.row( index ) = placeholder.transpose( ); } // Define expected radius gradient values. @@ -197,6 +229,9 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient, expectedValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient2, expectedValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient3, expectedValues, 1.0e-15 ); + } BOOST_AUTO_TEST_SUITE_END( ) diff --git a/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h b/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h index 8556c4a52a..1550d395bc 100644 --- a/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h +++ b/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h @@ -102,6 +102,51 @@ ScalarType computeModulo( const ScalarType dividend, const ScalarType divisor ) return dividend - divisor * std::floor( dividend / divisor ); } +//! Raise floating point variable to integer power. +template< typename ScalarType > +ScalarType raiseToIntegerPower( const ScalarType baseValue, + const int integerPower ) +{ + // Declare local variable. + // Declare result of raising base to integer power. + // Initialise with value. + ScalarType resultOfRaisingBaseToIntegerPower = 1; + // Declare absolute value of integerPower. + int absoluteValueOfIntegerPower + = std::abs( integerPower ); + // Declare copy of base value. + ScalarType copyOfBaseValue = baseValue; + + // Compute the result here using exponentiation by squares. + // Stop loop when absolute value of integer power is equal to zero. + while ( absoluteValueOfIntegerPower ) + { + // Check that absolute value of integer power. + if ( absoluteValueOfIntegerPower & 1 ) + { + // Compute intermediate result. + resultOfRaisingBaseToIntegerPower *= copyOfBaseValue; + } + + // Divide integer power by two. + absoluteValueOfIntegerPower >>= 1; + + // Square base value. + copyOfBaseValue *= copyOfBaseValue; + } + + // Check if sign of integerPower is negative. + if ( integerPower < 0 ) + { + // Switch sign of result. + resultOfRaisingBaseToIntegerPower + = 1.0 / resultOfRaisingBaseToIntegerPower; + } + + // Return result of raising base to integer power. + return resultOfRaisingBaseToIntegerPower; +} + } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp index 8a636d40ea..237692aab7 100644 --- a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp +++ b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp @@ -48,16 +48,101 @@ namespace tudat namespace basic_mathematics { -// Define maximum size of Legendre polynomials back-end cache. -#ifndef MAXIMUM_CACHE_ENTRIES -#define MAXIMUM_CACHE_ENTRIES 12000 -#endif + + +//! Default constructor, initializes cache object with 0 maximum degree and order. +LegendreCache::LegendreCache( const bool useGeodesyNormalization ) +{ + useGeodesyNormalization_ = useGeodesyNormalization; + + if( useGeodesyNormalization_ ) + { + legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction; + } + else + { + legendrePolynomialFunction_ = regularLegendrePolynomialFunction; + } + + resetMaximumDegreeAndOrder( 0, 0 ); + +} + +//! Constructor +LegendreCache::LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization ) +{ + useGeodesyNormalization_ = useGeodesyNormalization; + + if( useGeodesyNormalization_ ) + { + legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction; + } + else + { + legendrePolynomialFunction_ = regularLegendrePolynomialFunction; + } + + resetMaximumDegreeAndOrder( maximumDegree, maximumOrder ); +} + +//! Get Legendre polynomial from cache when possible, and from direct computation otherwise. +void LegendreCache::update( const double polynomialParameter ) +{ + currentPolynomialParameter_ = polynomialParameter; + currentPolynomialParameterComplement_ = std::sqrt( 1.0 - polynomialParameter * polynomialParameter ); // cosine of latitude is always positive! + + LegendreCache& thisReference = *this; + for( int i = 0; i <= maximumDegree_; i++ ) + { + for( int j = 0; ( ( j <= i ) && ( j <= maximumOrder_ ) ) ; j++ ) + { + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] = legendrePolynomialFunction_( i, j, thisReference ); + } + } +} + +//! Update maximum degree and order of cache +void LegendreCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ) +{ + maximumDegree_ = maximumDegree; + maximumOrder_ = maximumOrder; + legendreValues_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) ); + + currentPolynomialParameter_ = TUDAT_NAN; + currentPolynomialParameterComplement_ = TUDAT_NAN; +} + + +//! Get Legendre polynomial value from the cache. +double LegendreCache::getLegendrePolynomial( + const int degree, const int order ) +{ + if( degree > maximumDegree_ || order > maximumOrder_ ) + { + std::cerr<<"Error when requesting legendre cache, maximum degree or order exceeded "<< + degree<<" "< degree ) + { + return 0.0; + } + else + { + return legendreValues_[ degree * ( maximumOrder_ + 1 ) + order ]; + }; +} //! Compute unnormalized associated Legendre polynomial. double computeLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ) + LegendreCache& legendreCache ) { + if( legendreCache.getUseGeodesyNormalization( ) ) + { + throw std::runtime_error( "Error when computing Legendre polynomial, input uses normalization" ); + } + // If degree or order is negative... if ( degree < 0 || order < 0 ) { @@ -82,19 +167,19 @@ double computeLegendrePolynomial( const int degree, else if ( degree <= 1 && order <= 1 ) { // Compute polynomial explicitly. - return computeLegendrePolynomialExplicit( degree, order, polynomialParameter ); + return computeLegendrePolynomialExplicit( degree, order, legendreCache.getCurrentPolynomialParameter( ) ); } // Else if degree and order are sectoral... else if ( degree == order ) { // Obtain polynomial of degree one and order one. - const double degreeOneOrderOnePolynomial = legendreCache.getOrElseUpdate( - 1, 1, polynomialParameter, &computeLegendrePolynomial ); + const double degreeOneOrderOnePolynomial = legendreCache.getLegendrePolynomial( + 1, 1 ); // Obtain prior sectoral polynomial. - const double priorSectoralPolynomial = legendreCache.getOrElseUpdate( - degree - 1, order - 1, polynomialParameter, &computeLegendrePolynomial ); + const double priorSectoralPolynomial = legendreCache.getLegendrePolynomial( + degree - 1, order - 1 ); // Compute polynomial. return computeLegendrePolynomialDiagonal( @@ -105,27 +190,44 @@ double computeLegendrePolynomial( const int degree, else { // Obtain prior degree polynomial. - const double oneDegreePriorPolynomial = legendreCache.getOrElseUpdate( - degree - 1, order, polynomialParameter, &computeLegendrePolynomial ); + const double oneDegreePriorPolynomial = legendreCache.getLegendrePolynomial( + degree - 1, order ); // Obtain two degrees prior polynomial. - const double twoDegreesPriorPolynomial = legendreCache.getOrElseUpdate( - degree - 2, order, polynomialParameter, &computeLegendrePolynomial ); + const double twoDegreesPriorPolynomial = legendreCache.getLegendrePolynomial( + degree - 2, order ); // Compute polynomial. return computeLegendrePolynomialVertical( degree, order, - polynomialParameter, + legendreCache.getCurrentPolynomialParameter( ), oneDegreePriorPolynomial, twoDegreesPriorPolynomial ); } } + +double computeLegendrePolynomial( const int degree, + const int order, + const double legendreParameter ) +{ + LegendreCache legendreCache( degree, order, 0 ); + legendreCache.update( legendreParameter ); + return computeLegendrePolynomial( degree, order, legendreCache ); +} + + //! Compute geodesy-normalized associated Legendre polynomial. double computeGeodesyLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ) + LegendreCache& geodesyLegendreCache ) { + + if( !geodesyLegendreCache.getUseGeodesyNormalization( ) ) + { + throw std::runtime_error( "Error when computing Legendre polynomial, input uses no normalization" ); + } + // If degree or order is negative... if ( degree < 0 || order < 0 ) { @@ -150,19 +252,19 @@ double computeGeodesyLegendrePolynomial( const int degree, else if ( degree <= 1 && order <= 1 ) { // Compute polynomial explicitly. - return computeGeodesyLegendrePolynomialExplicit( degree, order, polynomialParameter ); + return computeGeodesyLegendrePolynomialExplicit( degree, order, geodesyLegendreCache.getCurrentPolynomialParameter( ) ); } // Else if degree and order are sectoral... else if ( degree == order ) { // Obtain polynomial of degree one and order one. - double degreeOneOrderOnePolynomial = geodesyLegendreCache.getOrElseUpdate( - 1, 1, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double degreeOneOrderOnePolynomial = geodesyLegendreCache.getLegendrePolynomial( + 1, 1 ); // Obtain prior sectoral polynomial. - double priorSectoralPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 1, order - 1, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double priorSectoralPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 1, order - 1 ); // Compute polynomial. return computeGeodesyLegendrePolynomialDiagonal( @@ -173,22 +275,31 @@ double computeGeodesyLegendrePolynomial( const int degree, else { // Obtain prior degree polynomial. - double oneDegreePriorPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 1, order, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double oneDegreePriorPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 1, order ); // Obtain two degrees prior polynomial. - double twoDegreesPriorPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 2, order, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double twoDegreesPriorPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 2, order ); // Compute polynomial. return computeGeodesyLegendrePolynomialVertical( degree, order, - polynomialParameter, + geodesyLegendreCache.getCurrentPolynomialParameter( ), oneDegreePriorPolynomial, twoDegreesPriorPolynomial ); } } +double computeGeodesyLegendrePolynomial( const int degree, + const int order, + const double legendreParameter ) +{ + LegendreCache legendreCache( degree, order, 1 ); + legendreCache.update( legendreParameter ); + return computeGeodesyLegendrePolynomial( degree, order, legendreCache ); +} + //! Compute derivative of unnormalized Legendre polynomial. double computeLegendrePolynomialDerivative( const int order, const double polynomialParameter, @@ -407,119 +518,14 @@ double computeGeodesyLegendrePolynomialVertical( const int degree, { // Return polynomial. return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 ) - / ( static_cast< double >( degree + order ) ) - / ( static_cast< double >( degree - order ) ) ) - * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter - * oneDegreePriorPolynomial + / ( ( static_cast< double >( degree + order ) ) * ( static_cast< double >( degree - order ) ) ) ) + * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter * oneDegreePriorPolynomial - std::sqrt( ( static_cast< double >( degree + order ) - 1.0 ) * ( static_cast< double >( degree - order ) - 1.0 ) / ( 2.0 * static_cast< double >( degree ) - 3.0 ) ) * twoDegreesPriorPolynomial ); } -//! Define overloaded 'equals' operator for use with 'Point' structure. -bool operator==( const Point& polynomialArguments1, const Point& polynomialArguments2 ) -{ - bool equal = polynomialArguments1.degree == polynomialArguments2.degree - && polynomialArguments1.order == polynomialArguments2.order - && polynomialArguments1.polynomialParameter - == polynomialArguments2.polynomialParameter; - - return equal; -} - -//! Set hash value. -std::size_t hash_value( Point const& polynomialArguments ) -{ - std::size_t seed = 0; - boost::hash_combine( seed, polynomialArguments.degree ); - boost::hash_combine( seed, polynomialArguments.order ); - boost::hash_combine( seed, polynomialArguments.polynomialParameter ); - - return seed; -} - -//! Get Legendre polynomial from cache when possible, and from direct computation otherwise. -double LegendreCache::getOrElseUpdate( - const int degree, const int order, const double polynomialParameter, - const LegendrePolynomialFunction legendrePolynomialFunction ) -{ - // Initialize structure with polynomial arguments. - Point polynomialArguments( degree, order, polynomialParameter ); - - // Initialize cache iterator. - CacheTable::iterator cachedEntry = backendCache.find( polynomialArguments ); - - // If the requested polynomial was not found in cache, compute polynomial. - if ( cachedEntry == backendCache.end( ) ) - { - double legendrePolynomial = legendrePolynomialFunction( degree, order, - polynomialParameter ); - // If cache is full, remove the oldest element. - if ( history.full( ) ) - { - backendCache.erase( backendCache.find( history[ 0 ] ) ); - history.pop_front( ); - } - - // Insert computed polynomial into cache. - backendCache.insert( std::pair< Point, double >( polynomialArguments, - legendrePolynomial ) ); - history.push_back( polynomialArguments ); - - // Return polynomial value from computation. - return legendrePolynomial; - } - - // Else the requested polynomial was found in cache; return polynomial value from cache entry. - else - { - return cachedEntry->second; - } -} - -//! Initialize LegendreCache objects. -LegendreCache::LegendreCache( ) : history( MAXIMUM_CACHE_ENTRIES ) { } - -//! Write contents of Legendre polynomial structure to string. -std::string writeLegendrePolynomialStructureToString( const Point legendrePolynomialStructure ) -{ - std::stringstream buffer; - - buffer << "(" << legendrePolynomialStructure.degree << ", " - << legendrePolynomialStructure.order << ", " - << legendrePolynomialStructure.polynomialParameter << ")"; - - return buffer.str( ); -} - -//! Dump Legendre polynomial cache data to stream (table and history). -void dumpLegendrePolynomialCacheData( std::ostream& outputStream, - boost::unordered_map< Point, double > cacheTable, - boost::circular_buffer< Point > cacheHistory ) -{ - outputStream << "Table:\n"; - - for ( boost::unordered_map< Point, double >::iterator iteratorCacheTable = cacheTable.begin( ); - iteratorCacheTable != cacheTable.end( ); iteratorCacheTable++ ) - { - outputStream << "\t" << writeLegendrePolynomialStructureToString( - iteratorCacheTable->first ).c_str( ) << " => " - << iteratorCacheTable->second << std::endl; - } - - outputStream << "History:\n"; - - for ( boost::circular_buffer< Point >::iterator iteratorCacheHistory = cacheHistory.begin( ); - iteratorCacheHistory != cacheHistory.end( ); iteratorCacheHistory++ ) - { - outputStream << "\t" - << writeLegendrePolynomialStructureToString( *iteratorCacheHistory ).c_str( ) - << ", "; - } - - outputStream << std::endl; -} //! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized. double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order ) @@ -538,5 +544,6 @@ double calculateLegendreGeodesyNormalizationFactor( const int degree, const int return 1.0 / factor; } + } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h index 075897bd23..bc2d9c0ec0 100644 --- a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h +++ b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h @@ -55,16 +55,189 @@ #include #include +#include + #include #include #include #include +#include + +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" namespace tudat { namespace basic_mathematics { +//! Class for creating and accessing a back-end cache of Legendre polynomials. +class LegendreCache +{ + +public: + + //! Define Legendre polynomial function pointer. + typedef boost::function< double ( int, int, LegendreCache& ) > LegendrePolynomialFunction; + + //! Default constructor, initializes cache object with 0 maximum degree and order. + /*! + * Default constructor, initializes cache object with 0 mazimum degree and order. + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + LegendreCache( const bool useGeodesyNormalization = 1 ); + + //! Constructor + /*! + * Constructor + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization = 1 ); + + //! Update maximum degree and order of cache + /*! + * Update maximum degree and order of cache + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + */ + void resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ); + + //! Update cache with new polynomial parameter (sine of latitude) + /*! + * Update cache with new polynomial parameter (sine of latitude). + * \param polynomialParameter Parameter used as input argument for Legendre polynomials, in astrodynamics + * applications, this is typically the sine of the body-fixed latitude. + */ + void update( const double polynomialParameter ); + + //! Function to return the current polynomial parameter (typically sine of latitude) + /*! + * Function to return the current polynomial parameter (typically sine of latitude) + * \return Current polynomial parameter + */ + double getCurrentPolynomialParameter( ) + { + return currentPolynomialParameter_; + } + + //! Function to return the complement to the current polynomial parameter (typically cosine of latitude) + /*! + * Function to return the complement to the current polynomial parameter (typically cosine of latitude) + * \return Complement to the current polynomial parameter + */ + double getCurrentPolynomialParameterComplement( ) + { + return currentPolynomialParameterComplement_; + } + + //! Get Legendre polynomial value from the cache. + /*! + * Get Legendre polynomial value from the cache, as computed by last call to update function. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \return Legendre polynomial value. + */ + double getLegendrePolynomial( const int degree, const int order ); + + //! Function to get the maximum degree of cache. + /*! + * Function to get the maximum degree of cache + * \return Maximum degree of cache. + */ + int getMaximumDegree( ) + { + return maximumDegree_; + } + + //! Function to get the maximum order of cache. + /*! + * Function to get the maximum order of cache. + * \return Maximum order of cache. + */ + int getMaximumOrder( ) + { + return maximumOrder_; + } + + //! Function to get whether the Legendre polynomials are geodesy-normalized or unnormalized + /*! + * Function to get whether the Legendre polynomials are geodesy-normalized or unnormalized + * \return Boolean denoting to get whether the Legendre polynomials are geodesy-normalized or unnormalized + */ + bool getUseGeodesyNormalization( ) + { + return useGeodesyNormalization_; + } + + +private: + + //! Maximum degree of cache. + int maximumDegree_; + + //! Maximum order of cache. + int maximumOrder_; + + //! Current polynomial parameter (sine of latitude). + double currentPolynomialParameter_; + + //! Current 'complement' to polynomial parameter (cosine of latitude). + double currentPolynomialParameterComplement_; + + //! List of current values of Legendre polynomials at degree and order (n,m) + /*! + * List of current values of Legendre polynomials at degree and order (n,m). The corresponding polynomial is at entry + * n * ( maximumOrder_ + 1 ) + m. + */ + std::vector< double > legendreValues_; + + //! Function from which to compute the Legendre polynomials. + LegendrePolynomialFunction legendrePolynomialFunction_; + + //! Boolean denoting whether the Legendre polynomials are geodesy-normalized or unnormalized + bool useGeodesyNormalization_; + + std::vector< double > referenceRadiusRatioPowers_; + + +}; + + + +//! Compute unnormalized associated Legendre polynomial. +/*! + * This function returns an unnormalized associated Legendre polynomial \f$ P _{ n, m }( u ) \f$ + * with degree \f$ n \f$, order \f$ m \f$ and polynomial parameter \f$ u \f$. + * \f$ P _{ n, m }( u ) \f$ obeys the definition given by Vallado [2001]: + * \f[ + * P _{ n, m } ( u ) = ( 1 - u ^ 2 ) ^ { m / 2 } \frac{ d ^ m }{ du ^ m } \left[ P_n ( u ) + * \right] \textrm{ for }n \geq 0, 0 \leq m \leq n + * \f] + * in which \f$ P_{ n, m }( u ) \f$ is the ordinary Legendre polynomial with degree \f$ n \f$, + * order \f$ m \f$ and polynomial parameter \f$ u \f$. + * + * For \f$ n \geq 0, m \geq n \f$ the \f$ P _{ n, m }( u ) \f$ has been defined here as + * follows: + * \f[ + * P _{ n, m } ( u ) = 0 \textrm{ for }n \geq 0, m \geq n + * \f] + * + * This function has been optimized for repeated calls with varying 'degree' and 'order' arguments + * (but with identical 'polynomialParameter' argument). To this end the function maintains a + * back-end cache with intermediate results which is automatically carried over between calls. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \param legendreCache Legendre cache from which to retrieve Legendre polynomial. + * \return Unnormalized Legendre polynomial. +*/ +double computeLegendrePolynomial( const int degree, + const int order, + LegendreCache& legendreCache ); + + //! Compute unnormalized associated Legendre polynomial. /*! * This function returns an unnormalized associated Legendre polynomial \f$ P _{ n, m }( u ) \f$ @@ -88,12 +261,13 @@ namespace basic_mathematics * back-end cache with intermediate results which is automatically carried over between calls. * \param degree Degree of requested Legendre polynomial. * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. + * \param legendreParameter Free variable of requested Legendre polynomial. * \return Unnormalized Legendre polynomial. */ double computeLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ); + const double legendreParameter ); + //! Compute geodesy-normalized associated Legendre polynomial. /*! @@ -131,12 +305,55 @@ double computeLegendrePolynomial( const int degree, * back-end cache with intermediate results which is automatically carried over between calls. * \param degree Degree of requested Legendre polynomial. * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. + * \param geodesyLegendreCache Legendre cache from which to retrieve Legendre polynomial. + * \return Geodesy-normalized Legendre polynomial. +*/ +double computeGeodesyLegendrePolynomial( const int degree, + const int order, + LegendreCache& geodesyLegendreCache ); + +//! Compute geodesy-normalized associated Legendre polynomial. +/*! + * This function returns a normalized associated Legendre polynomial + * \f$ \bar{ P }_{ n, m }( u ) \f$ with degree \f$ n \f$, order \f$ m \f$ and polynomial + * parameter \f$ u \f$. The normalization obeys the definition: + * \f[ + * \bar{ P }_{ n, m } ( u ) = \Pi_{ n, m } P_{ n, m } ( u ) + * \f] + * in which \f$ \Pi_{ n, m } \f$ is the normalization factor which is commonly used in geodesy and + * is given by Heiskanen & Moritz [1967] as: + * \f[ + * \Pi_{ n, m } = \sqrt{ \frac{ ( 2 - \delta_{ 0, m } ) ( 2 n + 1 ) ( n - m )! } + * { ( n + m )! } } + * \f] + * in which \f$ n \f$ is the degree, \f$ m \f$ is the order and \f$ \delta_{ 0, m } \f$ is the + * Kronecker delta. + + * \f$ P _{ n, m }( u ) \f$ obeys the definition given by Vallado [2001]: + * \f[ + * P _{ n, m } ( u ) = ( 1 - u ^ 2 ) ^ { m / 2 } \frac{ d ^ m }{ du ^ m } \left[ P_n ( u ) + * \right] + * \f] + * in which \f$ P_{ n, m }( u ) \f$ is the ordinary Legendre polynomial with degree \f$ n \f$, + * order \f$ m \f$ and polynomial parameter \f$ u \f$. + * + * For \f$ n \geq 0, m \geq n \f$ the \f$ P _{ n, m }( u ) \f$ has been defined here as + * follows: + * \f[ + * P _{ n, m } ( u ) = 0 \textrm{ for }n \geq 0, m \geq n + * \f] + * + * This function has been optimized for repeated calls with varying 'degree' and 'order' arguments + * (but with identical 'polynomialParameter' argument). To this end the function maintains a + * back-end cache with intermediate results which is automatically carried over between calls. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \param legendreParameter Free variable of requested Legendre polynomial. * \return Geodesy-normalized Legendre polynomial. */ double computeGeodesyLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ); + const double legendreParameter ); //! Compute derivative of unnormalized Legendre polynomial. /*! @@ -320,111 +537,11 @@ double computeGeodesyLegendrePolynomialVertical( const int degree, const double oneDegreePriorPolynomial, const double twoDegreesPriorPolynomial ); -//! Declare structure for arguments of Legendre polynomial (for use in back-end cache). -struct Point -{ -public: - - //! Self-referential structure. - Point( const int aDegree, const int anOrder, const double aPolynomialParameter ) - : degree( aDegree ), - order( anOrder ), - polynomialParameter( aPolynomialParameter ) - { } - - //! Degree of Legendre polynomial. - int degree; - //! Order of Legendre polynomial. - int order; - - //! Polynomial parameter of Legendre polynomial. - double polynomialParameter; - -protected: - -private: -}; - -//! Typedef for shared-pointer to Point object. -typedef boost::shared_ptr< Point > PointPointer; - -//! Define overloaded 'equals' operator for use with 'Point' structure. -bool operator==( const Point& polynomialArguments1, const Point& polynomialArguments2 ); - -//! Set hash value. -std::size_t hash_value( Point const& polynomialArguments ); - -//! Class for creating and accessing a back-end cache of Legendre polynomials. -class LegendreCache -{ -private: - - //! Define Legendre polynomial function pointer. - typedef boost::function< double ( int, int, double ) > LegendrePolynomialFunction; - - //! Define map variables type. - typedef boost::unordered_map< Point, double > CacheTable; - - //! Define buffer variables type. - typedef boost::circular_buffer< Point > CacheHistory; - -public: - - //! Initialize LegendreCache instance. - LegendreCache( ); - - //! Get Legendre polynomial value from either cache or from computation. - /*! - * \param degree Degree of requested Legendre polynomial. - * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. - * \param legendrePolynomialFunction Function which takes degree, order and - * polynomialParameter as arguments. The function must return the corresponding - * Legendre polynomial value. - * \return Legendre polynomial value. - */ - double getOrElseUpdate( const int degree, const int order, const double polynomialParameter, - const LegendrePolynomialFunction legendrePolynomialFunction ); - -private: - - //! Hashmap which links a specific degree, order and polynomial parameter to its - //! corresponding Legendre polynomial value. - CacheTable backendCache; - - //! History buffer. - CacheHistory history; -}; - -//! Typedef shared-pointer to LegendreCache object. -typedef boost::shared_ptr< LegendreCache > LegendreCachePointer; - -//! Global instances of LegendreCache class for unnormalized polynomials. -static LegendreCache legendreCache; - -//! Global instances of LegendreCache class for geodesy-normalized polynomials. -static LegendreCache geodesyLegendreCache; - -//! Write contents of Legendre polynomial structure to string. -/*! - * Writes contents of Legendre polynomial structure, containing degree, order, and value of - * polynomial parameter, to string. - * \param legendrePolynomialStructure Structure containing legendre polynomial data. - * \return String containing degree, order, and value of polynomial paramter stored in structure. - */ -std::string writeLegendrePolynomialStructureToString( const Point legendrePolynomialStructure ); - -//! Dump Legendre polynomial cache data to stream (table and history). -/*! - * Dumps cached table and history data for Legendre polynomials to given output stream. - * \param outputStream Output stream. - * \param cacheTable Cache table containing current values of Legendre polynomials. - * \param cacheHistory Cached history of Legendre polynomials. - */ -void dumpLegendrePolynomialCacheData( std::ostream& outputStream, - boost::unordered_map< Point, double > cacheTable, - boost::circular_buffer< Point > cacheHistory ); +//! Predefine boost function for geodesy-normalized Legendre polynomial. +static const LegendreCache::LegendrePolynomialFunction geodesyNormalizedLegendrePolynomialFunction = + boost::bind( static_cast< double(&)( const int, const int, LegendreCache& )>( + &computeGeodesyLegendrePolynomial ), _1, _2, _3 ); //! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized. /*! @@ -438,6 +555,10 @@ void dumpLegendrePolynomialCacheData( std::ostream& outputStream, */ double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order ); +//! Predefine boost function for unnormalized Legendre polynomial. +const LegendreCache::LegendrePolynomialFunction regularLegendrePolynomialFunction = + boost::bind( static_cast< double(&)( const int, const int, LegendreCache& )>( + &computeLegendrePolynomial ), _1, _2, _3 ); } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp index f9e02e6628..20031c8cf8 100644 --- a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp +++ b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp @@ -37,13 +37,60 @@ #include #include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" namespace tudat { namespace basic_mathematics { -// Compute the gradient of a single term of a spherical harmonics potential field. +//! Update maximum degree and order of cache +void SphericalHarmonicsCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ) +{ + maximumDegree_ = maximumDegree; + maximumOrder_ = maximumOrder; + + legendreCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ ); + + sinesOfLongitude_.resize( maximumOrder_ + 1 ); + cosinesOfLongitude_.resize( maximumOrder_ + 1 ); + referenceRadiusRatioPowers_.resize( maximumDegree_ + 2 ); +} + + +//! Compute the gradient of a single term of a spherical harmonics potential field. +Eigen::Vector3d computePotentialGradient( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ) +{ + // Return result. + return ( Eigen::Vector3d( ) << + - preMultiplier / distance + * radiusPowerTerm + * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial + * ( cosineHarmonicCoefficient * cosineOfOrderLongitude + + sineHarmonicCoefficient * sineOfOrderLongitude ), + preMultiplier * radiusPowerTerm + * legendrePolynomialDerivative * cosineOfLatitude * ( + cosineHarmonicCoefficient * cosineOfOrderLongitude + + sineHarmonicCoefficient * sineOfOrderLongitude ), + preMultiplier * radiusPowerTerm + * static_cast< double >( order ) * legendrePolynomial + * ( sineHarmonicCoefficient * cosineOfOrderLongitude + - cosineHarmonicCoefficient * sineOfOrderLongitude ) ).finished( ); +} + +//! Compute the gradient of a single term of a spherical harmonics potential field. Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, const double referenceRadius, @@ -55,41 +102,35 @@ Eigen::Vector3d computePotentialGradient( const double legendrePolynomial, const double legendrePolynomialDerivative ) { - // Initialize return variable. - Eigen::Vector3d potentialGradient; - - // Compute radius power term. - const double radiusPowerTerm = std::pow( referenceRadius / sphericalPosition( radiusIndex ), - static_cast< double >( degree ) + 1.0 ); - - // Calculate derivative with respect to radius. - potentialGradient( radiusIndex ) = - preMultiplier / sphericalPosition( radiusIndex ) - * radiusPowerTerm - * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial - * ( cosineHarmonicCoefficient - * std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ) - + sineHarmonicCoefficient - * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); - - // Calculate derivative with respect to latitude. - potentialGradient( latitudeIndex ) = preMultiplier * radiusPowerTerm - * legendrePolynomialDerivative - * std::cos( sphericalPosition( latitudeIndex ) ) * ( cosineHarmonicCoefficient - * std::cos( static_cast< double >( order ) * sphericalPosition ( longitudeIndex ) ) - + sineHarmonicCoefficient * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); - - // Calculate derivative with respect to longitude. - potentialGradient( longitudeIndex ) = preMultiplier * radiusPowerTerm - * static_cast< double >( order ) * legendrePolynomial - * ( sineHarmonicCoefficient * std::cos( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) - - cosineHarmonicCoefficient * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); + return computePotentialGradient( + sphericalPosition( radiusIndex ), + basic_mathematics::raiseToIntegerPower + ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ), + std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::cos( sphericalPosition( latitudeIndex ) ), preMultiplier, degree, order, + cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative ); +} - // Return result. - return potentialGradient; +//! Compute the gradient of a single term of a spherical harmonics potential field. +Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache ) +{ + return computePotentialGradient( + sphericalPosition( radiusIndex ), + sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ), + sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getSineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ), + preMultiplier, degree, order, + cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative ); } } // namespace basic_mathematics diff --git a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h index 67ea431806..c6bba5e45c 100644 --- a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h +++ b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h @@ -37,14 +37,325 @@ #include +#include + +#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h" + namespace tudat { namespace basic_mathematics { +//! Cache object in which variables that are required for the computation of spherical harmonic potential are stored. +/*! + * Cache object in which variables that are required for the computation of spherical harmonic potential are stored. + * The variables are the Legendre polynomials at the required degree and order, the cosine of teh latitude, the + * sine and cosine of the order times the longitude, and the ratio of the distance and the reference radius to the + * power degree + 1. + */ +class SphericalHarmonicsCache +{ +public: + + + //! Default constructor, initializes cache object with 0 maximum degree and order. + /*! + * Default constructor, initializes cache object with 0 mazimum degree and order. + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + SphericalHarmonicsCache( const bool useGeodesyNormalization = 1 ) + { + legendreCache_ = boost::make_shared< LegendreCache >( useGeodesyNormalization ); + + currentLongitude_ = TUDAT_NAN; + referenceRadiusRatio_ = TUDAT_NAN; + + resetMaximumDegreeAndOrder( 0, 0 ); + } + + + //! Constructor + /*! + * Constructor + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + SphericalHarmonicsCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization = 1 ) + { + legendreCache_ = boost::make_shared< LegendreCache >( maximumDegree, maximumOrder, useGeodesyNormalization ); + + currentLongitude_ = TUDAT_NAN; + referenceRadiusRatio_ = TUDAT_NAN; + + resetMaximumDegreeAndOrder( maximumDegree, maximumOrder ); + } + + //! Update maximum degree and order of cache + /*! + * Update maximum degree and order of cache + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + */ + void resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ); + + + //! Update cached variables to current state. + /*! + * Update cached variables to current state. + * \param radius Distance from origin + * \param polynomialParameter Input parameter to Legendre polynomials (sine of latitude) + * \param longitude Current latitude + * \param referenceRadius Reference (typically equatorial) radius of gravity field. + */ + void update( const double radius, const double polynomialParameter, + const double longitude, const double referenceRadius ) + { + legendreCache_->update( polynomialParameter ); + updateSines( longitude ); + updateRadiusPowers( referenceRadius / radius ); + } + + //! Function to retrieve the current sine of m times the longitude. + /*! + * Function to retrieve the current sine of order times the longitude. + * \param order Order as input to sine( order * longitude ) + * \return Sine( order * longitude ) + */ + double getSineOfMultipleLongitude( const int order ) + { + return sinesOfLongitude_[ order ]; + } + + //! Function to retrieve the current cosine of m times the longitude. + /*! + * Function to retrieve the current cosine of order times the longitude. + * \param order Order as input to cosine( order * longitude ) + * \return Cosine( order * longitude ) + */ + double getCosineOfMultipleLongitude( const int order ) + { + return cosinesOfLongitude_[ order ]; + } + + //! Function to get an integer power of the distance divided by the reference radius. + /*! + * Function to get an integer power of the distance divided by the reference radius. + * \param degreePlusOne Power to which the ratio of distance and reference radius is to be computed (typically + * degree + 1). + * \return Ratio of distance and reference radius to power of input argument. + */ + double getReferenceRadiusRatioPowers( const int degreePlusOne ) + { + return referenceRadiusRatioPowers_[ degreePlusOne ]; + } + + //! Function to get the maximum degree of cache. + /*! + * Function to get the maximum degree of cache + * \return Maximum degree of cache. + */ + int getMaximumDegree( ) + { + return maximumDegree_; + } + + //! Function to get the maximum order of cache. + /*! + * Function to get the maximum order of cache. + * \return Maximum order of cache. + */ + int getMaximumOrder( ) + { + return maximumOrder_; + } + + //! Function to get current longitude + /*! + * Function to get current longitude + * \return Current longitude + */ + double getCurrentLongitude( ) + { + return currentLongitude_; + } + + //! Function to get object for caching and computing Legendre polynomials. + /*! + * Function to get object for caching and computing Legendre polynomials. + * \return Object for caching and computing Legendre polynomials. + */ + boost::shared_ptr< LegendreCache > getLegendreCache( ) + { + return legendreCache_; + } + +private: + + //! Update cached values of sines and cosines of longitude/ + /*! + * Update cached values of sines and cosines of longitude/ + * \param longitude Current longitude. + */ + void updateSines( const double longitude ) + { + //! Check if update is needed. + if( !( currentLongitude_ == longitude ) ) + { + currentLongitude_ = longitude; + for( unsigned int i = 0; i < sinesOfLongitude_.size( ); i++ ) + { + sinesOfLongitude_[ i ] = std::sin( static_cast< double >( i ) * longitude ); + cosinesOfLongitude_[ i ] = std::cos( static_cast< double >( i ) * longitude ); + } + } + } + + //! Update cached values of powers of distance over reference radius. + /*! + * Update cached values of powers of distance over reference radius. + * \param referenceRadiusRatio Distance divided by reference radius. + */ + void updateRadiusPowers( const double referenceRadiusRatio ) + { + //! Check if update is needed. + if( !( referenceRadiusRatio_ == referenceRadiusRatio ) ) + { + referenceRadiusRatio_ = referenceRadiusRatio; + double currentRatioPower = 1.0; + for( int i = 0; i <= maximumDegree_ + 1; i++ ) + { + referenceRadiusRatioPowers_[ i ] = currentRatioPower; + currentRatioPower *= referenceRadiusRatio_; + } + } + } + + //! Maximum degree of cache. + int maximumDegree_; + + //! Maximum order of cache. + int maximumOrder_; + + //! Current longitude. + double currentLongitude_; + + //! Current ratio of distance to reference radius + double referenceRadiusRatio_; + + //! List of sines of order times longitude. + /*! + * List of sines of order times longitude. Entry i denotes sin(i times longitude). + */ + std::vector< double > sinesOfLongitude_; + + //! List of cosines of order times longitude. + /*! + * List of cosines of order times longitude. Entry i denotes cos(i times longitude). + */ + std::vector< double > cosinesOfLongitude_; + + //! List of powers of distance divided by reference radius. + /*! + * List of powers of distance divided by reference radius. Entry i denoted (distance/reference radius) to the power i. + */ + std::vector< double > referenceRadiusRatioPowers_; + + //! Object for caching and computing Legendre polynomials. + boost::shared_ptr< LegendreCache > legendreCache_; + + + +}; + //! Spherical coordinate indices. enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; +//! Compute the gradient of a single term of a spherical harmonics potential field. +/*! + * This function returns a vector with the derivatives of a generic potential field (defined by + * spherical harmonics) from pre-computed quatities. + * \param distance Distance to center of body with gravity field at which the potential gradient is to be calculated + * \param radiusPowerTerm Distance divided by the reference radius of the gravity field, to the power (degree + 1) + * \param cosineOfOrderLongitude Cosine of order times the longitude at which the potential is to be calculated + * \param sineOfOrderLongitude Sine of order times the longitude at which the potential is to be calculated + * \param cosineOfLatitude Cosine of the latitude at which the potential is to be calculated + * \param preMultiplier Generic multiplication factor. + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order + * as the to be computed harmonic, and with the sine of the latitude coordinate as + * polynomial parameter. Make sure that the Legendre polynomial has the same + * normalization as the harmonic coefficients. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' + * with respect to the sine of the latitude angle. + * \return Vector with derivatives of potential field. + * The order is important! + * gradient( 0 ) = derivative with respect to radial distance, + * gradient( 1 ) = derivative with respect to latitude angle, + * gradient( 2 ) = derivative with respect to longitude angle. + */ +Eigen::Vector3d computePotentialGradient( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ); + + + +//! Compute the gradient of a single term of a spherical harmonics potential field. +/*! + * This function returns a vector with the derivatives of a generic potential field (defined by + * spherical harmonics). * + * \param sphericalPosition Vector with spherical coordinates. + * The order is important! + * sphericalPosition( 0 ) = radial coordinate, + * sphericalPosition( 1 ) = latitude coordinate, + * sphericalPosition( 2 ) = longitude coordinate. + * \param referenceRadius Radius of harmonics reference sphere. + * \param preMultiplier Generic multiplication factor. + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order + * as the to be computed harmonic, and with the sine of the latitude coordinate as + * polynomial parameter. Make sure that the Legendre polynomial has the same + * normalization as the harmonic coefficients. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' + * with respect to the sine of the latitude angle. + * \return Vector with derivatives of potential field. + * The order is important! + * gradient( 0 ) = derivative with respect to radial distance, + * gradient( 1 ) = derivative with respect to latitude angle, + * gradient( 2 ) = derivative with respect to longitude angle. + */ +Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ); + //! Compute the gradient of a single term of a spherical harmonics potential field. /*! * This function returns a vector with the derivatives of a generic potential field (defined by @@ -80,7 +391,6 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * sphericalPosition( 0 ) = radial coordinate, * sphericalPosition( 1 ) = latitude coordinate, * sphericalPosition( 2 ) = longitude coordinate. - * \param referenceRadius Radius of harmonics reference sphere. * \param preMultiplier Generic multiplication factor. * \param degree Degree of the harmonic for which the gradient is to be computed. * \param order Order of the harmonic for which the gradient is to be computed. @@ -94,6 +404,8 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * normalization as the harmonic coefficients. * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' * with respect to the sine of the latitude angle. + * \param sphericalHarmonicsCache Cache object containing current values of trigonometric funtions of latitude anf longitude, + * as well as legendre polynomials at current state. * \return Vector with derivatives of potential field. * The order is important! * gradient( 0 ) = derivative with respect to radial distance, @@ -101,14 +413,14 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * gradient( 2 ) = derivative with respect to longitude angle. */ Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, - const double referenceRadius, const double preMultiplier, const int degree, const int order, const double cosineHarmonicCoefficient, const double sineHarmonicCoefficient, const double legendrePolynomial, - const double legendrePolynomialDerivative ); + const double legendrePolynomialDerivative, + const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache ); } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/Interpolators/createInterpolator.h b/Tudat/Mathematics/Interpolators/createInterpolator.h index 1d286818f3..10906dc250 100644 --- a/Tudat/Mathematics/Interpolators/createInterpolator.h +++ b/Tudat/Mathematics/Interpolators/createInterpolator.h @@ -99,13 +99,16 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings * \param useLongDoubleTimeStep Boolean denoting whether time step is to be a long double, * time step is a double if false. * \param selectedLookupScheme Selected type of lookup scheme for independent variables. + * \param boundaryHandling Variable denoting the method by which the boundary interpolation is handled. */ LagrangeInterpolatorSettings( const int interpolatorOrder, const bool useLongDoubleTimeStep = 0, - const AvailableLookupScheme selectedLookupScheme = huntingAlgorithm ): + const AvailableLookupScheme selectedLookupScheme = huntingAlgorithm, + const LagrangeInterpolatorBoundaryHandling boundaryHandling = lagrange_cubic_spline_boundary_interpolation ): InterpolatorSettings( lagrange_interpolator, selectedLookupScheme ), - interpolatorOrder_( interpolatorOrder ), useLongDoubleTimeStep_( useLongDoubleTimeStep ) + interpolatorOrder_( interpolatorOrder ), useLongDoubleTimeStep_( useLongDoubleTimeStep ), + boundaryHandling_( boundaryHandling ) { } //! Destructor @@ -131,6 +134,12 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings return useLongDoubleTimeStep_; } + LagrangeInterpolatorBoundaryHandling getBoundaryHandling( ) + { + return boundaryHandling_; + } + + protected: //! Order of the Lagrange interpolator that is to be created. @@ -139,6 +148,8 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings //! Boolean denoting whether time step is to be a long double. bool useLongDoubleTimeStep_; + LagrangeInterpolatorBoundaryHandling boundaryHandling_; + }; //! Function to create an interpolator @@ -185,14 +196,16 @@ createOneDimensionalInterpolator( createdInterpolator = boost::make_shared< LagrangeInterpolator < IndependentVariableType, DependentVariableType, double > >( dataToInterpolate, lagrangeInterpolatorSettings->getInterpolatorOrder( ), - interpolatorSettings->getSelectedLookupScheme( ) ); + interpolatorSettings->getSelectedLookupScheme( ), + lagrangeInterpolatorSettings->getBoundaryHandling( ) ); } else { createdInterpolator = boost::make_shared< LagrangeInterpolator < IndependentVariableType, DependentVariableType, long double > >( dataToInterpolate, lagrangeInterpolatorSettings->getInterpolatorOrder( ), - interpolatorSettings->getSelectedLookupScheme( ) ); + interpolatorSettings->getSelectedLookupScheme( ), + lagrangeInterpolatorSettings->getBoundaryHandling( ) ); } } else diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 17ee3a7939..4d7a7fdecd 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -1,5 +1,5 @@ -#ifndef CREATENUMERICALINTEGRATOR_H -#define CREATENUMERICALINTEGRATOR_H +#ifndef TUDAT_CREATENUMERICALINTEGRATOR_H +#define TUDAT_CREATENUMERICALINTEGRATOR_H #include #include @@ -305,4 +305,4 @@ DependentVariableType > > createFixedStepSizeIntegrator( } -#endif // CREATENUMERICALINTEGRATOR_H +#endif // TUDAT_CREATENUMERICALINTEGRATOR_H diff --git a/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp b/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp index 1c68141afa..9ebb666aa3 100644 --- a/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp +++ b/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp @@ -46,6 +46,7 @@ namespace tudat { + namespace unit_tests { diff --git a/Tudat/SimulationSetup/body.h b/Tudat/SimulationSetup/body.h index 267e3d3596..4b6433609e 100644 --- a/Tudat/SimulationSetup/body.h +++ b/Tudat/SimulationSetup/body.h @@ -744,7 +744,7 @@ class Body radiationPressureIterator_; }; -typedef std::map< std::string, boost::shared_ptr< Body > > NamedBodyMap; +typedef std::unordered_map< std::string, boost::shared_ptr< Body > > NamedBodyMap; } // namespace simulation_setup diff --git a/Tudat/SimulationSetup/createAccelerationModels.cpp b/Tudat/SimulationSetup/createAccelerationModels.cpp index bba9abc643..b417cfbb66 100644 --- a/Tudat/SimulationSetup/createAccelerationModels.cpp +++ b/Tudat/SimulationSetup/createAccelerationModels.cpp @@ -94,7 +94,8 @@ boost::shared_ptr< CentralGravitationalAccelerationModel3d > createCentralGravit boost::make_shared< CentralGravitationalAccelerationModel3d >( boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), gravitationalParameterFunction, - boost::bind( &Body::getPosition, bodyExertingAcceleration ) ); + boost::bind( &Body::getPosition, bodyExertingAcceleration ), + useCentralBodyFixedFrame ); } @@ -202,7 +203,7 @@ createSphericalHarmonicsGravityAcceleration( sphericalHarmonicsSettings->maximumOrder_ ), boost::bind( &Body::getPosition, bodyExertingAcceleration ), boost::bind( &Body::getCurrentRotationToGlobalFrame, - bodyExertingAcceleration ) ); + bodyExertingAcceleration ), useCentralBodyFixedFrame ); } } return accelerationModel; diff --git a/Tudat/SimulationSetup/createEphemeris.cpp b/Tudat/SimulationSetup/createEphemeris.cpp index c842feb0a9..c687be8209 100644 --- a/Tudat/SimulationSetup/createEphemeris.cpp +++ b/Tudat/SimulationSetup/createEphemeris.cpp @@ -31,41 +31,6 @@ namespace simulation_setup using namespace ephemerides; -#if USE_CSPICE -//! Function to create a tabulated ephemeris using data from Spice. -boost::shared_ptr< Ephemeris > createTabulatedEphemerisFromSpice( - const std::string& body, - const double initialTime, - const double endTime, - const double timeStep, - const std::string& observerName, - const std::string& referenceFrameName ) -{ - using namespace interpolators; - - std::map< double, basic_mathematics::Vector6d > timeHistoryOfState; - - // Calculate state from spice at given time intervals and store in timeHistoryOfState. - double currentTime = initialTime; - while( currentTime < endTime ) - { - timeHistoryOfState[ currentTime ] = spice_interface::getBodyCartesianStateAtEpoch( - body, observerName, referenceFrameName, "none", currentTime ); - currentTime += timeStep; - } - - // Create interpolator. - boost::shared_ptr< LagrangeInterpolator< double, basic_mathematics::Vector6d > > interpolator = - boost::make_shared< LagrangeInterpolator< double, basic_mathematics::Vector6d > >( - timeHistoryOfState, 6, huntingAlgorithm, - lagrange_cubic_spline_boundary_interpolation ); - - // Create ephemeris and return. - return boost::make_shared< TabulatedCartesianEphemeris< > >( - interpolator, observerName, referenceFrameName ); -} -#endif - //! Function to create a ephemeris model. boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( const boost::shared_ptr< EphemerisSettings > ephemerisSettings, @@ -77,7 +42,7 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( // Check which type of ephemeris model is to be created. switch( ephemerisSettings->getEphemerisType( ) ) { - #if USE_CSPICE +#if USE_CSPICE case direct_spice_ephemeris: { // Check consistency of type and class. @@ -128,17 +93,32 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( } // Create corresponding ephemeris object. - ephemeris = createTabulatedEphemerisFromSpice( + if( !interpolatedEphemerisSettings->getUseLongDoubleStates( ) ) + { + ephemeris = createTabulatedEphemerisFromSpice< double, double >( inputName, interpolatedEphemerisSettings->getInitialTime( ), interpolatedEphemerisSettings->getFinalTime( ), interpolatedEphemerisSettings->getTimeStep( ), interpolatedEphemerisSettings->getFrameOrigin( ), - interpolatedEphemerisSettings->getFrameOrientation( ) ); + interpolatedEphemerisSettings->getFrameOrientation( ), + interpolatedEphemerisSettings->getInterpolatorSettings( ) ); + } + else + { + ephemeris = createTabulatedEphemerisFromSpice< long double, double >( + inputName, + static_cast< long double >( interpolatedEphemerisSettings->getInitialTime( ) ), + static_cast< long double >( interpolatedEphemerisSettings->getFinalTime( ) ), + static_cast< long double >( interpolatedEphemerisSettings->getTimeStep( ) ), + interpolatedEphemerisSettings->getFrameOrigin( ), + interpolatedEphemerisSettings->getFrameOrientation( ), + interpolatedEphemerisSettings->getInterpolatorSettings( ) ); + } } break; } - #endif +#endif case tabulated_ephemeris: { // Check consistency of type and class. @@ -152,14 +132,39 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( else { // Create corresponding ephemeris object. - ephemeris = boost::make_shared< TabulatedCartesianEphemeris< > >( - boost::make_shared< - interpolators::LagrangeInterpolator< double, basic_mathematics::Vector6d > > - ( tabulatedEphemerisSettings->getBodyStateHistory( ), 6, - interpolators::huntingAlgorithm, - interpolators::lagrange_cubic_spline_boundary_interpolation ), - tabulatedEphemerisSettings->getFrameOrigin( ), - tabulatedEphemerisSettings->getFrameOrientation( ) ); + if( !tabulatedEphemerisSettings->getUseLongDoubleStates( ) ) + { + ephemeris = boost::make_shared< TabulatedCartesianEphemeris< > >( + boost::make_shared< + interpolators::LagrangeInterpolator< double, basic_mathematics::Vector6d > > + ( tabulatedEphemerisSettings->getBodyStateHistory( ), 6, + interpolators::huntingAlgorithm, + interpolators::lagrange_cubic_spline_boundary_interpolation ), + tabulatedEphemerisSettings->getFrameOrigin( ), + tabulatedEphemerisSettings->getFrameOrientation( ) ); + } + else + { + // Cast input history to required type. + std::map< double, basic_mathematics::Vector6d > originalStateHistory = + tabulatedEphemerisSettings->getBodyStateHistory( ); + std::map< double, Eigen::Matrix< long double, 6, 1 > > longStateHistory; + + for( std::map< double, basic_mathematics::Vector6d >::const_iterator stateIterator = + originalStateHistory.begin( ); stateIterator != originalStateHistory.end( ); stateIterator++ ) + { + longStateHistory[ stateIterator->first ] = stateIterator->second.cast< long double >( ); + ephemeris = + boost::make_shared< TabulatedCartesianEphemeris< long double, double > >( + boost::make_shared< interpolators::LagrangeInterpolator< + double, Eigen::Matrix< long double, 6, 1 > > > + ( longStateHistory, 6, + interpolators::huntingAlgorithm, + interpolators::lagrange_cubic_spline_boundary_interpolation ), + tabulatedEphemerisSettings->getFrameOrigin( ), + tabulatedEphemerisSettings->getFrameOrientation( ) ); + } + } } break; } diff --git a/Tudat/SimulationSetup/createEphemeris.h b/Tudat/SimulationSetup/createEphemeris.h index 43d76a9c04..c48e15218e 100644 --- a/Tudat/SimulationSetup/createEphemeris.h +++ b/Tudat/SimulationSetup/createEphemeris.h @@ -18,7 +18,10 @@ #include "Tudat/InputOutput/matrixTextFileReader.h" #include "Tudat/Astrodynamics/Ephemerides/ephemeris.h" +#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" #include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsBase.h" +#include "Tudat/Mathematics/Interpolators/createInterpolator.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" namespace tudat { @@ -217,37 +220,62 @@ class InterpolatedSpiceEphemerisSettings: public DirectSpiceEphemerisSettings * (optional "SSB" by default). * \param frameOrientation Orientatioan of the reference frame in which the epehemeris is to be * calculated (optional "ECLIPJ2000" by default). + * \param interpolatorSettings Settings to be used for the state interpolation. */ InterpolatedSpiceEphemerisSettings( double initialTime, double finalTime, double timeStep, std::string frameOrigin = "SSB", - std::string frameOrientation = "ECLIPJ2000" ): + std::string frameOrientation = "ECLIPJ2000", + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings = + boost::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ) ): DirectSpiceEphemerisSettings( frameOrigin, frameOrientation, 0, 0, 0, interpolated_spice ), - initialTime_( initialTime ), finalTime_( finalTime ), timeStep_( timeStep ){ } + initialTime_( initialTime ), finalTime_( finalTime ), timeStep_( timeStep ), + interpolatorSettings_( interpolatorSettings ), useLongDoubleStates_( 0 ){ } - //! Function to returns initial time from which interpolated data from Spice should be created. + //! Function to return initial time from which interpolated data from Spice should be created. /*! - * Function to returns initial time from which interpolated data from Spice should be created. + * Function to return initial time from which interpolated data from Spice should be created. * \return Initial time from which interpolated data from Spice should be created. */ double getInitialTime( ){ return initialTime_; } - //! Function to returns final time from which interpolated data from Spice should be created. + //! Function to return final time from which interpolated data from Spice should be created. /*! - * Function to returns final time from which interpolated data from Spice should be created. + * Function to return final time from which interpolated data from Spice should be created. * \return Final time from which interpolated data from Spice should be created. */ double getFinalTime( ){ return finalTime_; } - //! Function to returns time step with which interpolated data from Spice should be created. + //! Function to return time step with which interpolated data from Spice should be created. /*! - * Function to returns time step with which interpolated data from Spice should be created. + * Function to return time step with which interpolated data from Spice should be created. * \return Time step with which interpolated data from Spice should be created. */ double getTimeStep( ){ return timeStep_; } + //! Function to return settings to be used for the state interpolation. + /*! + * Function to return settings to be used for the state interpolation. + * \return Settings to be used for the state interpolation. + */ + + boost::shared_ptr< interpolators::InterpolatorSettings > getInterpolatorSettings( ) + { + return interpolatorSettings_; + } + + bool getUseLongDoubleStates( ) + { + return useLongDoubleStates_; + } + + void setUseLongDoubleStates( const bool useLongDoubleStates ) + { + useLongDoubleStates_ = useLongDoubleStates; + } + private: //! Initial time from which interpolated data from Spice should be created. @@ -259,6 +287,10 @@ class InterpolatedSpiceEphemerisSettings: public DirectSpiceEphemerisSettings //! Time step with which interpolated data from Spice should be created. double timeStep_; + //! Settings to be used for the state interpolation. + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings_; + + bool useLongDoubleStates_; }; //! EphemerisSettings derived class for defining settings of an approximate ephemeris for major @@ -495,7 +527,7 @@ class TabulatedEphemerisSettings: public EphemerisSettings std::string frameOrigin = "SSB", std::string frameOrientation = "ECLIPJ2000" ): EphemerisSettings( tabulated_ephemeris, frameOrigin, frameOrientation ), - bodyStateHistory_( bodyStateHistory ){ } + bodyStateHistory_( bodyStateHistory ), useLongDoubleStates_( ){ } //! Function returning data map defining discrete data from which an ephemeris is to be created. /*! @@ -505,6 +537,17 @@ class TabulatedEphemerisSettings: public EphemerisSettings std::map< double, basic_mathematics::Vector6d > getBodyStateHistory( ) { return bodyStateHistory_; } + + bool getUseLongDoubleStates( ) + { + return useLongDoubleStates_; + } + + void setUseLongDoubleStates( const bool useLongDoubleStates ) + { + useLongDoubleStates_ = useLongDoubleStates; + } + private: //! Data map defining discrete data from which an ephemeris is to be created. @@ -513,8 +556,12 @@ class TabulatedEphemerisSettings: public EphemerisSettings * ephemeris is to be created. */ std::map< double, basic_mathematics::Vector6d > bodyStateHistory_; + + bool useLongDoubleStates_; }; +#if USE_CSPICE + //! Function to create a tabulated ephemeris using data from Spice. /*! * Function to create a tabulated ephemeris using data from Spice. @@ -532,13 +579,41 @@ class TabulatedEphemerisSettings: public EphemerisSettings * calculated. * \return Tabulated ephemeris using data from Spice. */ +template< typename StateScalarType = double, typename TimeType = double > boost::shared_ptr< ephemerides::Ephemeris > createTabulatedEphemerisFromSpice( const std::string& body, - const double initialTime, - const double endTime, - const double timeStep, + const TimeType initialTime, + const TimeType endTime, + const TimeType timeStep, const std::string& observerName, - const std::string& referenceFrameName ); + const std::string& referenceFrameName, + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings = + boost::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) ) +{ + using namespace interpolators; + + std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > timeHistoryOfState; + + // Calculate state from spice at given time intervals and store in timeHistoryOfState. + TimeType currentTime = initialTime; + while( currentTime < endTime ) + { + timeHistoryOfState[ currentTime ] = spice_interface::getBodyCartesianStateAtEpoch( + body, observerName, referenceFrameName, "none", static_cast< double >( currentTime ) ). + template cast< StateScalarType >( ); + currentTime += timeStep; + } + + // Create interpolator. + boost::shared_ptr< OneDimensionalInterpolator< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > > interpolator = + interpolators::createOneDimensionalInterpolator( + timeHistoryOfState, interpolatorSettings ); + + // Create ephemeris and return. + return boost::make_shared< ephemerides::TabulatedCartesianEphemeris< StateScalarType, TimeType > >( + interpolator, observerName, referenceFrameName ); +} +#endif //! Function to create a ephemeris model. /*! diff --git a/Tudat/SimulationSetup/createFlightConditions.h b/Tudat/SimulationSetup/createFlightConditions.h index f6c7cc499a..59b6b6947b 100644 --- a/Tudat/SimulationSetup/createFlightConditions.h +++ b/Tudat/SimulationSetup/createFlightConditions.h @@ -612,8 +612,8 @@ boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions( const boost::function< double( ) > bankAngleFunction = boost::lambda::constant ( 0.0 ) ); - } // namespace simulation_setup } // namespace tudat + #endif // TUDAT_CREATEACCELERATIONMODELS_H