interpolation.cpp 6.79 KB
Newer Older
1
#include "toolboxes/interpolation.h"
jannick.wolters's avatar
jannick.wolters committed
2
3
4
#include "toolboxes/errormessages.h"
#include <blaze/math/lapack/posv.h>

5
// Change Vector type to blaze for std input
6
7
8
9
Interpolation::Interpolation( const std::vector<double>& x, const std::vector<double>& y, TYPE type ) : _dim( 1u ), _type( type ) {
    _x = Vector( x.size(), x.data() );
    _y = Vector( y.size(), y.data() );
    Setup();
jannick.wolters's avatar
jannick.wolters committed
10
11
}

12
// 1D Constructor: Initialise values and call Set-up
13
Interpolation::Interpolation( const Vector& x, const Vector& y, TYPE type ) : _dim( 1u ), _x( x ), _y( y ), _type( type ) { Setup(); }
jannick.wolters's avatar
jannick.wolters committed
14

15
// 2D Constructor: Initialise values and call Set-up
16
17
18
Interpolation::Interpolation( const Vector& x, const Vector& y, const Matrix& data, TYPE type )
    : _dim( 2u ), _x( x ), _y( y ), _data( data ), _type( type ) {
    Setup();
jannick.wolters's avatar
jannick.wolters committed
19
20
}

21
// Set-up: check validity of input
22
void Interpolation::Setup() {
jannick.wolters's avatar
jannick.wolters committed
23
    // 1D
24
    if( _dim == 1u ) {
25
        // Length of tables must be equal
26
27
28
        if( _x.size() != _y.size() ) ErrorMessages::Error( "Vectors are of unequal length!", CURRENT_FUNCTION );
        if( _type == loglinear ) _y = blaze::log( _y );

29
        // Values must be sorted ascendingly (property is used later when searching tables)
30
        for( unsigned i = 0; i < _x.size() - 1u; i++ ) {
jannick.wolters's avatar
jannick.wolters committed
31
            if( !( _x[i] < _x[i + 1] ) ) ErrorMessages::Error( "x is not sorted ascendingly!", CURRENT_FUNCTION );
32
33
        }
    }
34
35

    // 2D
36
    else if( _dim == 2u ) {
37
        // Length of tables must be equal
38
39
        if( _x.size() != _data.rows() ) ErrorMessages::Error( "x and data are of unequal length!", CURRENT_FUNCTION );
        if( _y.size() != _data.columns() ) ErrorMessages::Error( "y and data are of unequal length!", CURRENT_FUNCTION );
jannick.wolters's avatar
jannick.wolters committed
40

41
        // Values must be sorted ascendingly (property is used later when searching tables)
42
43
44
45
46
47
        for( unsigned i = 0; i < _x.size() - 1u; i++ ) {
            if( !( _x[i] < _x[i + 1] ) ) ErrorMessages::Error( "x is not sorted ascendingly!", CURRENT_FUNCTION );
        }
        for( unsigned i = 0; i < _y.size() - 1u; i++ ) {
            if( !( _y[i] < _y[i + 1] ) ) ErrorMessages::Error( "y is not sorted ascendingly!", CURRENT_FUNCTION );
        }
jannick.wolters's avatar
jannick.wolters committed
48
49
50
    }
}

51
// 1D interpolation
52
double Interpolation::operator()( double x ) const {
jannick.wolters's avatar
jannick.wolters committed
53
    // Check whether 1D
54
    if( _dim != 1u ) ErrorMessages::Error( "Invalid data dimension for operator(x)!", CURRENT_FUNCTION );
55
    // x must be between min and max of table values
jannick.wolters's avatar
jannick.wolters committed
56
    if( ( x < _x[0] || x > _x[_x.size() - 1u] ) && _type != linear ) {
57
58
59
60
        // std::cout << x << "\t" << _x[0] << std::endl;
        // std::cout << x << "\t" << _x[_x.size() - 1u] << std::endl;
        ErrorMessages::Error( "Extrapolation is not supported!", CURRENT_FUNCTION );
    }
jannick.wolters's avatar
jannick.wolters committed
61

62
    // points to first value in _x that is not smaller than x (upper bound)
63
    Vector::ConstIterator it = std::lower_bound( _x.begin(), _x.end(), x );
jannick.wolters's avatar
jannick.wolters committed
64

65
    // index of the lower bound to x in _x
jannick.wolters's avatar
jannick.wolters committed
66
67
68
69
70
71
72
73
74
75
    unsigned idx = 0;
    if( x >= _x[0] && x <= _x[_x.size() - 1u] ) {
        idx = static_cast<unsigned>( std::max( int( it - _x.begin() ) - 1, 0 ) );
    }
    else if( x < _x[0] ) {
        idx = 0;
    }
    else if( x > _x[_x.size() - 1u] ) {
        idx = _x.size() - 1u;
    }
jannick.wolters's avatar
jannick.wolters committed
76

77
    // linear interpolation
78
    if( _type == linear || _type == loglinear ) {
79
        // interpolate between lower bound and upper bound of x in _x
80
81
82
83
84
85
        double res = _y[idx] + ( _y[idx + 1] - _y[idx] ) / ( _x[idx + 1] - _x[idx] ) * ( x - _x[idx] );

        if( _type == loglinear )
            return std::exp( res );
        else
            return res;
jannick.wolters's avatar
jannick.wolters committed
86
    }
87
88

    // cubic interpolation
89
90
91
92
    else if( _type == cubic ) {
        double param[4] = { _y[idx - 1], _y[idx], _y[idx + 1], _y[idx + 2] };
        double t        = ( x - _x[idx] ) / ( _x[idx + 1] - _x[idx] );
        return EvalCubic1DSpline( param, t );
jannick.wolters's avatar
jannick.wolters committed
93
    }
jannick.wolters's avatar
jannick.wolters committed
94
    else {
95
96
        ErrorMessages::Error( "Invalid type!", CURRENT_FUNCTION );
        return -1.0;
jannick.wolters's avatar
jannick.wolters committed
97
    }
98
}
jannick.wolters's avatar
jannick.wolters committed
99

100
// 2D interpolation
101
double Interpolation::operator()( double x, double y ) const {
102
    // Check whether 2D
103
104
    if( _dim != 2u ) ErrorMessages::Error( "Invalid data dimension for operator(x,y)!", CURRENT_FUNCTION );
    if( _type == cubic ) {
jannick.wolters's avatar
jannick.wolters committed
105

106
        // find closest values to x and y in table (lower bounds)
107
108
        int xId = IndexOfClosestValue( x, _x );
        int yId = IndexOfClosestValue( y, _y );
jannick.wolters's avatar
jannick.wolters committed
109

jannick.wolters's avatar
jannick.wolters committed
110
        // store all 16 interpolation points needed
111
112
113
114
        double points[4][4];
        for( int i = -1; i < 3; ++i ) {
            unsigned idx_y;
            idx_y = yId + i < 0 ? 0 : yId + i;
115
            idx_y = yId + i > static_cast<int>( _data.rows() - 1 ) ? _data.rows() - 1 : yId + i;
116
117
118
            for( int j = -1; j < 3; ++j ) {
                unsigned idx_x;
                idx_x = xId + j < 0 ? 0 : xId + j;
119
                idx_x = xId + j > static_cast<int>( _data.columns() - 1 ) ? _data.columns() - 1 : xId + j;
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

                points[i + 1][j + 1] = _data( idx_x, idx_y );
            }
        }

        // rescale data to [0,1]
        double t = ( x - _x[xId] ) / ( _x[xId + 1] - _x[xId] );
        double u = ( y - _y[yId] ) / ( _y[yId + 1] - _y[yId] );

        // first interpolate in x-direction and store the results on which the final interpolation in y will be done
        double buffer[4];
        buffer[0] = EvalCubic1DSpline( points[0], t );
        buffer[1] = EvalCubic1DSpline( points[1], t );
        buffer[2] = EvalCubic1DSpline( points[2], t );
        buffer[3] = EvalCubic1DSpline( points[3], t );
        return EvalCubic1DSpline( buffer, u );
jannick.wolters's avatar
jannick.wolters committed
136
137
    }
    else {
138
139
        ErrorMessages::Error( "Unsupported interpolation type!", CURRENT_FUNCTION );
        return -1.0;
jannick.wolters's avatar
jannick.wolters committed
140
141
    }
}
142

jannick.wolters's avatar
jannick.wolters committed
143
// extension of 1D interpolation to Vector input
144
145
146
147
148
149
150
Vector Interpolation::operator()( Vector v ) const {
    Vector res( v.size() );
    for( unsigned i = 0; i < v.size(); ++i ) {
        res[i] = this->operator()( v[i] );
    }
    return res;
}
151

152
// extension of 1D interpolation to Vector input for std::vectors
153
154
155
156
157
158
159
160
std::vector<double> Interpolation::operator()( std::vector<double> v ) const {
    std::vector<double> res( v.size() );
    for( unsigned i = 0; i < v.size(); ++i ) {
        res[i] = this->operator()( v[i] );
    }
    return res;
}

161
// implementation of third degree polynomial
162
163
164
165
166
167
168
inline double Interpolation::EvalCubic1DSpline( double param[4], double x ) const {
    return ( param[1] + 0.5 * x *
                            ( param[2] - param[0] +
                              x * ( 2.0 * param[0] - 5.0 * param[1] + 4.0 * param[2] - param[3] +
                                    x * ( 3.0 * ( param[1] - param[2] ) + param[3] - param[0] ) ) ) );
}

jannick.wolters's avatar
jannick.wolters committed
169
// find index of closes value to 'value' in vector 'v'
170
171
172
173
unsigned Interpolation::IndexOfClosestValue( double value, const Vector& v ) const {
    auto i = std::min_element( begin( v ), end( v ), [=]( double x, double y ) { return abs( x - value ) < abs( y - value ); } );
    return std::distance( begin( v ), i );
}