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.clear();
334  coefs.push_back(d);
335  coefs.push_back(c);
336  coefs.push_back(b);
337  coefs.push_back(a);
338  }
339 
340  template<typename Ret, typename Arg>
342  // first row
343  int i=0;
344  int N = x.size();
345  if(N<4) this->throwError("(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
346  fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
347  fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
348  double hi = x(i+1)-x(i);
349  double hii = x(i+2)-x(i+1);
350  C(i,i) = 2*hi+2*hii;
351  C(i,i+1) = hii;
352  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
353 
354  // last row
355  i = (N-3);
356  hi = x(i+1)-x(i);
357  hii = x(i+2)-x(i+1);
358  C(i,i-1) = hi;
359  C(i,i) = 2*hii + 2*hi;
360  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
361 
362  for(i=1;i<N-3;i++) {
363  hi = x(i+1)-x(i);
364  hii = x(i+2)-x(i+1);
365  C(i,i-1) = hi;
366  C(i,i) = 2*(hi+hii);
367  C(i,i+1) = hii;
368  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
369  }
370 
371  // solve C*c = rs with C tridiagonal
372  fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
373  C_rs(0,0,N-3,N-3) = C; // C_rs=[C rs] for Gauss in matrix
374  C_rs(0,N-2,N-3,N-2+y.cols()-1) = rs;
375  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
376  fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+y.cols()-1);
377  fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
378  fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
379  for(i=N-3;i>=0 ;i--) { // backward substitution
380  fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
381  sum_ciCi.init(0.);
382  for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
383  c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
384  }
385  fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
386  ctmp(1,0,N-2,y.cols()-1) = c; // c1=cN=0 natural splines c=[ 0; c; 0]
387 
388  // vector ordering of the further coefficients
389  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
390  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
391  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
392 
393  for(i=0; i<N-1; i++) {
394  hi = x(i+1)-x(i);
395  a.row(i) = y.row(i);
396  d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
397  b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
398  }
399 
400  breaks.resize(N);
401  breaks = x;
402  coefs.clear();
403  coefs.push_back(d);
404  coefs.push_back(ctmp(0,0,N-2,y.cols()-1));
405  coefs.push_back(b);
406  coefs.push_back(a);
407  }
408 
409  template<typename Ret, typename Arg>
411  int N = x.size(); // number of supporting points
412 
413  breaks.resize(N);
414  breaks = x;
415 
416  fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
417  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
418  for(int i=1;i<N;i++) {
419  m.row(i-1) = (y.row(i)-y.row(i-1))/(x(i)-x(i-1)); // slope
420  a.row(i-1) = y.row(i-1);
421  }
422  coefs.clear();
423  coefs.push_back(m);
424  coefs.push_back(a);
425  }
426 
427  template<typename Ret, typename Arg>
428  Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
429  double x = ToDouble<Arg>::cast(x_);
430  if(x-1e-13>(parent->breaks)(parent->nPoly))
431  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
432  if(x+1e-13<(parent->breaks)(0))
433  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", lower bound= "+fmatvec::toString((parent->breaks)(0)));
434 
435  if ((fabs(x-xSave)<macheps) && !firstCall)
436  return FromVecV<Ret>::cast(ySave);
437  else {
438  firstCall = false;
439  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
440  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
441  (parent->index)--;
442  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
443  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
444  (parent->index)++;
445  (parent->index)--;
446  }
447 
448  const double dx = x - (parent->breaks)(parent->index); // local coordinate
449  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
450  for(int i=1;i<=(parent->order);i++) // Horner scheme
451  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
452  xSave=x;
453  ySave=yi;
454  return FromVecV<Ret>::cast(yi);
455  }
456  }
457 
458  template<typename Ret, typename Arg>
459  Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
460  double x = ToDouble<Arg>::cast(x_);
461  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)));
462  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)));
463 
464  if ((fabs(x-xSave)<macheps) && !firstCall)
465  return FromVecV<Ret>::cast(ySave);
466  else {
467  firstCall = false;
468  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
469  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
470  (parent->index)--;
471  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
472  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
473  (parent->index)++;
474  (parent->index)--;
475  }
476 
477  double dx = x - (parent->breaks)(parent->index);
478  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
479  for(int i=1;i<parent->order;i++)
480  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
481  xSave=x;
482  ySave=yi;
483  return FromVecV<Ret>::cast(yi);
484  }
485  }
486 
487  template<typename Ret, typename Arg>
488  Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
489  double x = ToDouble<Arg>::cast(x_);
490  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)));
491  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)));
492 
493  if ((fabs(x-xSave)<macheps) && !firstCall)
494  return FromVecV<Ret>::cast(ySave);
495  else {
496  firstCall = false;
497  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
498  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
499  (parent->index)--;
500  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
501  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
502  (parent->index)++;
503  (parent->index)--;
504  }
505 
506  double dx = x - (parent->breaks)(parent->index);
507  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
508  for(int i=1;i<=((parent->order)-2);i++)
509  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
510  xSave=x;
511  ySave=yi;
512  return FromVecV<Ret>::cast(yi);
513  }
514  }
515 
516  template<typename Ret, typename Arg>
517  void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
518  xercesc::DOMElement *e;
519  fmatvec::VecV x;
520  fmatvec::MatV y;
521  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
522  if (e) {
523  setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
524  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
525  sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
526  }
527  else {
528  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
529  setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
530  }
531  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
532  if(e) {
533  std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
534  str=str.substr(1,str.length()-2);
535  if(str=="cSplinePeriodic") method=cSplinePeriodic;
536  else if(str=="cSplineNatural") method=cSplineNatural;
537  else if(str=="piecewiseLinear") method=piecewiseLinear;
538  else method=unknown;
539  }
540  }
541 
542 }
543 
544 #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