mbsim  4.0.0
MBSim Kernel
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)> {
64  using B = fmatvec::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  typename B::DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(arg)*ToDouble<Arg>::cast(argDir); }
111 
115  std::vector<fmatvec::MatV> getCoefficients() { return coefs; }
116 
120  fmatvec::VecV getBreaks() { return breaks; }
121 
132  void setInterpolationMethod(InterpolationMethod method_) { method = method_; }
133 
134  void setx(const fmatvec::VecV &x_) { x <<= x_; }
135  void sety(const fmatvec::MatV &y_) { y <<= y_; }
136  void setxy(const fmatvec::MatV &xy) {
137  if(xy.cols() <= 1)
138  this->throwError("Dimension missmatch in size of xy");
139  x <<= xy.col(0);
140  y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
141  }
142 
147  void setCoefficients(const std::vector<fmatvec::MatV> &coefs_u) { coefs = coefs_u; }
148 
153  void setBreaks(const std::vector<fmatvec::MatV> &coefs_u, const fmatvec::VecV &breaks_u) { breaks = breaks_u; }
154 
159  virtual void initializeUsingXML(xercesc::DOMElement *element);
160 
161  protected:
165  std::vector<fmatvec::MatV> coefs;
166 
170  fmatvec::VecV breaks;
171 
175  int nPoly;
176 
180  int order;
181 
185  int index;
186 
187  fmatvec::VecV x;
188  fmatvec::MatV y;
189 
193  InterpolationMethod method;
194 
200  void calculateSplinePeriodic();
201 
207  void calculateSplineNatural();
208 
209  /*
210  * \brief calculation of piecewise linear interpolation
211  * \param interpolated arguments
212  * \param interpolated function values
213  *
214  * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
215  */
216  void calculatePLinear();
217 
221  class ZerothDerivative {
222  public:
223  ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
224 
225  void reset() { firstCall = true; }
226 
227  Ret operator()(const Arg &x);
228 
229  private:
230  PiecewisePolynomFunction<Ret(Arg)> *parent;
231  double xSave;
232  fmatvec::VecV ySave;
233  bool firstCall;
234  };
235 
239  class FirstDerivative {
240  public:
241  FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
242 
243  void reset() { firstCall = true; }
244 
245  Ret operator()(const Arg& x);
246 
247  private:
248  PiecewisePolynomFunction<Ret(Arg)> *parent;
249  double xSave;
250  fmatvec::VecV ySave;
251  bool firstCall;
252  };
253 
257  class SecondDerivative {
258  public:
259  SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
260 
261  void reset() { firstCall = true; }
262 
263  Ret operator()(const Arg& x);
264 
265  private:
266  PiecewisePolynomFunction<Ret(Arg)> *parent;
267  double xSave;
268  fmatvec::VecV ySave;
269  bool firstCall;
270  };
271 
272  private:
273  ZerothDerivative f;
274  FirstDerivative fd;
275  SecondDerivative fdd;
276  };
277 
278  template<typename Ret, typename Arg>
279  void PiecewisePolynomFunction<Ret(Arg)>::calculateSplinePeriodic() {
280  double hi, hii;
281  int N = x.size();
282  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)");
283  fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
284  fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
285 
286  // Matrix C and vector rs C*c=rs
287  for(int i=0; i<N-3;i++) {
288  hi = x(i+1) - x(i);
289  hii = x(i+2)-x(i+1);
290  C(i,i) = hi;
291  C(i,i+1) = 2*(hi+hii);
292  C(i,i+2) = hii;
293  rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
294  }
295 
296  // last but one row
297  hi = x(N-2)-x(N-3);
298  hii = x(N-1)-x(N-2);
299  C(N-3,N-3) = hi;
300  C(N-3,N-2)= 2*(hi+hii);
301  C(N-3,0)= hii;
302  rs.set(N-3, 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi));
303 
304  // last row
305  double h1 = x(1)-x(0);
306  double hN_1 = x(N-1)-x(N-2);
307  C(N-2,0) = 2*(h1+hN_1);
308  C(N-2,1) = h1;
309  C(N-2,N-2)= hN_1;
310  rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
311 
312  // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
313  fmatvec::Mat c = slvLU(C,rs);
314  fmatvec::Mat ctmp(N,y.cols());
315  ctmp.set(N-1, c.row(0)); // CN = c1
316  ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
317 
318  // vector ordering of the further coefficients
319  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
320  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
321  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
322 
323  for(int i=0; i<N-1; i++) {
324  hi = x(i+1)-x(i);
325  a.set(i, y.row(i));
326  d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
327  b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
328  }
329 
330  breaks.resize(N);
331  breaks = x;
332  coefs.clear();
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>
340  void PiecewisePolynomFunction<Ret(Arg)>::calculateSplineNatural() {
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.set(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.set(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.set(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.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3), C); // C_rs=[C rs] for Gauss in matrix
373  C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1), rs);
374  for(i=1; i<N-2; i++) C_rs.set(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(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1));
376  fmatvec::Mat C1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,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.set(i, (rs1.row(i) - sum_ciCi)/C1(i,i));
383  }
384  fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
385  ctmp.set(fmatvec::RangeV(1,N-2),fmatvec::RangeV(0,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.set(i, y.row(i));
395  d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
396  b.set(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.clear();
402  coefs.push_back(d);
403  coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
404  coefs.push_back(b);
405  coefs.push_back(a);
406  }
407 
408  template<typename Ret, typename Arg>
409  void PiecewisePolynomFunction<Ret(Arg)>::calculatePLinear() {
410  int N = x.size(); // number of supporting points
411 
412  breaks.resize(N);
413  breaks = x;
414 
415  fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
416  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
417  for(int i=1;i<N;i++) {
418  m.set(i-1, (y.row(i)-y.row(i-1))/(x(i)-x(i-1))); // slope
419  a.set(i-1, y.row(i-1));
420  }
421  coefs.clear();
422  coefs.push_back(m);
423  coefs.push_back(a);
424  }
425 
426  template<typename Ret, typename Arg>
427  Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
428  double x = ToDouble<Arg>::cast(x_);
429  if(x-1e-13>(parent->breaks)(parent->nPoly))
430  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
431  if(x+1e-13<(parent->breaks)(0))
432  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", lower bound= "+fmatvec::toString((parent->breaks)(0)));
433 
434  if ((fabs(x-xSave)<macheps) && !firstCall)
435  return FromVecV<Ret>::cast(ySave);
436  else {
437  firstCall = false;
438  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
439  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
440  (parent->index)--;
441  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
442  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
443  (parent->index)++;
444  (parent->index)--;
445  }
446 
447  const double dx = x - (parent->breaks)(parent->index); // local coordinate
448  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
449  for(int i=1;i<=(parent->order);i++) // Horner scheme
450  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
451  xSave=x;
452  ySave <<= yi;
453  return FromVecV<Ret>::cast(yi);
454  }
455  }
456 
457  template<typename Ret, typename Arg>
458  Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
459  double x = ToDouble<Arg>::cast(x_);
460  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)));
461  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)));
462 
463  if ((fabs(x-xSave)<macheps) && !firstCall)
464  return FromVecV<Ret>::cast(ySave);
465  else {
466  firstCall = false;
467  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
468  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
469  (parent->index)--;
470  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
471  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
472  (parent->index)++;
473  (parent->index)--;
474  }
475 
476  double dx = x - (parent->breaks)(parent->index);
477  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
478  for(int i=1;i<parent->order;i++)
479  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
480  xSave=x;
481  ySave <<= yi;
482  return FromVecV<Ret>::cast(yi);
483  }
484  }
485 
486  template<typename Ret, typename Arg>
487  Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
488  double x = ToDouble<Arg>::cast(x_);
489  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)));
490  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)));
491 
492  if ((fabs(x-xSave)<macheps) && !firstCall)
493  return FromVecV<Ret>::cast(ySave);
494  else {
495  firstCall = false;
496  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
497  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
498  (parent->index)--;
499  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
500  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
501  (parent->index)++;
502  (parent->index)--;
503  }
504 
505  double dx = x - (parent->breaks)(parent->index);
506  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
507  for(int i=1;i<=((parent->order)-2);i++)
508  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
509  xSave=x;
510  ySave <<= yi;
511  return FromVecV<Ret>::cast(yi);
512  }
513  }
514 
515  template<typename Ret, typename Arg>
516  void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
517  xercesc::DOMElement *e;
518  fmatvec::VecV x;
519  fmatvec::MatV y;
520  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
521  if (e) {
522  setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
523  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
524  sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
525  }
526  else {
527  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
528  setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
529  }
530  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
531  if(e) {
532  std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
533  str=str.substr(1,str.length()-2);
534  if(str=="cSplinePeriodic") method=cSplinePeriodic;
535  else if(str=="cSplineNatural") method=cSplineNatural;
536  else if(str=="piecewiseLinear") method=piecewiseLinear;
537  else method=unknown;
538  }
539  }
540 
541 }
542 
543 #endif /* PPOLYNOM */
RowVector< Row, AT > trans(const Vector< Row, AT > &x)
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:147
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:193
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:120
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:132
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:185
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:153
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:180
Definition: planar_contour.h:30
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:115
Definition: element.h:64
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:170
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:175
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:165
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
namespace MBSim
Definition: eigenanalyzer.cc:47