Feature/improved interpolator default

include/tudat/basics/identityElements.h

@@ -43,6 +43,11 @@ class IdentityElement
                     ( VariableType::ColsAtCompileTime > 0 ) ? VariableType::ColsAtCompileTime : 0 );
     }
 
+    template< typename VariableType, typename std::enable_if< is_eigen_matrix< VariableType >::value, int >::type = 0 >
+    static VariableType getAdditionIdentity( const VariableType& variable )
+    {
+        return VariableType::Zero( variable.rows( ), variable.cols( ) );
+    }
     //! Function to output the zero value (i.e., the addition identity) for integer and floating point types.
     /*!
      *  Function to output the zero value (i.e., the addition identity) for integer and floating point types.
@@ -55,6 +60,13 @@ class IdentityElement
         return tudat::mathematical_constants::getFloatingInteger< VariableType >( 0 );
     }
 
+    template< typename VariableType, typename std::enable_if< ( std::is_integral< VariableType >::value ||
+                                                                std::is_floating_point< VariableType >::value ), int >::type = 0 >
+    static VariableType getAdditionIdentity( const VariableType& variable )
+    {
+        return tudat::mathematical_constants::getFloatingInteger< VariableType >( 0 );
+    }
+
     //! Function to output the unit value (i.e., the multiplication identity) for Eigen types.
     /*!
      *  Function to output the unit value (i.e., the multiplication identity) for Eigen types.
@@ -75,6 +87,19 @@ class IdentityElement
         }
     }
 
+    template< typename VariableType, typename std::enable_if< is_eigen_matrix< VariableType >::value, int >::type = 0 >
+    static VariableType getMultiplicationIdentity( const VariableType& variable )
+    {
+        if( VariableType::RowsAtCompileTime == VariableType::ColsAtCompileTime )
+        {
+            return VariableType::Identity( variable.rows( ), variable.cols( ) );
+        }
+        else
+        {
+            throw std::runtime_error( "Error, multiplication identity not defined for non-square matrix" );
+        }
+    }
+
     //! Function to output the unit value (i.e., the multiplication identity) for integer and floating point types.
     /*!
      *  Function to output the unit value (i.e., the multiplication identity) for integer and floating point types.
@@ -87,25 +112,44 @@ class IdentityElement
         return tudat::mathematical_constants::getFloatingInteger< VariableType >( 1 );
     }
 
+    template< typename VariableType, typename std::enable_if< ( std::is_integral< VariableType >::value ||
+                                                                std::is_floating_point< VariableType >::value ), int >::type = 0 >
+    static VariableType getMultiplicationIdentity( const VariableType& variable )
+    {
+        return tudat::mathematical_constants::getFloatingInteger< VariableType >( 1 );
+    }
+
     //! Function to output the NaN value (i.e., the null identity) for Eigen types.
     /*!
      *  Function to output the NaN value (i.e., the null identity) for Eigen types.
      *  \return Null identity of Eigen types.
      */
     template< typename VariableType, typename std::enable_if< is_eigen_matrix< VariableType >::value, int >::type = 0 >
-    static VariableType getNullIdentity( )
+    static VariableType getNanIdentity( )
     {
         return VariableType::Constant( ( VariableType::RowsAtCompileTime > 0 ) ? VariableType::RowsAtCompileTime : 0,
                                        ( VariableType::ColsAtCompileTime > 0 ) ? VariableType::ColsAtCompileTime : 0, TUDAT_NAN );
     }
 
+    template< typename VariableType, typename std::enable_if< is_eigen_matrix< VariableType >::value, int >::type = 0 >
+    static VariableType getNanIdentity( const VariableType& variable )
+    {
+        return VariableType::Constant( variable.rows( ), variable.cols( ), TUDAT_NAN );
+    }
+
     //! Function to output the NaN value (i.e., the null identity) for floating point types.
     /*!
      *  Function to output the NaN value (i.e., the null identity) for floating point types.
      *  \return Null identity of floating point types.
      */
     template< typename VariableType, typename std::enable_if< std::is_floating_point< VariableType >::value, int >::type = 0 >
-    static VariableType getNullIdentity( )
+    static VariableType getNanIdentity( )
+    {
+        return TUDAT_NAN;
+    }
+
+    template< typename VariableType, typename std::enable_if< std::is_floating_point< VariableType >::value, int >::type = 0 >
+    static VariableType getNanIdentity( const VariableType& variable  )
     {
         return TUDAT_NAN;
     }

include/tudat/math/interpolators/interpolator.h

@@ -37,7 +37,9 @@ enum BoundaryInterpolationType
     extrapolate_at_boundary = 3,
     extrapolate_at_boundary_with_warning = 4,
     use_default_value = 5,
-    use_default_value_with_warning = 6
+    use_default_value_with_warning = 6,
+    use_nan_value = 7,
+    use_nan_value_with_warning = 8
 };
 
 //! Base class for interpolator.

include/tudat/math/interpolators/lagrangeInterpolator.h

@@ -36,7 +36,10 @@ namespace interpolators
 enum LagrangeInterpolatorBoundaryHandling
 {
     lagrange_cubic_spline_boundary_interpolation = 0,
-    lagrange_no_boundary_interpolation = 1
+    lagrange_cubic_spline_boundary_interpolation_with_warning = 1,
+    lagrange_boundary_nan_interpolation = 2,
+    lagrange_boundary_nan_interpolation_with_warning = 3,
+    lagrange_no_boundary_interpolation = 4
 };
 
 //! Class to perform Lagrange polynomial interpolation
@@ -260,29 +263,11 @@ class LagrangeInterpolator : public OneDimensionalInterpolator< IndependentVaria
         // can be used.
         if( lowerEntry < offsetEntries_ )
         {
-            if( lagrangeBoundaryHandling_ == lagrange_no_boundary_interpolation )
-            {
-                throw std::runtime_error(
-                            "Error: Lagrange interpolator below allowed bounds." );
-            }
-            else if( numberOfStages_ > 2 )
-            {
-                interpolatedValue = beginInterpolator_->interpolate(
-                            targetIndependentVariableValue );
-            }
+            interpolatedValue = performLagrangeBoundaryInterpolation( beginInterpolator_, targetIndependentVariableValue );
         }
         else if( lowerEntry >= numberOfIndependentValues_ - offsetEntries_ - 1 )
         {
-            if( lagrangeBoundaryHandling_ == lagrange_no_boundary_interpolation )
-            {
-                throw std::runtime_error(
-                            "Error: Lagrange interpolator above allowed bounds." );
-            }
-            else if( numberOfStages_ > 2 )
-            {
-                interpolatedValue = endInterpolator_->interpolate(
-                            targetIndependentVariableValue );
-            }
+            interpolatedValue = performLagrangeBoundaryInterpolation( endInterpolator_, targetIndependentVariableValue );
         }
         else
         {
@@ -356,6 +341,39 @@ class LagrangeInterpolator : public OneDimensionalInterpolator< IndependentVaria
 
 private:
 
+    DependentVariableType performLagrangeBoundaryInterpolation(
+        const std::shared_ptr< OneDimensionalInterpolator< IndependentVariableType, DependentVariableType > > boundaryInterpolator,
+        const IndependentVariableType& targetIndependentVariableValue )
+    {
+        std::cout<<"Lagrange handling "<<lagrangeBoundaryHandling_<<std::endl;
+        DependentVariableType interpolatedValue;
+        switch( lagrangeBoundaryHandling_ )
+        {
+        case lagrange_cubic_spline_boundary_interpolation_with_warning:
+            std::cerr<<"Warning, calling Lagrange interpolator near boundary (at "<<targetIndependentVariableValue<<" ), using cubic-spline interpolation"<<std::endl;
+            interpolatedValue = boundaryInterpolator->interpolate( targetIndependentVariableValue );
+            break;
+        case lagrange_cubic_spline_boundary_interpolation:
+            interpolatedValue = boundaryInterpolator->interpolate( targetIndependentVariableValue );
+            break;
+        case lagrange_boundary_nan_interpolation_with_warning:
+            std::cerr<<"Warning, calling Lagrange interpolator near boundary (at "<<targetIndependentVariableValue<<" ), returning NaN"<<std::endl;
+            interpolatedValue = IdentityElement::getNanIdentity< DependentVariableType >( zeroEntry_ );
+            break;
+        case lagrange_boundary_nan_interpolation:
+            interpolatedValue = IdentityElement::getNanIdentity< DependentVariableType >( zeroEntry_ );
+            break;
+        case lagrange_no_boundary_interpolation:
+            throw std::runtime_error( "Error: Lagrange interpolator called outside permitted bounds, at " + std::to_string(
+                static_cast< double >( targetIndependentVariableValue ) ) );
+            break;
+        default:
+            throw std::runtime_error( "Error when handling Lagrange boundary case, option not implemented" );
+
+        }
+        return interpolatedValue;
+    }
+
     //! Function called at initialization which pre-computes the denominators of the
     //! interpolants at each interval.
     /*!
@@ -423,7 +441,8 @@ class LagrangeInterpolator : public OneDimensionalInterpolator< IndependentVaria
             const AvailableLookupScheme selectedLookupScheme = huntingAlgorithm )
     {
         // Create interpolators
-        if( lagrangeBoundaryHandling_ == lagrange_cubic_spline_boundary_interpolation )
+        if( lagrangeBoundaryHandling_ == lagrange_cubic_spline_boundary_interpolation  ||
+            lagrangeBoundaryHandling_ == lagrange_cubic_spline_boundary_interpolation_with_warning)
         {
             // Ensure sufficient data points for spline.
             int cubicSplineInputSize = offsetEntries_;

include/tudat/math/interpolators/oneDimensionalInterpolator.h

@@ -326,6 +326,35 @@ class OneDimensionalInterpolator : public Interpolator< IndependentVariableType,
                     }
                     break;
                 }
+                case use_nan_value:
+                case use_nan_value_with_warning:
+                {
+                    // Warn user, if requested
+                    if ( boundaryHandling_ == use_nan_value_with_warning )
+                    {
+                        std::string errorMessage = "Warning in interpolator, requesting data point outside of boundaries, requested data at " +
+                                                   boost::lexical_cast< std::string >( targetIndependentVariable ) + " but limit values are " +
+                                                   boost::lexical_cast< std::string >( independentValues_.front( ) ) + " and " +
+                                                   boost::lexical_cast< std::string >( independentValues_.back( ) ) + ", taking NaN value instead.";
+                        std::cerr << errorMessage << std::endl;
+                    }
+
+                    // Get default value
+                    useValue = true;
+                    if ( isAtBoundary == -1 )
+                    {
+                        dependentVariable = IdentityElement::getNanIdentity< DependentVariableType >( dependentVariable );
+                    }
+                    else if ( isAtBoundary == 1 )
+                    {
+                        dependentVariable = IdentityElement::getNanIdentity< DependentVariableType >( dependentVariable );
+                    }
+                    else
+                    {
+                        throw std::runtime_error( "Error when checking interpolation boundary, inconsistent data encountered" );
+                    }
+                    break;
+                }
                 default:
                     throw std::runtime_error( "Error when checking interpolation boundary, boundary handling method not recognized." );
                 }

tests/src/math/interpolators/unitTestLagrangeInterpolators.cpp

@@ -67,11 +67,12 @@ std::map< int, double > getPolynomialCoefficients( const int polynomialOrder)
 
     // Copy subset of allCoefficients map into currentCoefficients.
     std::map< int, double > currentCoefficients( allCoefficients.begin(),
-        boost::next( allCoefficients.begin(), polynomialOrder + 1 ) );
+        std::next( allCoefficients.begin(), polynomialOrder + 1 ) );
     return currentCoefficients;
 
 }
 
+
 //! Create quasi-random vector of non-uniform independent variables
 /*!
  *  Create quasi-random vector of non-uniform independent variables
@@ -189,6 +190,7 @@ BOOST_AUTO_TEST_CASE( test_lagrange_interpolation_boundary )
 
     unsigned int independentVariableVectorSize = independentVariableVector.size( );
 
+    for( int testCase = 0; testCase < 4; testCase++ )
     {
         // Test interpolator for 4;6;8;10 data points per interpolant
         // (i.e. 3rd, 5th, 7th and 9th order polynomial)
@@ -210,8 +212,7 @@ BOOST_AUTO_TEST_CASE( test_lagrange_interpolation_boundary )
             interpolators::LagrangeInterpolator< double, double > lagrangeInterpolator =
                     interpolators::LagrangeInterpolator< double, double >(
                         independentVariableVector, dataVector, stages,
-                        interpolators::huntingAlgorithm,
-                        interpolators::lagrange_cubic_spline_boundary_interpolation );
+                        interpolators::huntingAlgorithm, static_cast< interpolators::LagrangeInterpolatorBoundaryHandling >( testCase ) );
 
             // Create spline interpolator from edge points at lower bound.
             int dataPointsForSpline = ( stages / 2 > 4 ) ? ( stages / 2 ) : 4;
@@ -234,10 +235,18 @@ BOOST_AUTO_TEST_CASE( test_lagrange_interpolation_boundary )
                 {
                     double currentTestIndependentVariable = independentVariableVector.at( i ) +
                             static_cast< double >( i ) * currentStepSize;
-                    BOOST_CHECK_EQUAL(
-                                lagrangeInterpolator.interpolate( currentTestIndependentVariable ),
-                                lowerBoundInterpolator.interpolate(
-                                    currentTestIndependentVariable ) );
+                    double interpolatedValue = lagrangeInterpolator.interpolate( currentTestIndependentVariable );
+                    if( testCase < 2 )
+                    {
+                        BOOST_CHECK_EQUAL(
+                            interpolatedValue,
+                            lowerBoundInterpolator.interpolate(
+                                currentTestIndependentVariable ));
+                    }
+                    else
+                    {
+                        BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+                    }
                 }
             }
 
@@ -265,10 +274,18 @@ BOOST_AUTO_TEST_CASE( test_lagrange_interpolation_boundary )
                     double currentTestIndependentVariable = independentVariableVector.at(
                                 independentVariableVectorSize - i - 2 ) +
                             static_cast< double >( i ) * currentStepSize;
-                    BOOST_CHECK_EQUAL( lagrangeInterpolator.interpolate(
-                                           currentTestIndependentVariable ),
-                                       upperBoundInterpolator.interpolate(
-                                           currentTestIndependentVariable ) );
+                    double interpolatedValue = lagrangeInterpolator.interpolate( currentTestIndependentVariable );
+
+                    if( testCase < 2 )
+                    {
+                        BOOST_CHECK_EQUAL( interpolatedValue,
+                                           upperBoundInterpolator.interpolate(
+                                               currentTestIndependentVariable ));
+                    }
+                    else
+                    {
+                        BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+                    }
                 }
             }
         }

tests/src/math/interpolators/unitTestLinearInterpolator.cpp

@@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case )
     double interpolatedValue = TUDAT_NAN, expectedValue = TUDAT_NAN;
     bool exceptionIsCaught = false;
 
-    for( unsigned int i = 0; i < 7; i++ )
+    for( unsigned int i = 0; i < 9; i++ )
     {
         LinearInterpolatorDouble linearInterpolator(
                     independentVariableValues, dependentVariableValues, huntingAlgorithm,
@@ -244,6 +244,15 @@ BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case )
             interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
             BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0, 1.0E-15 );
         }
+        else if( ( static_cast< BoundaryInterpolationType >( i ) == use_nan_value ) ||
+                 ( static_cast< BoundaryInterpolationType >( i ) == use_nan_value_with_warning ) )
+        {
+            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
+            BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+
+            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
+            BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+        }
     }
 }
 
@@ -318,6 +327,25 @@ BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case_extrapolation_defau
             interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
             BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Vector3d::Zero( ) ).norm( ), 1.0E-15 );
         }
+
+        for( unsigned int i = 7; i < 9; i++ )
+        {
+            LinearInterpolator< double, Eigen::Vector3d > linearInterpolator(
+                independentVariableValues, dependentVariableValues, huntingAlgorithm,
+                static_cast< BoundaryInterpolationType >( i ) );
+
+            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
+            for( unsigned int j = 0; j < 3; j++ )
+            {
+                BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+            }
+
+            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
+            for( unsigned int j = 0; j < 3; j++ )
+            {
+                BOOST_CHECK_EQUAL( !( interpolatedValue == interpolatedValue ), true );
+            }
+        }
     }
 
     // Test with Eigen::Matrix3d