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  useBreaksAndCoefs
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!=useBreaksAndCoefs) {
84  if(y.rows() != x.size())
85  this->throwError("Dimension missmatch in size of x");
86  for(int i=1; i<x.size(); i++)
87  if(x(i) <= x(i-1))
88  this->throwError("Values of x must be strictly monotonic increasing!");
89  calculateSpline();
90  }
91  else {
92  for(int i=1; i<breaks.size(); i++)
93  if(breaks(i) <= breaks(i-1))
94  this->throwError("Values of breaks must be strictly monotonic increasing!");
95  }
96  nPoly = (coefs[0]).rows();
97  order = coefs.size()-1;
98  }
99  }
100 
101  void calculateSpline() {
102  if(method == cSplinePeriodic) calculateSplinePeriodic();
103  else if(method == cSplineNatural) calculateSplineNatural();
104  else if(method == piecewiseLinear) calculatePLinear();
105  else this->throwError("(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
106  }
107 
108  void reset() {
109  index = 0;
110  f.reset();
111  fd.reset();
112  fdd.reset();
113  coefs.clear();
114  }
115 
116  Ret operator()(const Arg &x) { return f(x); }
117  typename B::DRetDArg parDer(const Arg &x) { return fd(x); }
118  typename B::DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(arg)*ToDouble<Arg>::cast(argDir); }
119 
123  std::vector<fmatvec::MatV> getCoefficients() { return coefs; }
124 
128  fmatvec::VecV getBreaks() { return breaks; }
129 
140  void setInterpolationMethod(InterpolationMethod method_) { method = method_; }
141 
142  void setx(const fmatvec::VecV &x_) { x <<= x_; }
143  void sety(const fmatvec::MatV &y_) { y <<= y_; }
144  void setxy(const fmatvec::MatV &xy) {
145  if(xy.cols() <= 1)
146  this->throwError("Dimension missmatch in size of xy");
147  x <<= xy.col(0);
148  y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
149  }
150 
155  void setCoefficients(const std::vector<fmatvec::MatV> &coefs_u) { coefs = coefs_u; }
156 
161  void setBreaks(const fmatvec::VecV &breaks_u) { breaks <<= breaks_u; }
162 
167  virtual void initializeUsingXML(xercesc::DOMElement *element);
168 
169  protected:
173  std::vector<fmatvec::MatV> coefs;
174 
178  fmatvec::VecV breaks;
179 
183  int nPoly;
184 
188  int order;
189 
193  int index;
194 
195  fmatvec::VecV x;
196  fmatvec::MatV y;
197 
201  InterpolationMethod method;
202 
208  void calculateSplinePeriodic();
209 
215  void calculateSplineNatural();
216 
217  /*
218  * \brief calculation of piecewise linear interpolation
219  * \param interpolated arguments
220  * \param interpolated function values
221  *
222  * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
223  */
224  void calculatePLinear();
225 
229  class ZerothDerivative {
230  public:
231  ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
232 
233  void reset() { firstCall = true; }
234 
235  Ret operator()(const Arg &x);
236 
237  private:
238  PiecewisePolynomFunction<Ret(Arg)> *parent;
239  double xSave;
240  fmatvec::VecV ySave;
241  bool firstCall;
242  };
243 
247  class FirstDerivative {
248  public:
249  FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
250 
251  void reset() { firstCall = true; }
252 
253  Ret operator()(const Arg& x);
254 
255  private:
256  PiecewisePolynomFunction<Ret(Arg)> *parent;
257  double xSave;
258  fmatvec::VecV ySave;
259  bool firstCall;
260  };
261 
265  class SecondDerivative {
266  public:
267  SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
268 
269  void reset() { firstCall = true; }
270 
271  Ret operator()(const Arg& x);
272 
273  private:
274  PiecewisePolynomFunction<Ret(Arg)> *parent;
275  double xSave;
276  fmatvec::VecV ySave;
277  bool firstCall;
278  };
279 
280  private:
281  ZerothDerivative f;
282  FirstDerivative fd;
283  SecondDerivative fdd;
284  };
285 
286  template<typename Ret, typename Arg>
287  void PiecewisePolynomFunction<Ret(Arg)>::calculateSplinePeriodic() {
288  double hi, hii;
289  int N = x.size();
290  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)");
291  fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
292  fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
293 
294  // Matrix C and vector rs C*c=rs
295  for(int i=0; i<N-3;i++) {
296  hi = x(i+1) - x(i);
297  hii = x(i+2)-x(i+1);
298  C(i,i) = hi;
299  C(i,i+1) = 2*(hi+hii);
300  C(i,i+2) = hii;
301  rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
302  }
303 
304  // last but one row
305  hi = x(N-2)-x(N-3);
306  hii = x(N-1)-x(N-2);
307  C(N-3,N-3) = hi;
308  C(N-3,N-2)= 2*(hi+hii);
309  C(N-3,0)= hii;
310  rs.set(N-3, 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi));
311 
312  // last row
313  double h1 = x(1)-x(0);
314  double hN_1 = x(N-1)-x(N-2);
315  C(N-2,0) = 2*(h1+hN_1);
316  C(N-2,1) = h1;
317  C(N-2,N-2)= hN_1;
318  rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
319 
320  // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
321  fmatvec::Mat c = slvLU(C,rs);
322  fmatvec::Mat ctmp(N,y.cols());
323  ctmp.set(N-1, c.row(0)); // CN = c1
324  ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
325 
326  // vector ordering of the further coefficients
327  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
328  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
329  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
330 
331  for(int i=0; i<N-1; i++) {
332  hi = x(i+1)-x(i);
333  a.set(i, y.row(i));
334  d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
335  b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
336  }
337 
338  breaks.resize(N);
339  breaks = x;
340  coefs.clear();
341  coefs.push_back(d);
342  coefs.push_back(c);
343  coefs.push_back(b);
344  coefs.push_back(a);
345  }
346 
347  template<typename Ret, typename Arg>
348  void PiecewisePolynomFunction<Ret(Arg)>::calculateSplineNatural() {
349  // first row
350  int i=0;
351  int N = x.size();
352  if(N<4) this->throwError("(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
353  fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
354  fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
355  double hi = x(i+1)-x(i);
356  double hii = x(i+2)-x(i+1);
357  C(i,i) = 2*hi+2*hii;
358  C(i,i+1) = hii;
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  // last row
362  i = (N-3);
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*hii + 2*hi;
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  for(i=1;i<N-3;i++) {
370  hi = x(i+1)-x(i);
371  hii = x(i+2)-x(i+1);
372  C(i,i-1) = hi;
373  C(i,i) = 2*(hi+hii);
374  C(i,i+1) = hii;
375  rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
376  }
377 
378  // solve C*c = rs with C tridiagonal
379  fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
380  C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3), C); // C_rs=[C rs] for Gauss in matrix
381  C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1), rs);
382  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
383  fmatvec::Mat rs1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1));
384  fmatvec::Mat C1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3));
385  fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
386  for(i=N-3;i>=0 ;i--) { // backward substitution
387  fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
388  sum_ciCi.init(0.);
389  for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
390  c.set(i, (rs1.row(i) - sum_ciCi)/C1(i,i));
391  }
392  fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
393  ctmp.set(fmatvec::RangeV(1,N-2),fmatvec::RangeV(0,y.cols()-1), c); // c1=cN=0 natural splines c=[ 0; c; 0]
394 
395  // vector ordering of the further coefficients
396  fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
397  fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
398  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
399 
400  for(i=0; i<N-1; i++) {
401  hi = x(i+1)-x(i);
402  a.set(i, y.row(i));
403  d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
404  b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
405  }
406 
407  breaks.resize(N);
408  breaks = x;
409  coefs.clear();
410  coefs.push_back(d);
411  coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
412  coefs.push_back(b);
413  coefs.push_back(a);
414  }
415 
416  template<typename Ret, typename Arg>
417  void PiecewisePolynomFunction<Ret(Arg)>::calculatePLinear() {
418  int N = x.size(); // number of supporting points
419 
420  breaks.resize(N);
421  breaks = x;
422 
423  fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
424  fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
425  for(int i=1;i<N;i++) {
426  m.set(i-1, (y.row(i)-y.row(i-1))/(x(i)-x(i-1))); // slope
427  a.set(i-1, y.row(i-1));
428  }
429  coefs.clear();
430  coefs.push_back(m);
431  coefs.push_back(a);
432  }
433 
434  template<typename Ret, typename Arg>
435  Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
436  double x = ToDouble<Arg>::cast(x_);
437  if(x-1e-13>(parent->breaks)(parent->nPoly))
438  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
439  if(x+1e-13<(parent->breaks)(0))
440  throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", lower bound= "+fmatvec::toString((parent->breaks)(0)));
441 
442  if ((fabs(x-xSave)<macheps) && !firstCall)
443  return FromVecV<Ret>::cast(ySave);
444  else {
445  firstCall = false;
446  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
447  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
448  (parent->index)--;
449  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
450  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
451  (parent->index)++;
452  (parent->index)--;
453  }
454 
455  const double dx = x - (parent->breaks)(parent->index); // local coordinate
456  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
457  for(int i=1;i<=(parent->order);i++) // Horner scheme
458  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
459  xSave=x;
460  ySave <<= yi;
461  return FromVecV<Ret>::cast(yi);
462  }
463  }
464 
465  template<typename Ret, typename Arg>
466  Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
467  double x = ToDouble<Arg>::cast(x_);
468  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)));
469  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)));
470 
471  if ((fabs(x-xSave)<macheps) && !firstCall)
472  return FromVecV<Ret>::cast(ySave);
473  else {
474  firstCall = false;
475  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
476  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
477  (parent->index)--;
478  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
479  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
480  (parent->index)++;
481  (parent->index)--;
482  }
483 
484  double dx = x - (parent->breaks)(parent->index);
485  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
486  for(int i=1;i<parent->order;i++)
487  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
488  xSave=x;
489  ySave <<= yi;
490  return FromVecV<Ret>::cast(yi);
491  }
492  }
493 
494  template<typename Ret, typename Arg>
495  Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
496  double x = ToDouble<Arg>::cast(x_);
497  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)));
498  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)));
499 
500  if ((fabs(x-xSave)<macheps) && !firstCall)
501  return FromVecV<Ret>::cast(ySave);
502  else {
503  firstCall = false;
504  if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
505  while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
506  (parent->index)--;
507  else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
508  while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
509  (parent->index)++;
510  (parent->index)--;
511  }
512 
513  double dx = x - (parent->breaks)(parent->index);
514  fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
515  for(int i=1;i<=((parent->order)-2);i++)
516  yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
517  xSave=x;
518  ySave <<= yi;
519  return FromVecV<Ret>::cast(yi);
520  }
521  }
522 
523  template<typename Ret, typename Arg>
524  void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
525  xercesc::DOMElement *e;
526  fmatvec::VecV x;
527  fmatvec::MatV y;
528  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
529  if (e) {
530  setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
531  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
532  sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
533  }
534  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
535  if (e) {
536  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
537  setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
538  }
539  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
540  if(e) {
541  std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
542  str=str.substr(1,str.length()-2);
543  if(str=="cSplinePeriodic") method=cSplinePeriodic;
544  else if(str=="cSplineNatural") method=cSplineNatural;
545  else if(str=="piecewiseLinear") method=piecewiseLinear;
546  else method=useBreaksAndCoefs;
547  }
548 
549  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"breaks");
550  if(e) {
551  setBreaks(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
552  e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"coefficients");
553  auto c=MBXMLUtils::E(e)->getText<fmatvec::Mat>();
554  std::vector<fmatvec::MatV> coefs;
555  coefs.reserve(c.cols());
556  for(int i=0; i<c.cols(); ++i)
557  coefs.emplace_back(c.col(i));
558  setCoefficients(coefs);
559  method=useBreaksAndCoefs;
560  }
561  }
562 
563 }
564 
565 #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:155
void setBreaks(const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:161
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:201
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:128
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:140
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:193
InitStage
The stages of the initialization.
Definition: element.h:64
Definition: piecewise_polynom_function.h:31
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:188
Definition: contact_kinematics.h:32
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:123
Definition: element.h:66
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:178
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:183
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:173
namespace MBSim
Definition: bilateral_constraint.cc:30