spline.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD-3 License)
00003  *
00004  * Copyright (c) 2011, 2014 Tino Kluge
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name Tino Kluge nor the
00015  *       names of its contributors may be used to endorse or promote products
00016  *       derived from this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021  * DISCLAIMED. IN NO EVENT SHALL TINO KLUGE BE LIABLE FOR ANY
00022  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028  *********************************************************************/
00029 
00030 /* Author: Tino Kluge (ttk448 at gmail.com)
00031  *
00032  * spline.h
00033  *
00034  * simple cubic spline interpolation library without external
00035  * dependencies
00036  *
00037  */
00038 
00039 #ifndef TK_SPLINE_H
00040 #define TK_SPLINE_H
00041 
00042 #include <cstdio>
00043 #include <cassert>
00044 #include <vector>
00045 #include <algorithm>
00046 
00047 // unnamed namespace only because the implementation is in this
00048 // header file and we don't want to export symbols to the obj files
00049 namespace
00050 {
00051 namespace tk
00052 {
00053 // band matrix solver
00054 class band_matrix
00055 {
00056 private:
00057   std::vector<std::vector<double> > m_upper;  // upper band
00058   std::vector<std::vector<double> > m_lower;  // lower band
00059 public:
00060   band_matrix(){};                         // constructor
00061   band_matrix(int dim, int n_u, int n_l);  // constructor
00062   ~band_matrix(){};                        // destructor
00063   void resize(int dim, int n_u, int n_l);  // init with dim,n_u,n_l
00064   int dim() const;                         // matrix dimension
00065   int num_upper() const
00066   {
00067     return m_upper.size() - 1;
00068   }
00069   int num_lower() const
00070   {
00071     return m_lower.size() - 1;
00072   }
00073   // access operator
00074   double& operator()(int i, int j);       // write
00075   double operator()(int i, int j) const;  // read
00076   // we can store an additional diogonal (in m_lower)
00077   double& saved_diag(int i);
00078   double saved_diag(int i) const;
00079   void lu_decompose();
00080   std::vector<double> r_solve(const std::vector<double>& b) const;
00081   std::vector<double> l_solve(const std::vector<double>& b) const;
00082   std::vector<double> lu_solve(const std::vector<double>& b, bool is_lu_decomposed = false);
00083 };
00084 
00085 // spline interpolation
00086 class spline
00087 {
00088 public:
00089   enum bd_type
00090   {
00091     first_deriv = 1,
00092     second_deriv = 2
00093   };
00094 
00095 private:
00096   std::vector<double> m_x, m_y;  // x,y coordinates of points
00097   // interpolation parameters
00098   // f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
00099   std::vector<double> m_a, m_b, m_c;  // spline coefficients
00100   double m_b0, m_c0;                  // for left extrapol
00101   bd_type m_left, m_right;
00102   double m_left_value, m_right_value;
00103   bool m_force_linear_extrapolation;
00104 
00105 public:
00106   // set default boundary condition to be zero curvature at both ends
00107   spline()
00108     : m_left(second_deriv)
00109     , m_right(second_deriv)
00110     , m_left_value(0.0)
00111     , m_right_value(0.0)
00112     , m_force_linear_extrapolation(false)
00113   {
00114     ;
00115   }
00116 
00117   // optional, but if called it has to come be before set_points()
00118   void set_boundary(bd_type left, double left_value, bd_type right, double right_value,
00119                     bool force_linear_extrapolation = false);
00120   void set_points(const std::vector<double>& x, const std::vector<double>& y, bool cubic_spline = true);
00121   double operator()(double x) const;
00122   double deriv(int order, double x) const;
00123 };
00124 
00125 // ---------------------------------------------------------------------
00126 // implementation part, which could be separated into a cpp file
00127 // ---------------------------------------------------------------------
00128 
00129 // band_matrix implementation
00130 // -------------------------
00131 
00132 band_matrix::band_matrix(int dim, int n_u, int n_l)
00133 {
00134   resize(dim, n_u, n_l);
00135 }
00136 void band_matrix::resize(int dim, int n_u, int n_l)
00137 {
00138   assert(dim > 0);
00139   assert(n_u >= 0);
00140   assert(n_l >= 0);
00141   m_upper.resize(n_u + 1);
00142   m_lower.resize(n_l + 1);
00143   for (size_t i = 0; i < m_upper.size(); i++)
00144   {
00145     m_upper[i].resize(dim);
00146   }
00147   for (size_t i = 0; i < m_lower.size(); i++)
00148   {
00149     m_lower[i].resize(dim);
00150   }
00151 }
00152 int band_matrix::dim() const
00153 {
00154   if (m_upper.size() > 0)
00155   {
00156     return m_upper[0].size();
00157   }
00158   else
00159   {
00160     return 0;
00161   }
00162 }
00163 
00164 // defines the new operator (), so that we can access the elements
00165 // by A(i,j), index going from i=0,...,dim()-1
00166 double& band_matrix::operator()(int i, int j)
00167 {
00168   int k = j - i;  // what band is the entry
00169   assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
00170   assert((-num_lower() <= k) && (k <= num_upper()));
00171   // k=0 -> diogonal, k<0 lower left part, k>0 upper right part
00172   if (k >= 0)
00173     return m_upper[k][i];
00174   else
00175     return m_lower[-k][i];
00176 }
00177 double band_matrix::operator()(int i, int j) const
00178 {
00179   int k = j - i;  // what band is the entry
00180   assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
00181   assert((-num_lower() <= k) && (k <= num_upper()));
00182   // k=0 -> diogonal, k<0 lower left part, k>0 upper right part
00183   if (k >= 0)
00184     return m_upper[k][i];
00185   else
00186     return m_lower[-k][i];
00187 }
00188 // second diag (used in LU decomposition), saved in m_lower
00189 double band_matrix::saved_diag(int i) const
00190 {
00191   assert((i >= 0) && (i < dim()));
00192   return m_lower[0][i];
00193 }
00194 double& band_matrix::saved_diag(int i)
00195 {
00196   assert((i >= 0) && (i < dim()));
00197   return m_lower[0][i];
00198 }
00199 
00200 // LR-Decomposition of a band matrix
00201 void band_matrix::lu_decompose()
00202 {
00203   int i_max, j_max;
00204   int j_min;
00205   double x;
00206 
00207   // preconditioning
00208   // normalize column i so that a_ii=1
00209   for (int i = 0; i < this->dim(); i++)
00210   {
00211     assert(this->operator()(i, i) != 0.0);
00212     this->saved_diag(i) = 1.0 / this->operator()(i, i);
00213     j_min = std::max(0, i - this->num_lower());
00214     j_max = std::min(this->dim() - 1, i + this->num_upper());
00215     for (int j = j_min; j <= j_max; j++)
00216     {
00217       this->operator()(i, j) *= this->saved_diag(i);
00218     }
00219     this->operator()(i, i) = 1.0;  // prevents rounding errors
00220   }
00221 
00222   // Gauss LR-Decomposition
00223   for (int k = 0; k < this->dim(); k++)
00224   {
00225     i_max = std::min(this->dim() - 1, k + this->num_lower());  // num_lower not a mistake!
00226     for (int i = k + 1; i <= i_max; i++)
00227     {
00228       assert(this->operator()(k, k) != 0.0);
00229       x = -this->operator()(i, k) / this->operator()(k, k);
00230       this->operator()(i, k) = -x;  // assembly part of L
00231       j_max = std::min(this->dim() - 1, k + this->num_upper());
00232       for (int j = k + 1; j <= j_max; j++)
00233       {
00234         // assembly part of R
00235         this->operator()(i, j) = this->operator()(i, j) + x * this->operator()(k, j);
00236       }
00237     }
00238   }
00239 }
00240 // solves Ly=b
00241 std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const
00242 {
00243   assert(this->dim() == (int)b.size());
00244   std::vector<double> x(this->dim());
00245   int j_start;
00246   double sum;
00247   for (int i = 0; i < this->dim(); i++)
00248   {
00249     sum = 0;
00250     j_start = std::max(0, i - this->num_lower());
00251     for (int j = j_start; j < i; j++)
00252       sum += this->operator()(i, j) * x[j];
00253     x[i] = (b[i] * this->saved_diag(i)) - sum;
00254   }
00255   return x;
00256 }
00257 // solves Rx=y
00258 std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const
00259 {
00260   assert(this->dim() == (int)b.size());
00261   std::vector<double> x(this->dim());
00262   int j_stop;
00263   double sum;
00264   for (int i = this->dim() - 1; i >= 0; i--)
00265   {
00266     sum = 0;
00267     j_stop = std::min(this->dim() - 1, i + this->num_upper());
00268     for (int j = i + 1; j <= j_stop; j++)
00269       sum += this->operator()(i, j) * x[j];
00270     x[i] = (b[i] - sum) / this->operator()(i, i);
00271   }
00272   return x;
00273 }
00274 
00275 std::vector<double> band_matrix::lu_solve(const std::vector<double>& b, bool is_lu_decomposed)
00276 {
00277   assert(this->dim() == (int)b.size());
00278   std::vector<double> x, y;
00279   if (is_lu_decomposed == false)
00280   {
00281     this->lu_decompose();
00282   }
00283   y = this->l_solve(b);
00284   x = this->r_solve(y);
00285   return x;
00286 }
00287 
00288 // spline implementation
00289 // -----------------------
00290 
00291 void spline::set_boundary(spline::bd_type left, double left_value, spline::bd_type right, double right_value,
00292                           bool force_linear_extrapolation)
00293 {
00294   assert(m_x.size() == 0);  // set_points() must not have happened yet
00295   m_left = left;
00296   m_right = right;
00297   m_left_value = left_value;
00298   m_right_value = right_value;
00299   m_force_linear_extrapolation = force_linear_extrapolation;
00300 }
00301 
00302 void spline::set_points(const std::vector<double>& x, const std::vector<double>& y, bool cubic_spline)
00303 {
00304   assert(x.size() == y.size());
00305   assert(x.size() > 2);
00306   m_x = x;
00307   m_y = y;
00308   int n = x.size();
00309   // TODO: maybe sort x and y, rather than returning an error
00310   for (int i = 0; i < n - 1; i++)
00311   {
00312     assert(m_x[i] < m_x[i + 1]);
00313   }
00314 
00315   if (cubic_spline == true)
00316   {  // cubic spline interpolation
00317     // setting up the matrix and right hand side of the equation system
00318     // for the parameters b[]
00319     band_matrix A(n, 1, 1);
00320     std::vector<double> rhs(n);
00321     for (int i = 1; i < n - 1; i++)
00322     {
00323       A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]);
00324       A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]);
00325       A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]);
00326       rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
00327     }
00328     // boundary conditions
00329     if (m_left == spline::second_deriv)
00330     {
00331       // 2*b[0] = f''
00332       A(0, 0) = 2.0;
00333       A(0, 1) = 0.0;
00334       rhs[0] = m_left_value;
00335     }
00336     else if (m_left == spline::first_deriv)
00337     {
00338       // c[0] = f', needs to be re-expressed in terms of b:
00339       // (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
00340       A(0, 0) = 2.0 * (x[1] - x[0]);
00341       A(0, 1) = 1.0 * (x[1] - x[0]);
00342       rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value);
00343     }
00344     else
00345     {
00346       assert(false);
00347     }
00348     if (m_right == spline::second_deriv)
00349     {
00350       // 2*b[n-1] = f''
00351       A(n - 1, n - 1) = 2.0;
00352       A(n - 1, n - 2) = 0.0;
00353       rhs[n - 1] = m_right_value;
00354     }
00355     else if (m_right == spline::first_deriv)
00356     {
00357       // c[n-1] = f', needs to be re-expressed in terms of b:
00358       // (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
00359       // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
00360       A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]);
00361       A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]);
00362       rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]));
00363     }
00364     else
00365     {
00366       assert(false);
00367     }
00368 
00369     // solve the equation system to obtain the parameters b[]
00370     m_b = A.lu_solve(rhs);
00371 
00372     // calculate parameters a[] and c[] based on b[]
00373     m_a.resize(n);
00374     m_c.resize(n);
00375     for (int i = 0; i < n - 1; i++)
00376     {
00377       m_a[i] = 1.0 / 3.0 * (m_b[i + 1] - m_b[i]) / (x[i + 1] - x[i]);
00378       m_c[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - 1.0 / 3.0 * (2.0 * m_b[i] + m_b[i + 1]) * (x[i + 1] - x[i]);
00379     }
00380   }
00381   else
00382   {  // linear interpolation
00383     m_a.resize(n);
00384     m_b.resize(n);
00385     m_c.resize(n);
00386     for (int i = 0; i < n - 1; i++)
00387     {
00388       m_a[i] = 0.0;
00389       m_b[i] = 0.0;
00390       m_c[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]);
00391     }
00392   }
00393 
00394   // for left extrapolation coefficients
00395   m_b0 = (m_force_linear_extrapolation == false) ? m_b[0] : 0.0;
00396   m_c0 = m_c[0];
00397 
00398   // for the right extrapolation coefficients
00399   // f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
00400   double h = x[n - 1] - x[n - 2];
00401   // m_b[n-1] is determined by the boundary condition
00402   m_a[n - 1] = 0.0;
00403   m_c[n - 1] = 3.0 * m_a[n - 2] * h * h + 2.0 * m_b[n - 2] * h + m_c[n - 2];  // = f'_{n-2}(x_{n-1})
00404   if (m_force_linear_extrapolation == true)
00405     m_b[n - 1] = 0.0;
00406 }
00407 
00408 double spline::operator()(double x) const
00409 {
00410   size_t n = m_x.size();
00411   // find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
00412   std::vector<double>::const_iterator it;
00413   it = std::lower_bound(m_x.begin(), m_x.end(), x);
00414   int idx = std::max(int(it - m_x.begin()) - 1, 0);
00415 
00416   double h = x - m_x[idx];
00417   double interpol;
00418   if (x < m_x[0])
00419   {
00420     // extrapolation to the left
00421     interpol = (m_b0 * h + m_c0) * h + m_y[0];
00422   }
00423   else if (x > m_x[n - 1])
00424   {
00425     // extrapolation to the right
00426     interpol = (m_b[n - 1] * h + m_c[n - 1]) * h + m_y[n - 1];
00427   }
00428   else
00429   {
00430     // interpolation
00431     interpol = ((m_a[idx] * h + m_b[idx]) * h + m_c[idx]) * h + m_y[idx];
00432   }
00433   return interpol;
00434 }
00435 
00436 double spline::deriv(int order, double x) const
00437 {
00438   assert(order > 0);
00439 
00440   size_t n = m_x.size();
00441   // find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
00442   std::vector<double>::const_iterator it;
00443   it = std::lower_bound(m_x.begin(), m_x.end(), x);
00444   int idx = std::max(int(it - m_x.begin()) - 1, 0);
00445 
00446   double h = x - m_x[idx];
00447   double interpol;
00448   if (x < m_x[0])
00449   {
00450     // extrapolation to the left
00451     switch (order)
00452     {
00453       case 1:
00454         interpol = 2.0 * m_b0 * h + m_c0;
00455         break;
00456       case 2:
00457         interpol = 2.0 * m_b0 * h;
00458         break;
00459       default:
00460         interpol = 0.0;
00461         break;
00462     }
00463   }
00464   else if (x > m_x[n - 1])
00465   {
00466     // extrapolation to the right
00467     switch (order)
00468     {
00469       case 1:
00470         interpol = 2.0 * m_b[n - 1] * h + m_c[n - 1];
00471         break;
00472       case 2:
00473         interpol = 2.0 * m_b[n - 1];
00474         break;
00475       default:
00476         interpol = 0.0;
00477         break;
00478     }
00479   }
00480   else
00481   {
00482     // interpolation
00483     switch (order)
00484     {
00485       case 1:
00486         interpol = (3.0 * m_a[idx] * h + 2.0 * m_b[idx]) * h + m_c[idx];
00487         break;
00488       case 2:
00489         interpol = 6.0 * m_a[idx] * h + 2.0 * m_b[idx];
00490         break;
00491       case 3:
00492         interpol = 6.0 * m_a[idx];
00493         break;
00494       default:
00495         interpol = 0.0;
00496         break;
00497     }
00498   }
00499   return interpol;
00500 }
00501 
00502 }  // namespace tk
00503 
00504 }  // namespace
00505 
00506 #endif /* TK_SPLINE_H */


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36