algorithm/linterp/linterp.h

0001
0002 //
0003 // Copyright (c) 2012 Ronaldo Carpio
0004 //
0005 // Permission to use, copy, modify, distribute and sell this software
0006 // and its documentation for any purpose is hereby granted without fee,
0007 // provided that the above copyright notice appear in all copies and
0008 // that both that copyright notice and this permission notice appear
0009 // in supporting documentation.  The authors make no representations
0010 // about the suitability of this software for any purpose.
0011 // It is provided "as is" without express or implied warranty.
0012 //
0013
0014 /*
0015 This is a C++ header-only library for N-dimensional linear interpolation on a rectangular grid. Implements two methods:
0016 * Multilinear: Interpolate using the N-dimensional hypercube containing the point. Interpolation step is O(2^N)
0017 * Simplicial: Interpolate using the N-dimensional simplex containing the point. Interpolation step is O(N log N), but less accurate.
0018 Requires boost/multi_array library.
0019
0020 For a description of the algorithms, see:
0021 * Weiser & Zarantonello (1988), "A Note on Piecewise Linear and Multilinear Table Interpolation in Many Dimensions", _Mathematics of Computation_ 50 (181), p. 189-196
0022 * Davies (1996), "Multidimensional Triangulation and Interpolation for Reinforcement Learning", _Proceedings of Neural Information Processing Systems 1996_
0023 */
0024
0025 #ifndef _linterp_h
0026 #define _linterp_h
0027
0028 #include <assert.h>
0029 #include <math.h>
0030 #include <stdarg.h>
0031 #include <float.h>
0032 #include <cstdarg>
0033 #include <string>
0034 #include <vector>
0035 #include <array>
0036 #include <functional>
0037
0038 #include <boost/multi_array.hpp>
0039 #include <boost/numeric/ublas/matrix.hpp>
0040 #include <boost/numeric/ublas/matrix_proxy.hpp>
0041 #include <boost/numeric/ublas/storage.hpp>
0042
0043 using std::vector;
0044 using std::array;
0045 typedef unsigned int uint;
0046 typedef vector<int> iVec;
0047 typedef vector<double> dVec;
0048
0049
0050 // TODO:
0051 //  - specify behavior past grid boundaries.
0052 //    1) clamp
0053 //    2) return a pre-determined value (e.g. NaN)
0054
0055 // compile-time params:
0056 //   1) number of dimensions
0057 //   2) scalar type T
0058 //   3) copy data or not (default: false). The grids will always be copied
0059 //   4) ref count class (default: none)
0060 //   5) continuous or not
0061
0062 // run-time constructor params:
0063 //   1) f
0064 //   2) grids
0065 //   3) behavior outside grid: default=clamp
0066 //   4) value to return outside grid, defaut=nan
0067
0068 struct EmptyClass {};
0069
0070 template <int N, class T, bool CopyData = true, bool Continuous = true, class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
0071 class NDInterpolator {
0072 public:
0073   typedef T value_type;
0074   typedef ArrayRefCountT array_ref_count_type;
0075   typedef GridRefCountT grid_ref_count_type;
0076
0077   static const int m_N = N;
0078   static const bool m_bCopyData = CopyData;
0079   static const bool m_bContinuous = Continuous;
0080
0081   typedef boost::numeric::ublas::array_adaptor<T> grid_type;
0082   typedef boost::const_multi_array_ref<T, N> array_type;
0083   typedef std::unique_ptr<array_type> array_type_ptr;
0084
0085   array_type_ptr m_pF;
0086   ArrayRefCountT m_ref_F;                   // reference count for m_pF
0087   vector<T> m_F_copy;                       // if CopyData == true, this holds the copy of F
0088
0089   vector<grid_type> m_grid_list;
0090   vector<GridRefCountT> m_grid_ref_list;    // reference counts for grids
0091   vector<vector<T> > m_grid_copy_list;      // if CopyData == true, this holds the copies of the grids
0092
0093   // constructors assume that [f_begin, f_end) is a contiguous array in C-order
0094   // non ref-counted constructor.
0095   template <class IterT1, class IterT2, class IterT3>
0096   NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end) {
0097     init(grids_begin, grids_len_begin, f_begin, f_end);
0098   }
0099
0100   // ref-counted constructor
0101   template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
0102   NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT grid_refs_begin) {
0103     init_refcount(grids_begin, grids_len_begin, f_begin, f_end, refF, grid_refs_begin);
0104   }
0105
0106   template <class IterT1, class IterT2, class IterT3>
0107   void init(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end) {
0108     set_grids(grids_begin, grids_len_begin, m_bCopyData);
0109     set_f_array(f_begin, f_end, m_bCopyData);
0110   }
0111   template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
0112   void init_refcount(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT grid_refs_begin) {
0113     set_grids(grids_begin, grids_len_begin, m_bCopyData);
0114     set_grids_refcount(grid_refs_begin, grid_refs_begin + N);
0115     set_f_array(f_begin, f_end, m_bCopyData);
0116     set_f_refcount(refF);
0117   }
0118
0119   template <class IterT1, class IterT2>
0120   void set_grids(IterT1 grids_begin, IterT2 grids_len_begin, bool bCopy) {
0121     m_grid_list.clear();
0122     m_grid_ref_list.clear();
0123     m_grid_copy_list.clear();
0124     for (int i=0; i<N; i++) {
0125       int gridLength = grids_len_begin[i];
0126       if (bCopy == false) {
0127         T const *grid_ptr = &(*grids_begin[i]);
0128         m_grid_list.push_back(grid_type(gridLength, (T*) grid_ptr ));                   // use the given pointer
0129       } else {
0130         m_grid_copy_list.push_back( vector<T>(grids_begin[i], grids_begin[i] + grids_len_begin[i]) );   // make our own copy of the grid
0131         T *begin = &(m_grid_copy_list[i][0]);
0132         m_grid_list.push_back(grid_type(gridLength, begin));                            // use our copy
0133       }
0134     }
0135   }
0136   template <class IterT1, class RefCountIterT>
0137   void set_grids_refcount(RefCountIterT refs_begin, RefCountIterT refs_end) {
0138     assert(refs_end - refs_begin == N);
0139     m_grid_ref_list.assign(refs_begin, refs_begin + N);
0140   }
0141
0142   // assumes that [f_begin, f_end) is a contiguous array in C-order
0143   template <class IterT>
0144   void set_f_array(IterT f_begin, IterT f_end, bool bCopy) {
0145     unsigned int nGridPoints = 1;
0146     array<int,N> sizes;
0147     for (unsigned int i=0; i<m_grid_list.size(); i++) {
0148       sizes[i] = m_grid_list[i].size();
0149       nGridPoints *= sizes[i];
0150     }
0151
0152     int f_len = f_end - f_begin;
0153     if ( (m_bContinuous && f_len != nGridPoints) || (!m_bContinuous && f_len != 2 * nGridPoints) ) {
0154       throw std::invalid_argument("f has wrong size");
0155     }
0156     for (unsigned int i=0; i<m_grid_list.size(); i++) {
0157       if (!m_bContinuous) { sizes[i] *= 2; }
0158     }
0159
0160     m_F_copy.clear();
0161     if (bCopy == false) {
0162       m_pF.reset(new array_type(f_begin, sizes));
0163     } else {
0164       m_F_copy = vector<T>(f_begin, f_end);
0165       m_pF.reset(new array_type(&m_F_copy[0], sizes));
0166     }
0167   }
0168   void set_f_refcount(ArrayRefCountT &refF) {
0169     m_ref_F = refF;
0170   }
0171
0172   // -1 is before the first grid point
0173   // N-1 (where grid.size() == N) is after the last grid point
0174   int find_cell(int dim, T x) const {
0175     grid_type const &grid(m_grid_list[dim]);
0176     if (x < *(grid.begin())) return -1;
0177     else if (x >= *(grid.end()-1)) return grid.size()-1;
0178     else {
0179       auto i_upper = std::upper_bound(grid.begin(), grid.end(), x);
0180       return i_upper - grid.begin() - 1;
0181     }
0182   }
0183
0184   // return the value of f at the given cell and vertex
0185   T get_f_val(array<int,N> const &cell_index, array<int,N> const &v_index) const {
0186     array<int,N> f_index;
0187
0188     if (m_bContinuous) {
0189       for (int i=0; i<N; i++) {
0190         if (cell_index[i] < 0) {
0191           f_index[i] = 0;
0192         } else if (cell_index[i] >= m_grid_list[i].size()-1) {
0193           f_index[i] = m_grid_list[i].size()-1;
0194         } else {
0195           f_index[i] = cell_index[i] + v_index[i];
0196         }
0197       }
0198     } else {
0199       for (int i=0; i<N; i++) {
0200         if (cell_index[i] < 0) {
0201           f_index[i] = 0;
0202         } else if (cell_index[i] >= m_grid_list[i].size()-1) {
0203           f_index[i] = (2*m_grid_list[i].size())-1;
0204         } else {
0205           f_index[i] = 1 + (2*cell_index[i]) + v_index[i];
0206         }
0207       }
0208     }
0209     return (*m_pF)(f_index);
0210   }
0211
0212   T get_f_val(array<int,N> const &cell_index, int v) const {
0213     array<int,N> v_index;
0214     for (int dim=0; dim<N; dim++) {
0215       v_index[dim] = (v >> (N-dim-1)) & 1;                      // test if the i-th bit is set
0216     }
0217     return get_f_val(cell_index, v_index);
0218   }
0219 };
0220
0221 template <int N, class T, bool CopyData = true, bool Continuous = true, class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
0222 class InterpSimplex : public NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT> {
0223 public:
0224   typedef NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT> super;
0225
0226   template <class IterT1, class IterT2, class IterT3>
0227   InterpSimplex(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end)
0228     : super(grids_begin, grids_len_begin, f_begin, f_end)
0229   {}
0230   template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
0231   InterpSimplex(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT ref_begins)
0232     : super(grids_begin, grids_len_begin, f_begin, f_end, refF, ref_begins)
0233   {}
0234
0235   template <class IterT>
0236   T interp(IterT x_begin) const {
0237     array<T,1> result;
0238     array< array<T,1>, N > coord_iter;
0239     for (int i=0; i<N; i++) {
0240       coord_iter[i][0] = x_begin[i];
0241     }
0242     interp_vec(1, coord_iter.begin(), coord_iter.end(), result.begin());
0243     return result[0];
0244   }
0245
0246   template <class IterT1, class IterT2>
0247   void interp_vec(int n, IterT1 coord_iter_begin, IterT1 coord_iter_end, IterT2 i_result) const {
0248     assert(N == coord_iter_end - coord_iter_begin);
0249
0250     array<int,N> cell_index, v_index;
0251     array<std::pair<T, int>,N> xipair;
0252     int c;
0253     T y, v0, v1;
0254     //mexPrintf("%d\n", n);
0255     for (int i=0; i<n; i++) {           // for each point
0256       for (int dim=0; dim<N; dim++) {
0257         typename super::grid_type const &grid(super::m_grid_list[dim]);
0258         c = this->find_cell(dim, coord_iter_begin[dim][i]);
0259         //mexPrintf("%d\n", c);
0260         if (c == -1) {                  // before first grid point
0261           y = 1.0;
0262         } else if (c == grid.size()-1) {    // after last grid point
0263           y = 0.0;
0264         } else {
0265           //mexPrintf("%f %f\n", grid[c], grid[c+1]);
0266           y = (coord_iter_begin[dim][i] - grid[c]) / (grid[c + 1] - grid[c]);
0267           if (y < 0.0) y=0.0;
0268           else if (y > 1.0) y=1.0;
0269         }
0270         xipair[dim].first = y;
0271         xipair[dim].second = dim;
0272         cell_index[dim] = c;
0273       }
0274       // sort xi's and get the permutation
0275       std::sort(xipair.begin(), xipair.end(), [](std::pair<T, int> const &a, std::pair<T, int> const &b) {
0276         return (a.first < b.first);
0277       });
0278       // walk the vertices of the simplex determined by the permutation
0279       for (int j=0; j<N; j++) {
0280         v_index[j] = 1;
0281       }
0282       v0 = this->get_f_val(cell_index, v_index);
0283       y = v0;
0284       for (int j=0; j<N; j++) {
0285         v_index[xipair[j].second]--;
0286         v1 = this->get_f_val(cell_index, v_index);
0287         y += (1.0 - xipair[j].first) * (v1-v0);     // interpolate
0288         v0 = v1;
0289       }
0290       *i_result++ = y;
0291     }
0292   }
0293 };
0294
0295 template <int N, class T, bool CopyData = true, bool Continuous = true, class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
0296 class InterpMultilinear : public NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT> {
0297 public:
0298   typedef NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT> super;
0299
0300   template <class IterT1, class IterT2, class IterT3>
0301   InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end)
0302     : super(grids_begin, grids_len_begin, f_begin, f_end)
0303   {}
0304   template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
0305   InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT ref_begins)
0306     : super(grids_begin, grids_len_begin, f_begin, f_end, refF, ref_begins)
0307   {}
0308
0309   template <class IterT1, class IterT2>
0310   static T linterp_nd_unitcube(IterT1 f_begin, IterT1 f_end, IterT2 xi_begin, IterT2 xi_end) {
0311     int n = xi_end - xi_begin;
0312     int f_len = f_end - f_begin;
0313     assert(1 << n == f_len);
0314     T sub_lower, sub_upper;
0315     if (n == 1) {
0316       sub_lower = f_begin[0];
0317       sub_upper = f_begin[1];
0318     } else {
0319       sub_lower = linterp_nd_unitcube(f_begin, f_begin + (f_len/2), xi_begin + 1, xi_end);
0320       sub_upper = linterp_nd_unitcube(f_begin + (f_len/2), f_end, xi_begin + 1, xi_end);
0321     }
0322     T result = sub_lower + (*xi_begin)*(sub_upper - sub_lower);
0323     return result;
0324   }
0325
0326   template <class IterT>
0327   T interp(IterT x_begin) const {
0328     array<T,1> result;
0329     array< array<T,1>, N > coord_iter;
0330     for (int i=0; i<N; i++) {
0331       coord_iter[i][0] = x_begin[i];
0332     }
0333     interp_vec(1, coord_iter.begin(), coord_iter.end(), result.begin());
0334     return result[0];
0335   }
0336
0337   template <class IterT1, class IterT2>
0338   void interp_vec(int n, IterT1 coord_iter_begin, IterT1 coord_iter_end, IterT2 i_result) const {
0339     assert(N == coord_iter_end - coord_iter_begin);
0340     array<int,N> index;
0341     int c;
0342     T y, xi;
0343     vector<T> f(1 << N);
0344     array<T,N> x;
0345
0346     for (int i=0; i<n; i++) {                               // loop over each point
0347       for (int dim=0; dim<N; dim++) {                       // loop over each dimension
0348         auto const &grid(super::m_grid_list[dim]);
0349         xi = coord_iter_begin[dim][i];
0350         c = this->find_cell(dim, coord_iter_begin[dim][i]);
0351         if (c == -1) {                  // before first grid point
0352           y = 1.0;
0353         } else if (c == grid.size()-1) {    // after last grid point
0354           y = 0.0;
0355         } else {
0356           y = (coord_iter_begin[dim][i] - grid[c]) / (grid[c + 1] - grid[c]);
0357           if (y < 0.0) y=0.0;
0358           else if (y > 1.0) y=1.0;
0359         }
0360         index[dim] = c;
0361         x[dim] = y;
0362       }
0363       // copy f values at vertices
0364       for (int v=0; v < (1 << N); v++) {                    // loop over each vertex of hypercube
0365         f[v] = this->get_f_val(index, v);
0366       }
0367       *i_result++ = linterp_nd_unitcube(f.begin(), f.end(), x.begin(), x.end());
0368     }
0369   }
0370 };
0371
0372 typedef InterpSimplex<1,double> NDInterpolator_1_S;
0373 typedef InterpSimplex<2,double> NDInterpolator_2_S;
0374 typedef InterpSimplex<3,double> NDInterpolator_3_S;
0375 typedef InterpSimplex<4,double> NDInterpolator_4_S;
0376 typedef InterpSimplex<5,double> NDInterpolator_5_S;
0377 typedef InterpMultilinear<1,double> NDInterpolator_1_ML;
0378 typedef InterpMultilinear<2,double> NDInterpolator_2_ML;
0379 typedef InterpMultilinear<3,double> NDInterpolator_3_ML;
0380 typedef InterpMultilinear<4,double> NDInterpolator_4_ML;
0381 typedef InterpMultilinear<5,double> NDInterpolator_5_ML;
0382
0383 // C interface
0384 extern "C" {
0385   void linterp_simplex_1(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult);
0386   void linterp_simplex_2(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult);
0387   void linterp_simplex_3(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult);
0388 }
0389
0390 void inline linterp_simplex_1(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult) {
0391   const int N=1;
0392   size_t total_size = 1;
0393   for (int i=0; i<N; i++)   {
0394     total_size *= grid_len_begin[i];
0395   }
0396   InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF, pF + total_size);
0397   interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
0398 }
0399
0400 void inline linterp_simplex_2(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult) {
0401   const int N=2;
0402   size_t total_size = 1;
0403   for (int i=0; i<N; i++)   {
0404     total_size *= grid_len_begin[i];
0405   }
0406   InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF, pF + total_size);
0407   interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
0408 }
0409
0410 void inline linterp_simplex_3(double **grids_begin, int *grid_len_begin, double *pF, int xi_len, double **xi_begin, double *pResult) {
0411   const int N=3;
0412   size_t total_size = 1;
0413   for (int i=0; i<N; i++)   {
0414     total_size *= grid_len_begin[i];
0415   }
0416   InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF, pF + total_size);
0417   interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
0418 }
0419
0420
0421
0422
0423 #endif //_linterp_h