All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
piecewise_polynom_function.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  */
19 
20 #ifndef PIECEWISE_POLYNOM
21 #define PIECEWISE_POLYNOM
22 
23 #include "fmatvec/fmatvec.h"
24 #include "mbsim/functions/function.h"
25 #include "mbsim/utils/utils.h"
26 #include "mbsim/utils/eps.h"
27 #include "mbsim/mbsim_event.h"
28 
29 namespace MBSim {
30 
31  template<typename Sig> class PiecewisePolynomFunction;
32 
62  template<typename Ret, typename Arg>
63  class PiecewisePolynomFunction<Ret(Arg)> : public Function<Ret(Arg)> {
65 
66  public:
67  enum InterpolationMethod {
68  cSplinePeriodic,
69  cSplineNatural,
70  piecewiseLinear,
71  unknown
72  };
73 
74  PiecewisePolynomFunction() : index(0), method(cSplineNatural), f(this), fd(this), fdd(this) { }
75 
76  int getArgSize() const { return 1; }
77 
78  std::pair<int, int> getRetSize() const { return std::make_pair(y.cols(),1); }
79 
80  void init(Element::InitStage stage, const InitConfigSet &config) {
81  Function<Ret(Arg)>::init(stage, config);
82  if(stage==Element::preInit) {
83  if(method==unknown)
84  Element::throwError("(PiecewisePolynomFunction::init): interpolation method unknown");
85  if(y.rows() != x.size())
86  this->throwError("Dimension missmatch in size of x");
87  if(x.size()) calculateSpline();
88  nPoly = (coefs[0]).rows();
89  order = coefs.size()-1;
90  }
91  }
92 
93  void calculateSpline() {
94  if(method == cSplinePeriodic) calculateSplinePeriodic();
95  else if(method == cSplineNatural) calculateSplineNatural();
96  else if(method == piecewiseLinear) calculatePLinear();
97  else this->throwError("(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
98  }
99 
100  void reset() {
101  index = 0;
102  f.reset();
103  fd.reset();
104  fdd.reset();
105  coefs.clear();
106  }
107 
108  Ret operator()(const Arg &x) { return f(x); }
109  typename B::DRetDArg parDer(const Arg &x) { return fd(x); }
110  Ret parDerParDer(const double &x) { return fdd(x); }
111  typename B::DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(arg)*ToDouble<Arg>::cast(argDir); }
112 
116  std::vector<fmatvec::MatV> getCoefficients() { return coefs; }
117 
121  fmatvec::VecV getBreaks() { return breaks; }
122 
133  void setInterpolationMethod(InterpolationMethod method_) { method = method_; }
134 
135  void setx(const fmatvec::VecV &x_) { x = x_; }
136  void sety(const fmatvec::MatV &y_) { y = y_; }
137  void setxy(const fmatvec::MatV &xy) {
138  if(xy.cols() <= 1)
139  this->throwError("Dimension missmatch in size of xy");
140  x = xy.col(0);
141  y = xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
142  }
143 
148  void setCoefficients(const std::vector<fmatvec::MatV> &coefs_u) { coefs = coefs_u; }
149 
154  void setBreaks(const std::vector<fmatvec::MatV> &coefs_u, const fmatvec::VecV &breaks_u) { breaks = breaks_u; }
155 
160  virtual void initializeUsingXML(xercesc::DOMElement *element);
161 
162  protected:
166  std::vector<fmatvec::MatV> coefs;
167 
171  fmatvec::VecV breaks;
172 
176  int nPoly;
177 
181  int order;
182 
186  int index;
187 
188  fmatvec::VecV x;
189  fmatvec::MatV y;
190 
194  InterpolationMethod method;
195 
201  void calculateSplinePeriodic();
202 
208  void calculateSplineNatural();
209 
210  /*
211  * \brief calculation of piecewise linear interpolation
212  * \param interpolated arguments
213  * \param interpolated function values
214  *
215  * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
216  */
217  void calculatePLinear();
218 
222  class ZerothDerivative {
223  public:
224  ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
225 
226  void reset() { firstCall = true; }
227 
228  Ret operator()(const Arg &x);
229 
230  private:
232  double xSave;
233  fmatvec::VecV ySave;
234  bool firstCall;
235  };
236 
240  class FirstDerivative {
241  public:
242  FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
243 
244  void reset() { firstCall = true; }
245 
246  Ret operator()(const Arg& x);
247 
248  private:
250  double xSave;
251  fmatvec::VecV ySave;
252  bool firstCall;
253  };
254 
258  class SecondDerivative {
259  public:
260  SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
261 
262  void reset() { firstCall = true; }
263 
264  Ret operator()(const Arg& x);
265 
266  private:
268  double xSave;
269  fmatvec::VecV ySave;
270  bool firstCall;
271  };
272 
273  private:
274  ZerothDerivative f;
275  FirstDerivative fd;
276  SecondDerivative fdd;
277  };
278 
279  template<typename Ret, typename Arg>
281  double hi, hii;
282  int N = x.size();
283  if(nrm2(y.row(0)-y.row(y.rows()-1))>epsroot) this->throwError("(PiecewisePolynomFunction::calculateSplinePeriodic): f(0)= "+fmatvec::toString(y.row(0))+"!="+fmatvec::toString(y.row(y.rows()-1))+" =f(end)");
284  fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
285  fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
286 
287  // Matrix C and vector rs C*c=rs
288  for(int i=0; i<N-3;i++) {
289  hi = x(i+1) - x(i);
290  hii = x(i+2)-x(i+1);
291  C(i,i) = hi;
292  C(i,i+1) = 2*(hi+hii);
293  C(i,i+2) = hii;
294  rs.row(i) = 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi);
295  }
296 
297  // last but one row
298  hi = x(N-2)-x(N-3);
299  hii = x(N-1)-x(N-2);
300  C(N-3,N-3) = hi;
301  C(N-3,N-2)= 2*(hi+hii);
302  C(N-3,0)= hii;
303  rs.row(N-3) = 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi);
304 
305  // last row
306  double h1 = x(1)-x(0);
307  double hN_1 = x(N-1)-x(N-2);
308  C(N-2,0) = 2*(h1+hN_1);
309  C(N-2,1) = h1;
310  C(N-2,N-2)= hN_1;
311  rs.row(N-2) = 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1);
312 
313  // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
314  fmatvec::Mat c = slvLU(C,rs);
315  fmatvec::Mat ctmp(N,y.cols());
316  ctmp.row(N-1) = c.row(0); // CN = c1
317  ctmp(0,0,N-2,y.cols()-1)= c;
318 
319  // vector ordering of the further coefficients
320  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
321  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
322  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
323 
324  for(int i=0; i<N-1; i++) {
325  hi = x(i+1)-x(i);
326  a.row(i) = y.row(i);
327  d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
328  b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
329  }
330 
331  breaks.resize(N);
332  breaks = x;
333  coefs.push_back(d);
334  coefs.push_back(c);
335  coefs.push_back(b);
336  coefs.push_back(a);
337  }
338 
339  template<typename Ret, typename Arg>
341  // first row
342  int i=0;
343  int N = x.size();
344  if(N<4) this->throwError("(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
345  fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
346  fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
347  double hi = x(i+1)-x(i);
348  double hii = x(i+2)-x(i+1);
349  C(i,i) = 2*hi+2*hii;
350  C(i,i+1) = hii;
351  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
352 
353  // last row
354  i = (N-3);
355  hi = x(i+1)-x(i);
356  hii = x(i+2)-x(i+1);
357  C(i,i-1) = hi;
358  C(i,i) = 2*hii + 2*hi;
359  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
360 
361  for(i=1;i<N-3;i++) {
362  hi = x(i+1)-x(i);
363  hii = x(i+2)-x(i+1);
364  C(i,i-1) = hi;
365  C(i,i) = 2*(hi+hii);
366  C(i,i+1) = hii;
367  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
368  }
369 
370  // solve C*c = rs with C tridiagonal
371  fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
372  C_rs(0,0,N-3,N-3) = C; // C_rs=[C rs] for Gauss in matrix
373  C_rs(0,N-2,N-3,N-2+y.cols()-1) = rs;
374  for(i=1; i<N-2; i++) C_rs.row(i) = C_rs.row(i) - C_rs.row(i-1)*C_rs(i,i-1)/C_rs(i-1,i-1); // C_rs -> upper triangular matrix C1 -> C1 rs1
375  fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+y.cols()-1);
376  fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
377  fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
378  for(i=N-3;i>=0 ;i--) { // backward substitution
379  fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
380  sum_ciCi.init(0.);
381  for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
382  c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
383  }
384  fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
385  ctmp(1,0,N-2,y.cols()-1) = c; // c1=cN=0 natural splines c=[ 0; c; 0]
386 
387  // vector ordering of the further coefficients
388  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
389  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
390  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
391 
392  for(i=0; i<N-1; i++) {
393  hi = x(i+1)-x(i);
394  a.row(i) = y.row(i);
395  d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
396  b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
397  }
398 
399  breaks.resize(N);
400  breaks = x;
401  coefs.push_back(d);
402  coefs.push_back(ctmp(0,0,N-2,y.cols()-1));
403  coefs.push_back(b);
404  coefs.push_back(a);
405  }
406 
407  template<typename Ret, typename Arg>
409  int N = x.size(); // number of supporting points
410 
411  breaks.resize(N);
412  breaks = x;
413 
414  fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
415  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
416  for(int i=1;i<N;i++) {
417  m.row(i-1) = (y.row(i)-y.row(i-1))/(x(i)-x(i-1)); // slope
418  a.row(i-1) = y.row(i-1);
419  }
420  coefs.push_back(m);
421  coefs.push_back(a);
422  }
423 
424  template<typename Ret, typename Arg>
425  Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
426  double x = ToDouble<Arg>::cast(x_);
427  if(x-1e-13>(parent->breaks)(parent->nPoly))
428  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
429  if(x+1e-13<(parent->breaks)(0))
430  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", lower bound= "+fmatvec::toString((parent->breaks)(0)));
431 
432  if ((fabs(x-xSave)<macheps) && !firstCall)
433  return FromVecV<Ret>::cast(ySave);
434  else {
435  firstCall = false;
436  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
437  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
438  (parent->index)--;
439  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
440  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
441  (parent->index)++;
442  (parent->index)--;
443  }
444 
445  const double dx = x - (parent->breaks)(parent->index); // local coordinate
446  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
447  for(int i=1;i<=(parent->order);i++) // Horner scheme
448  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
449  xSave=x;
450  ySave=yi;
451  return FromVecV<Ret>::cast(yi);
452  }
453  }
454 
455  template<typename Ret, typename Arg>
456  Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
457  double x = ToDouble<Arg>::cast(x_);
458  if(x-1e-13>(parent->breaks)(parent->nPoly)) throw std::runtime_error("(PiecewisePolynomFunction::diff1): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
459  if(x+1e-13<(parent->breaks)(0)) throw std::runtime_error("(PiecewisePolynomFunction::diff1): x out of range! x= "+fmatvec::toString(x)+" lower bound= "+fmatvec::toString((parent->breaks)(0)));
460 
461  if ((fabs(x-xSave)<macheps) && !firstCall)
462  return FromVecV<Ret>::cast(ySave);
463  else {
464  firstCall = false;
465  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
466  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
467  (parent->index)--;
468  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
469  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
470  (parent->index)++;
471  (parent->index)--;
472  }
473 
474  double dx = x - (parent->breaks)(parent->index);
475  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
476  for(int i=1;i<parent->order;i++)
477  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
478  xSave=x;
479  ySave=yi;
480  return FromVecV<Ret>::cast(yi);
481  }
482  }
483 
484  template<typename Ret, typename Arg>
485  Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
486  double x = ToDouble<Arg>::cast(x_);
487  if(x-1e-13>(parent->breaks)(parent->nPoly)) throw std::runtime_error("(PiecewisePolynomFunction::diff2): x out of range! x= "+fmatvec::toString(x)+" upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
488  if(x+1e-13<(parent->breaks)(0)) throw std::runtime_error("(PiecewisePolynomFunction::diff2): x out of range! x= "+fmatvec::toString(x)+" lower bound= "+fmatvec::toString((parent->breaks)(0)));
489 
490  if ((fabs(x-xSave)<macheps) && !firstCall)
491  return FromVecV<Ret>::cast(ySave);
492  else {
493  firstCall = false;
494  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
495  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
496  (parent->index)--;
497  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
498  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
499  (parent->index)++;
500  (parent->index)--;
501  }
502 
503  double dx = x - (parent->breaks)(parent->index);
504  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
505  for(int i=1;i<=((parent->order)-2);i++)
506  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
507  xSave=x;
508  ySave=yi;
509  return FromVecV<Ret>::cast(yi);
510  }
511  }
512 
513  template<typename Ret, typename Arg>
514  void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
515  xercesc::DOMElement *e;
516  fmatvec::VecV x;
517  fmatvec::MatV y;
518  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
519  if (e) {
520  setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
521  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
522  sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
523  }
524  else {
525  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
526  setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
527  }
528  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
529  if(e) {
530  std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
531  str=str.substr(1,str.length()-2);
532  if(str=="cSplinePeriodic") method=cSplinePeriodic;
533  else if(str=="cSplineNatural") method=cSplineNatural;
534  else if(str=="piecewiseLinear") method=piecewiseLinear;
535  else method=unknown;
536  }
537  }
538 
539 }
540 
541 #endif /* PPOLYNOM */
void init(Element::InitStage stage, const InitConfigSet &config)
plots time series header
Definition: piecewise_polynom_function.h:80
void setCoefficients(const std::vector< fmatvec::MatV > &coefs_u)
set polynomial coefficients
Definition: piecewise_polynom_function.h:148
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:194
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:121
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:133
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:93
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:186
InitStage
The stages of the initialization.
Definition: element.h:62
Definition: piecewise_polynom_function.h:31
void setBreaks(const std::vector< fmatvec::MatV > &coefs_u, const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:154
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:181
Definition: planar_contour.h:30
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:116
Definition: element.h:64
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:171
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:176
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:166
class for piecewise-polynomials and cubic spline interpolation
Definition: piecewise_polynom_function.h:63
RowVector< Ref, AT > trans(const Vector< Ref, AT > &x)
std::string toString(const T &value, int precision=0)

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Home