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/element.h"
26 #include "mbsim/utils/utils.h"
27 #include "mbsim/utils/eps.h"
28 #include "mbsim/mbsim_event.h"
29 
30 namespace MBSim {
31 
32  template<typename Sig> class PiecewisePolynomFunction;
33 
63  template<typename Ret, typename Arg>
64  class PiecewisePolynomFunction<Ret(Arg)> : public Function<Ret(Arg)> {
66 
67  public:
68  enum InterpolationMethod {
69  cSplinePeriodic,
70  cSplineNatural,
71  piecewiseLinear
72  };
73 
74  PiecewisePolynomFunction() : index(0), method(cSplineNatural), f(this), fd(this), fdd(this) { }
75 
76  int getArgSize() const { return 1; }
77 
78  void init(Element::InitStage stage) {
80  if(stage==Element::preInit) {
81  if(y.rows() != x.size())
82  THROW_MBSIMERROR("Dimension missmatch in size of x");
83  if(x.size()) calculateSpline();
84  nPoly = (coefs[0]).rows();
85  order = coefs.size()-1;
86  }
87  }
88 
89  void calculateSpline() {
90  if(method == cSplinePeriodic) calculateSplinePeriodic();
91  else if(method == cSplineNatural) calculateSplineNatural();
92  else if(method == piecewiseLinear) calculatePLinear();
93  else THROW_MBSIMERROR("(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
94  }
95 
96  void reset() {
97  index = 0;
98  f.reset();
99  fd.reset();
100  fdd.reset();
101  coefs.clear();
102  }
103 
104  Ret operator()(const Arg &x) { return f(x); }
105  typename B::DRetDArg parDer(const Arg &x) { return fd(x); }
106  Ret parDerParDer(const double &x) { return fdd(x); }
107  typename B::DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(arg)*ToDouble<Arg>::cast(argDir); }
108 
112  std::vector<fmatvec::MatV> getCoefficients() { return coefs; }
113 
117  fmatvec::VecV getBreaks() { return breaks; }
118 
129  void setInterpolationMethod(InterpolationMethod method_) { method = method_; }
130 
131  void setx(const fmatvec::VecV &x_) { x = x_; }
132  void sety(const fmatvec::MatV &y_) { y = y_; }
133  void setxy(const fmatvec::MatV &xy) {
134  if(xy.cols() <= 1)
135  THROW_MBSIMERROR("Dimension missmatch in size of xy");
136  x = xy.col(0);
137  y = xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
138  }
139 
144  void setCoefficients(const std::vector<fmatvec::MatV> &coefs_u) { coefs = coefs_u; }
145 
150  void setBreaks(const std::vector<fmatvec::MatV> &coefs_u, const fmatvec::VecV &breaks_u) { breaks = breaks_u; }
151 
156  virtual void initializeUsingXML(xercesc::DOMElement *element);
157 
158  protected:
162  std::vector<fmatvec::MatV> coefs;
163 
167  fmatvec::VecV breaks;
168 
172  int nPoly;
173 
177  int order;
178 
182  int index;
183 
184  fmatvec::VecV x;
185  fmatvec::MatV y;
186 
190  InterpolationMethod method;
191 
197  void calculateSplinePeriodic();
198 
204  void calculateSplineNatural();
205 
206  /*
207  * \brief calculation of piecewise linear interpolation
208  * \param interpolated arguments
209  * \param interpolated function values
210  *
211  * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
212  */
213  void calculatePLinear();
214 
218  class ZerothDerivative {
219  public:
220  ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
221 
222  void reset() { firstCall = true; }
223 
224  Ret operator()(const Arg &x);
225 
226  private:
228  double xSave;
229  fmatvec::VecV ySave;
230  bool firstCall;
231  };
232 
236  class FirstDerivative {
237  public:
238  FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
239 
240  void reset() { firstCall = true; }
241 
242  Ret operator()(const Arg& x);
243 
244  private:
246  double xSave;
247  fmatvec::VecV ySave;
248  bool firstCall;
249  };
250 
254  class SecondDerivative {
255  public:
256  SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
257 
258  void reset() { firstCall = true; }
259 
260  Ret operator()(const Arg& x);
261 
262  private:
264  double xSave;
265  fmatvec::VecV ySave;
266  bool firstCall;
267  };
268 
269  private:
270  ZerothDerivative f;
271  FirstDerivative fd;
272  SecondDerivative fdd;
273  };
274 
275  template<typename Ret, typename Arg>
277  double hi, hii;
278  int N = x.size();
279  if(nrm2(y.row(0)-y.row(y.rows()-1))>epsroot()) THROW_MBSIMERROR("(PiecewisePolynomFunction::calculateSplinePeriodic): f(0)= "+numtostr(y.row(0))+"!="+numtostr(y.row(y.rows()-1))+" =f(end)");
280  fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
281  fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
282 
283  // Matrix C and vector rs C*c=rs
284  for(int i=0; i<N-3;i++) {
285  hi = x(i+1) - x(i);
286  hii = x(i+2)-x(i+1);
287  C(i,i) = hi;
288  C(i,i+1) = 2*(hi+hii);
289  C(i,i+2) = hii;
290  rs.row(i) = 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi);
291  }
292 
293  // last but one row
294  hi = x(N-2)-x(N-3);
295  hii = x(N-1)-x(N-2);
296  C(N-3,N-3) = hi;
297  C(N-3,N-2)= 2*(hi+hii);
298  C(N-3,0)= hii;
299  rs.row(N-3) = 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi);
300 
301  // last row
302  double h1 = x(1)-x(0);
303  double hN_1 = x(N-1)-x(N-2);
304  C(N-2,0) = 2*(h1+hN_1);
305  C(N-2,1) = h1;
306  C(N-2,N-2)= hN_1;
307  rs.row(N-2) = 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1);
308 
309  // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
310  fmatvec::Mat c = slvLU(C,rs);
311  fmatvec::Mat ctmp(N,y.cols());
312  ctmp.row(N-1) = c.row(0); // CN = c1
313  ctmp(0,0,N-2,y.cols()-1)= c;
314 
315  // vector ordering of the further coefficients
316  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
317  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
318  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
319 
320  for(int i=0; i<N-1; i++) {
321  hi = x(i+1)-x(i);
322  a.row(i) = y.row(i);
323  d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
324  b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
325  }
326 
327  breaks.resize(N);
328  breaks = x;
329  coefs.push_back(d);
330  coefs.push_back(c);
331  coefs.push_back(b);
332  coefs.push_back(a);
333  }
334 
335  template<typename Ret, typename Arg>
337  // first row
338  int i=0;
339  int N = x.size();
340  fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
341  fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
342  double hi = x(i+1)-x(i);
343  double hii = x(i+2)-x(i+1);
344  C(i,i) = 2*hi+2*hii;
345  C(i,i+1) = hii;
346  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
347 
348  // last row
349  i = (N-3);
350  hi = x(i+1)-x(i);
351  hii = x(i+2)-x(i+1);
352  C(i,i-1) = hi;
353  C(i,i) = 2*hii + 2*hi;
354  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
355 
356  for(i=1;i<N-3;i++) {
357  hi = x(i+1)-x(i);
358  hii = x(i+2)-x(i+1);
359  C(i,i-1) = hi;
360  C(i,i) = 2*(hi+hii);
361  C(i,i+1) = hii;
362  rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
363  }
364 
365  // solve C*c = rs with C tridiagonal
366  fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
367  C_rs(0,0,N-3,N-3) = C; // C_rs=[C rs] for Gauss in matrix
368  C_rs(0,N-2,N-3,N-2+y.cols()-1) = rs;
369  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
370  fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+y.cols()-1);
371  fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
372  fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
373  for(i=N-3;i>=0 ;i--) { // backward substitution
374  fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
375  sum_ciCi.init(0.);
376  for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
377  c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
378  }
379  fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
380  ctmp(1,0,N-2,y.cols()-1) = c; // c1=cN=0 natural splines c=[ 0; c; 0]
381 
382  // vector ordering of the further coefficients
383  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
384  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
385  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
386 
387  for(i=0; i<N-1; i++) {
388  hi = x(i+1)-x(i);
389  a.row(i) = y.row(i);
390  d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
391  b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
392  }
393 
394  breaks.resize(N);
395  breaks = x;
396  coefs.push_back(d);
397  coefs.push_back(ctmp(0,0,N-2,y.cols()-1));
398  coefs.push_back(b);
399  coefs.push_back(a);
400  }
401 
402  template<typename Ret, typename Arg>
404  int N = x.size(); // number of supporting points
405 
406  breaks.resize(N);
407  breaks = x;
408 
409  fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
410  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
411  for(int i=1;i<N;i++) {
412  m.row(i-1) = (y.row(i)-y.row(i-1))/(x(i)-x(i-1)); // slope
413  a.row(i-1) = y.row(i-1);
414  }
415  coefs.push_back(m);
416  coefs.push_back(a);
417  }
418 
419  template<typename Ret, typename Arg>
420  Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
421  double x = ToDouble<Arg>::cast(x_);
422  if(x>(parent->breaks)(parent->nPoly))
423  throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
424  if(x<(parent->breaks)(0))
425  throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", lower bound= "+numtostr((parent->breaks)(0)));
426 
427  if ((fabs(x-xSave)<macheps()) && !firstCall)
428  return FromVecV<Ret>::cast(ySave);
429  else {
430  firstCall = false;
431  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
432  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
433  (parent->index)--;
434  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
435  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
436  (parent->index)++;
437  (parent->index)--;
438  }
439 
440  const double dx = x - (parent->breaks)(parent->index); // local coordinate
441  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
442  for(int i=1;i<=(parent->order);i++) // Horner scheme
443  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
444  xSave=x;
445  ySave=yi;
446  return FromVecV<Ret>::cast(yi);
447  }
448  }
449 
450  template<typename Ret, typename Arg>
451  Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
452  double x = ToDouble<Arg>::cast(x_);
453  if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
454  if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
455 
456  if ((fabs(x-xSave)<macheps()) && !firstCall)
457  return FromVecV<Ret>::cast(ySave);
458  else {
459  firstCall = false;
460  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
461  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
462  (parent->index)--;
463  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
464  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
465  (parent->index)++;
466  (parent->index)--;
467  }
468 
469  double dx = x - (parent->breaks)(parent->index);
470  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
471  for(int i=1;i<parent->order;i++)
472  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
473  xSave=x;
474  ySave=yi;
475  return FromVecV<Ret>::cast(yi);
476  }
477  }
478 
479  template<typename Ret, typename Arg>
480  Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
481  double x = ToDouble<Arg>::cast(x_);
482  if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
483  if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
484 
485  if ((fabs(x-xSave)<macheps()) && !firstCall)
486  return FromVecV<Ret>::cast(ySave);
487  else {
488  firstCall = false;
489  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
490  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
491  (parent->index)--;
492  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
493  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
494  (parent->index)++;
495  (parent->index)--;
496  }
497 
498  double dx = x - (parent->breaks)(parent->index);
499  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
500  for(int i=1;i<=((parent->order)-2);i++)
501  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
502  xSave=x;
503  ySave=yi;
504  return FromVecV<Ret>::cast(yi);
505  }
506  }
507 
508  template<typename Ret, typename Arg>
509  void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
510  xercesc::DOMElement *e;
511  fmatvec::VecV x;
512  fmatvec::MatV y;
513  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
514  if (e) {
515  setx(Element::getVec(e));
516  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
517  sety(Element::getMat(e, x.size(), 0));
518  }
519  else {
520  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
521  setxy(Element::getMat(e));
522  }
523  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
524  if(e) {
525  std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
526  str=str.substr(1,str.length()-2);
527  if(str=="cSplinePeriodic") method=cSplinePeriodic;
528  else if(str=="cSplineNatural") method=cSplineNatural;
529  else if(str=="piecewiseLinear") method=piecewiseLinear;
530  }
531  }
532 
533 }
534 
535 #endif /* PPOLYNOM */
void setCoefficients(const std::vector< fmatvec::MatV > &coefs_u)
set polynomial coefficients
Definition: piecewise_polynom_function.h:144
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:190
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:117
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:129
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:48
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:182
InitStage
The stages of the initialization.
Definition: element.h:97
Definition: piecewise_polynom_function.h:32
void setBreaks(const std::vector< fmatvec::MatV > &coefs_u, const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:150
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:177
Definition: planar_contour.h:31
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:112
Definition: element.h:100
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:167
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:172
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:162
class for piecewise-polynomials and cubic spline interpolation
Definition: piecewise_polynom_function.h:64
RowVector< Ref, AT > trans(const Vector< Ref, AT > &x)
void init(Element::InitStage stage)
plots time series header
Definition: piecewise_polynom_function.h:78

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML