Merge pull request #241 from DominicDirkx/aleixpinardell-AssessPropag…

…ationTerminationConditionDuringIntegrationSubsteps

Aleixpinardell assess propagation termination condition during integration substeps

Tudat/Astrodynamics/Propagators/CMakeLists.txt

@@ -95,6 +95,9 @@ add_executable(test_CR3BPPropagation "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unit
 setup_custom_test_program(test_CR3BPPropagation "${SRCROOT}${PROPAGATORSDIR}/")
 target_link_libraries(test_CR3BPPropagation ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES})
 
+add_executable(test_PropagationTerminationCheckOnFinalStep "${SRCROOT}${PROPAGATORSDIR}/UnitTests/uniTestPropagationTerminationCheckOnFinalStep.cpp")
+setup_custom_test_program(test_PropagationTerminationCheckOnFinalStep "${SRCROOT}${PROPAGATORSDIR}/")
+target_link_libraries(test_PropagationTerminationCheckOnFinalStep ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES})
 endif( )
 
 add_executable(test_StateDerivativeRestrictedThreeBodyProblem "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestStateDerivativeCircularRestrictedThreeBodyProblem.cpp")

Tudat/Astrodynamics/Propagators/integrateEquations.h

@@ -97,6 +97,19 @@ PropagationTerminationReason integrateEquationsFromIntegrator(
 
             // Perform integration step.
             newState = integrator->performIntegrationStep( timeStep );
+
+            // Check if the termination condition was reached during evaluation of integration sub-steps.
+            // If evaluation of the termination condition during integration sub-steps is disabled,
+            // this function returns always `false`.
+            // If the termination condition was reached, the last step could not be computed correctly because some
+            // of the integrator sub-steps were not computed. Thus, return immediately without saving the `newState`.
+            if ( integrator->getPropagationTerminationConditionReached() )
+            {
+                propagationTerminationReason = termination_condition_reached;
+                break;
+            }
+
+            // Update epoch and step-size
             currentTime = integrator->getCurrentIndependentVariable( );
             timeStep = integrator->getNextStepSize( );
 
@@ -228,6 +241,11 @@ class EquationIntegrationInterface< StateType, double >
                 numerical_integrators::createIntegrator< double, StateType >(
                     stateDerivativeFunction, initialState, integratorSettings );
 
+        if ( integratorSettings->assessPropagationTerminationConditionDuringIntegrationSubsteps_ )
+        {
+            integrator->setPropagationTerminationFunction( stopPropagationFunction );
+        }
+
         return integrateEquationsFromIntegrator< StateType, double >(
                     integrator, integratorSettings->initialTimeStep_, stopPropagationFunction, solutionHistory,
                     dependentVariableHistory,
@@ -274,6 +292,11 @@ class EquationIntegrationInterface< StateType, Time >
                 numerical_integrators::createIntegrator< Time, StateType, long double  >(
                     stateDerivativeFunction, initialState, integratorSettings );
 
+        if ( integratorSettings->assessPropagationTerminationConditionDuringIntegrationSubsteps_ )
+        {
+            integrator->setPropagationTerminationFunction( stopPropagationFunction );
+        }
+
         return integrateEquationsFromIntegrator< StateType, Time, long double >(
                     integrator, integratorSettings->initialTimeStep_, stopPropagationFunction, solutionHistory,
                     dependentVariableHistory,

Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h

@@ -58,12 +58,18 @@ class IntegratorSettings
      *  for variable step size integrators.
      *  \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration
      *  time steps, with n = saveFrequency).
+     *  \param assessPropagationTerminationConditionDuringIntegrationSubsteps Whether the propagation termination
+     *  conditions should be evaluated during the intermediate sub-steps of the integrator (`true`) or only at the end of
+     *  each integration step (`false`).
      */
     IntegratorSettings( const AvailableIntegrators integratorType, const TimeType initialTime,
                         const TimeType initialTimeStep,
-                        const int saveFrequency = 1 ): integratorType_( integratorType ),
+                        const int saveFrequency = 1,
+                        const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false )
+        : integratorType_( integratorType ),
         initialTime_( initialTime ), initialTimeStep_( initialTimeStep ),
-        saveFrequency_( saveFrequency ){ }
+        saveFrequency_( saveFrequency ), assessPropagationTerminationConditionDuringIntegrationSubsteps_(
+                                             assessPropagationTerminationConditionDuringIntegrationSubsteps ){ }
 
     //! Virtual destructor.
     /*!
@@ -96,6 +102,17 @@ class IntegratorSettings
      *  time steps, with n = saveFrequency).
      */
     int saveFrequency_;
+
+    //! Whether the propagation termination conditions should be evaluated during the intermediate sub-steps.
+    /*!
+     * Whether the propagation termination conditions should be evaluated during the intermediate updates
+     * performed by the integrator to compute the quantities necessary to integrate the state to a new epoch.
+     * The default value is false, so the propagation termination condition is only checked at the end of each
+     * integration step.
+     */
+    bool assessPropagationTerminationConditionDuringIntegrationSubsteps_;
+
+
 };
 
 //! Class to define settings of variable step RK numerical integrator
@@ -123,10 +140,12 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType >
      *  \param absoluteErrorTolerance Absolute error tolerance for step size control
      *  \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration
      *  time steps, with n = saveFrequency).
+     *  \param assessPropagationTerminationConditionDuringIntegrationSubsteps Whether the propagation termination
+     *  conditions should be evaluated during the intermediate sub-steps of the integrator (`true`) or only at the end of
+     *  each integration step (`false`).
      *  \param safetyFactorForNextStepSize Safety factor for step size control
      *  \param maximumFactorIncreaseForNextStepSize Maximum increase factor in time step in subsequent iterations.
      *  \param minimumFactorDecreaseForNextStepSize Maximum decrease factor in time step in subsequent iterations.
-
      */
     RungeKuttaVariableStepSizeSettings(
             const AvailableIntegrators integratorType,
@@ -137,10 +156,12 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType >
             const TimeType relativeErrorTolerance = 1.0E-12,
             const TimeType absoluteErrorTolerance = 1.0E-12,
             const int saveFrequency = 1,
+            const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false,
             const TimeType safetyFactorForNextStepSize = 0.8,
             const TimeType maximumFactorIncreaseForNextStepSize = 4.0,
             const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ):
-        IntegratorSettings< TimeType >( integratorType, initialTime, initialTimeStep, saveFrequency ),
+        IntegratorSettings< TimeType >( integratorType, initialTime, initialTimeStep, saveFrequency,
+                                        assessPropagationTerminationConditionDuringIntegrationSubsteps ),
         coefficientSet_( coefficientSet ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ),
         relativeErrorTolerance_( relativeErrorTolerance ), absoluteErrorTolerance_( absoluteErrorTolerance ),
         safetyFactorForNextStepSize_( safetyFactorForNextStepSize ),

Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h

@@ -16,6 +16,7 @@
 #include <limits>
 
 #include <boost/function.hpp>
+#include <boost/lambda/lambda.hpp>
 
 #include <Eigen/Core>
 
@@ -137,6 +138,28 @@ class NumericalIntegrator
         return stateDerivativeFunction_;
     }
 
+    //! Function to return the termination condition was reached during the current step
+    /*!
+     *  Function to return the termination condition was reached during the current step
+     * \return  True if the termination condition was reached during the current step
+     */
+    bool getPropagationTerminationConditionReached( )
+    {
+        return propagationTerminationConditionReachedDuringStep_;
+    }
+
+    //! Setter for the (optional) propagation termination function
+    /*!
+     *  Setter for the (optional) propagation termination function, to be evaluated during the intermediate state updates
+     *  performed to compute the quantities necessary to integrate the state to a new epoch (e.g. k1-k4 for RK$).
+     *  \param terminationFunction Function that returns true if termination condition is reached, false if it has not,
+     *  as a function of current time.
+     */
+    void setPropagationTerminationFunction( boost::function< bool( const double ) > terminationFunction )
+    {
+        propagationTerminationFunction_ = terminationFunction;
+    }
+
 protected:
 
     //! Function that returns the state derivative.
@@ -145,6 +168,21 @@ class NumericalIntegrator
      */
     StateDerivativeFunction stateDerivativeFunction_;
 
+    //! Boolean to denote whether the propagation termination condition was reached during the evaluation of one of the sub-steps
+    /*! Boolean to denote whether the propagation termination condition was reached during the evaluation of one of the sub-steps
+     *  necessary to perform the last integration step. Parameter is false by default, and when set to true must be accompanied by
+     *  propagationTerminationFunction_ (which is non-active by default)
+     */
+    bool propagationTerminationConditionReachedDuringStep_ = false;
+
+    //! Propagation termination function
+    /*!
+     *  Propagation termination function to be evaluated during the intermediate state updates performed to compute
+     *  the quantities necessary to integrate the state to a new epoch.
+     *  By default, this function evaluates always to false, so the propagation termination conditions will not be
+     *  checked during the integration subteps.
+     */
+    boost::function< bool( const double ) > propagationTerminationFunction_ = boost::lambda::constant( false );
 };
 
 //! Perform an integration to a specified independent variable value.

Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h

@@ -109,20 +109,43 @@ class RungeKutta4Integrator
         lastState_ = currentState_;
 
         // Calculate k1-k4.
-        const StateDerivativeType k1 = stepSize * this->stateDerivativeFunction_(
-                    currentIndependentVariable_, currentState_ );
-
-        const StateDerivativeType k2 = stepSize * this->stateDerivativeFunction_(
-                    currentIndependentVariable_ + stepSize / 2.0,
-                    static_cast< StateType >( currentState_ + k1 / 2.0 ) );
-
-        const StateDerivativeType k3 = stepSize * this->stateDerivativeFunction_(
-                    currentIndependentVariable_ + stepSize / 2.0,
-                    static_cast< StateType >( currentState_ + k2 / 2.0 ) );
-
-        const StateDerivativeType k4 = stepSize * this->stateDerivativeFunction_(
-                    currentIndependentVariable_ + stepSize,
-                    static_cast< StateType >( currentState_ + k3 ) );
+        StateDerivativeType k1, k2, k3, k4;
+        for ( unsigned int i = 1; i <= 4; i++ )
+        {
+            IndependentVariableType time;
+            StateType state;
+            switch ( i ) {
+            case 1:
+                time = currentIndependentVariable_;
+                state = currentState_;
+                k1 = stepSize * this->stateDerivativeFunction_( time, state );
+                break;
+            case 2:
+                time = currentIndependentVariable_ + stepSize / 2.0;
+                state = static_cast< StateType >( currentState_ + k1 / 2.0 );
+                k2 = stepSize * this->stateDerivativeFunction_( time, state );
+                break;
+            case 3:
+                time = currentIndependentVariable_ + stepSize / 2.0;
+                state = static_cast< StateType >( currentState_ + k2 / 2.0 );
+                k3 = stepSize * this->stateDerivativeFunction_( time, state );
+                break;
+            case 4:
+                time = currentIndependentVariable_ + stepSize;
+                state = static_cast< StateType >( currentState_ + k3 );
+                k4 = stepSize * this->stateDerivativeFunction_( time, state );
+                break;
+            }
+
+            // Check if propagation should terminate because the propagation termination condition has been reached
+            // while computing k1, k2, k3 or k4. If so, return immediately the current state (not recomputed yet),
+            // which will be discarded.
+            if ( this->propagationTerminationFunction_( static_cast< double >( time ) ) )
+            {
+                this->propagationTerminationConditionReachedDuringStep_ = true;
+                return currentState_;
+            }
+        }
 
         stepSize_ = stepSize;
         currentIndependentVariable_ += stepSize_;

Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h

@@ -455,11 +455,18 @@ ::performIntegrationStep( const TimeStepType stepSize )
         }
 
         // Compute the state derivative.
-        currentStateDerivatives_.push_back(
-                    this->stateDerivativeFunction_(
-                        this->currentIndependentVariable_ +
-                        this->coefficients_.cCoefficients( stage ) * stepSize,
-                        intermediateState ) );
+        const IndependentVariableType time = this->currentIndependentVariable_ +
+                this->coefficients_.cCoefficients( stage ) * stepSize;
+        currentStateDerivatives_.push_back( this->stateDerivativeFunction_( time, intermediateState ) );
+
+        // Check if propagation should terminate because the propagation termination condition has been reached
+        // while computing the intermediate state.
+        // If so, return immediately the current state (not recomputed yet), which will be discarded.
+        if ( this->propagationTerminationFunction_( static_cast< double >( time ) ) )
+        {
+            this->propagationTerminationConditionReachedDuringStep_ = true;
+            return this->currentState_;
+        }
 
         // Update the estimate.
         lowerOrderEstimate += this->coefficients_.bCoefficients( 0, stage ) * stepSize *