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 enum ExtrapolationMethod {
75 error,
76 continuePolynom,
77 linear,
78 };
79
80 PiecewisePolynomFunction() : index(0), interpolationMethod(cSplineNatural), extrapolationMethod(error), f(this), fd(this), fdd(this) { }
81
82 int getArgSize() const { return 1; }
83
84 std::pair<int, int> getRetSize() const { return std::make_pair(coefs[0].cols(),1); }
85
86 void init(Element::InitStage stage, const InitConfigSet &config) {
87 Function<Ret(Arg)>::init(stage, config);
88 if(stage==Element::preInit) {
89 if(interpolationMethod!=useBreaksAndCoefs) {
90 if(y.rows() != x.size())
91 this->throwError("Dimension missmatch in size of x");
92 for(int i=1; i<x.size(); i++)
93 if(x(i) <= x(i-1))
94 this->throwError("Values of x must be strictly monotonic increasing!");
95 calculateSpline();
96 }
97 else {
98 for(int i=1; i<breaks.size(); i++)
99 if(breaks(i) < breaks(i-1))
100 this->throwError("Values of breaks must be monotonic increasing!");
101 for(int j=0; j<static_cast<int>(coefs.size()); j++) {
102 if(coefs[j].rows()!=breaks.size()-1)
103 this->throwError("Dimension missmatch in size of breaks and rows of coefficients!");
104 if(coefs[0].cols()!=coefs[j].cols())
105 this->throwError("Dimension missmatch in columns of coefficients!");
106 }
107
108 // remove duplicate entries from breaks
109 std::set<int> removeIdx;
110 for(int i=0; i<breaks.size()-1; ++i)
111 if(breaks(i)==breaks(i+1))
112 removeIdx.emplace(i);
113 if(!removeIdx.empty()) {
114 auto breaksOld(std::move(breaks));
115 auto coefsOld(std::move(coefs));
116 breaks.resize(breaksOld.size()-removeIdx.size());
117 coefs.resize(coefsOld.size());
118 for(auto &c : coefs)
119 c.resize(coefsOld[0].rows()-removeIdx.size(),coefsOld[0].cols());
120 int ii=0, i=0;
121 for(i=0; i<breaksOld.size()-1; ++i) {
122 if(removeIdx.find(i)!=removeIdx.end())
123 continue;
124 breaks(ii)=breaksOld(i);
125 for(int o=0; o<static_cast<int>(coefs.size()); o++)
126 coefs[o].set(ii,coefsOld[o].row(i));
127 ++ii;
128 }
129 breaks(ii)=breaksOld(i);
130 }
131
132 }
133 nPoly = (coefs[0]).rows();
134 order = coefs.size()-1;
135 }
136 }
137
138 void calculateSpline() {
139 if(interpolationMethod == cSplinePeriodic) calculateSplinePeriodic();
140 else if(interpolationMethod == cSplineNatural) calculateSplineNatural();
141 else if(interpolationMethod == piecewiseLinear) calculatePLinear();
142 else this->throwError("(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
143 }
144
145 void reset() {
146 index = 0;
147 f.reset();
148 fd.reset();
149 fdd.reset();
150 coefs.clear();
151 }
152
153 Ret operator()(const Arg &x) { return f(x); }
154 typename B::DRetDArg parDer(const Arg &x) { return fd(x); }
155 typename B::DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(arg)*ToDouble<Arg>::cast(argDir); }
156
160 std::vector<fmatvec::MatV> getCoefficients() { return coefs; }
161
165 fmatvec::VecV getBreaks() { return breaks; }
166
177 void setInterpolationMethod(InterpolationMethod method_) { interpolationMethod = method_; }
178
179 void setExtrapolationMethod(ExtrapolationMethod method_) { extrapolationMethod = method_; }
180
181 void setx(const fmatvec::VecV &x_) { x <<= x_; }
182 void sety(const fmatvec::MatV &y_) { y <<= y_; }
183 void setxy(const fmatvec::MatV &xy) {
184 if(xy.cols() <= 1)
185 this->throwError("Dimension missmatch in size of xy");
186 x <<= xy.col(0);
187 y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
188 }
189
194 void setCoefficients(const std::vector<fmatvec::MatV> &coefs_u) { coefs = coefs_u; }
195
200 void setBreaks(const fmatvec::VecV &breaks_u) { breaks <<= breaks_u; }
201
206 virtual void initializeUsingXML(xercesc::DOMElement *element);
207
208 protected:
212 std::vector<fmatvec::MatV> coefs;
213
217 fmatvec::VecV breaks;
218
222 int nPoly;
223
227 int order;
228
232 int index;
233
234 fmatvec::VecV x;
235 fmatvec::MatV y;
236
240 InterpolationMethod interpolationMethod;
241
242 ExtrapolationMethod extrapolationMethod;
243
249 void calculateSplinePeriodic();
250
256 void calculateSplineNatural();
257
258 /*
259 * \brief calculation of piecewise linear interpolation
260 * \param interpolated arguments
261 * \param interpolated function values
262 *
263 * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
264 */
265 void calculatePLinear();
266
270 class ZerothDerivative {
271 public:
272 ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
273
274 void reset() { firstCall = true; }
275
276 Ret operator()(const Arg &x);
277
278 private:
279 PiecewisePolynomFunction<Ret(Arg)> *parent;
280 double xSave;
281 fmatvec::VecV ySave;
282 bool firstCall;
283 };
284
288 class FirstDerivative {
289 public:
290 FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
291
292 void reset() { firstCall = true; }
293
294 Ret operator()(const Arg& x);
295
296 private:
297 PiecewisePolynomFunction<Ret(Arg)> *parent;
298 double xSave;
299 fmatvec::VecV ySave;
300 bool firstCall;
301 };
302
306 class SecondDerivative {
307 public:
308 SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
309
310 void reset() { firstCall = true; }
311
312 Ret operator()(const Arg& x);
313
314 private:
315 PiecewisePolynomFunction<Ret(Arg)> *parent;
316 double xSave;
317 fmatvec::VecV ySave;
318 bool firstCall;
319 };
320
321 private:
322 ZerothDerivative f;
323 FirstDerivative fd;
324 SecondDerivative fdd;
325 };
326
327 template<typename Ret, typename Arg>
328 void PiecewisePolynomFunction<Ret(Arg)>::calculateSplinePeriodic() {
329 double hi, hii;
330 int N = x.size();
331 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)");
332 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
333 fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
334
335 // Matrix C and vector rs C*c=rs
336 for(int i=0; i<N-3;i++) {
337 hi = x(i+1) - x(i);
338 hii = x(i+2)-x(i+1);
339 C(i,i) = hi;
340 C(i,i+1) = 2*(hi+hii);
341 C(i,i+2) = hii;
342 rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
343 }
344
345 // last but one row
346 hi = x(N-2)-x(N-3);
347 hii = x(N-1)-x(N-2);
348 C(N-3,N-3) = hi;
349 C(N-3,N-2)= 2*(hi+hii);
350 C(N-3,0)= hii;
351 rs.set(N-3, 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi));
352
353 // last row
354 double h1 = x(1)-x(0);
355 double hN_1 = x(N-1)-x(N-2);
356 C(N-2,0) = 2*(h1+hN_1);
357 C(N-2,1) = h1;
358 C(N-2,N-2)= hN_1;
359 rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
360
361 // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
362 fmatvec::Mat c = slvLU(C,rs);
363 fmatvec::Mat ctmp(N,y.cols());
364 ctmp.set(N-1, c.row(0)); // CN = c1
365 ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
366
367 // vector ordering of the further coefficients
368 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
369 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
370 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
371
372 for(int i=0; i<N-1; i++) {
373 hi = x(i+1)-x(i);
374 a.set(i, y.row(i));
375 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
376 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
377 }
378
379 breaks.resize(N);
380 breaks = x;
381 coefs.clear();
382 coefs.push_back(d);
383 coefs.push_back(c);
384 coefs.push_back(b);
385 coefs.push_back(a);
386 }
387
388 template<typename Ret, typename Arg>
389 void PiecewisePolynomFunction<Ret(Arg)>::calculateSplineNatural() {
390 // first row
391 int i=0;
392 int N = x.size();
393 if(N<4) this->throwError("(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
394 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
395 fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
396 double hi = x(i+1)-x(i);
397 double hii = x(i+2)-x(i+1);
398 C(i,i) = 2*hi+2*hii;
399 C(i,i+1) = hii;
400 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
401
402 // last row
403 i = (N-3);
404 hi = x(i+1)-x(i);
405 hii = x(i+2)-x(i+1);
406 C(i,i-1) = hi;
407 C(i,i) = 2*hii + 2*hi;
408 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
409
410 for(i=1;i<N-3;i++) {
411 hi = x(i+1)-x(i);
412 hii = x(i+2)-x(i+1);
413 C(i,i-1) = hi;
414 C(i,i) = 2*(hi+hii);
415 C(i,i+1) = hii;
416 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
417 }
418
419 // solve C*c = rs with C tridiagonal
420 fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
421 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3), C); // C_rs=[C rs] for Gauss in matrix
422 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1), rs);
423 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
424 fmatvec::Mat rs1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1));
425 fmatvec::Mat C1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3));
426 fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
427 for(i=N-3;i>=0 ;i--) { // backward substitution
428 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
429 sum_ciCi.init(0.);
430 for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
431 c.set(i, (rs1.row(i) - sum_ciCi)/C1(i,i));
432 }
433 fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
434 ctmp.set(fmatvec::RangeV(1,N-2),fmatvec::RangeV(0,y.cols()-1), c); // c1=cN=0 natural splines c=[ 0; c; 0]
435
436 // vector ordering of the further coefficients
437 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
438 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
439 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
440
441 for(i=0; i<N-1; i++) {
442 hi = x(i+1)-x(i);
443 a.set(i, y.row(i));
444 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
445 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
446 }
447
448 breaks.resize(N);
449 breaks = x;
450 coefs.clear();
451 coefs.push_back(d);
452 coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
453 coefs.push_back(b);
454 coefs.push_back(a);
455 }
456
457 template<typename Ret, typename Arg>
458 void PiecewisePolynomFunction<Ret(Arg)>::calculatePLinear() {
459 int N = x.size(); // number of supporting points
460
461 breaks.resize(N);
462 breaks = x;
463
464 fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
465 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
466 for(int i=1;i<N;i++) {
467 m.set(i-1, (y.row(i)-y.row(i-1))/(x(i)-x(i-1))); // slope
468 a.set(i-1, y.row(i-1));
469 }
470 coefs.clear();
471 coefs.push_back(m);
472 coefs.push_back(a);
473 }
474
475 template<typename Ret, typename Arg>
476 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
477 double x = ToDouble<Arg>::cast(x_);
478 if(parent->extrapolationMethod==error) {
479 if(x-1e-13>(parent->breaks)(parent->nPoly))
480 throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
481 if(x+1e-13<(parent->breaks)(0))
482 throw std::runtime_error("(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+", lower bound= "+fmatvec::toString((parent->breaks)(0)));
483 }
484 if(parent->extrapolationMethod==linear && (x<parent->breaks(0) || x>parent->breaks(parent->nPoly))) {
485 int idx = x<parent->breaks(0) ? 0 : parent->nPoly;
486 auto xv = FromDouble<Arg>::cast(parent->breaks(idx));
487 auto value = parent->f(xv);
488 auto der = parent->fd(xv);
489 return value + der * (x - parent->breaks(idx));
490 }
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 const double dx = x - (parent->breaks)(parent->index); // local coordinate
506 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
507 for(int i=1;i<=(parent->order);i++) // Horner scheme
508 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
509 xSave=x;
510 ySave <<= yi;
511 return FromVecV<Ret>::cast(yi);
512 }
513 }
514
515 template<typename Ret, typename Arg>
516 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
517 double x = ToDouble<Arg>::cast(x_);
518 if(parent->extrapolationMethod==error) {
519 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)));
520 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)));
521 }
522 if(parent->extrapolationMethod==linear && (x<parent->breaks(0) || x>parent->breaks(parent->nPoly))) {
523 int idx = x<parent->breaks(0) ? 0 : parent->nPoly;
524 auto xv = FromDouble<Arg>::cast(parent->breaks(idx));
525 auto der = parent->fd(xv);
526 return der;
527 }
528
529 if ((fabs(x-xSave)<macheps) && !firstCall)
530 return FromVecV<Ret>::cast(ySave);
531 else {
532 firstCall = false;
533 if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
534 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
535 (parent->index)--;
536 else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
537 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
538 (parent->index)++;
539 (parent->index)--;
540 }
541
542 double dx = x - (parent->breaks)(parent->index);
543 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
544 for(int i=1;i<parent->order;i++)
545 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
546 xSave=x;
547 ySave <<= yi;
548 return FromVecV<Ret>::cast(yi);
549 }
550 }
551
552 template<typename Ret, typename Arg>
553 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
554 double x = ToDouble<Arg>::cast(x_);
555 if(parent->extrapolationMethod==error) {
556 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)));
557 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)));
558 }
559 if(parent->extrapolationMethod==linear && (x<parent->breaks(0) || x>parent->breaks(parent->nPoly)))
560 return FromVecV<Ret>::cast(fmatvec::VecV(parent->getRetSize().first, fmatvec::INIT, 0.0));
561
562 if ((fabs(x-xSave)<macheps) && !firstCall)
563 return FromVecV<Ret>::cast(ySave);
564 else {
565 firstCall = false;
566 if(x<(parent->breaks)(parent->index)) // saved index still OK? otherwise search downwards
567 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
568 (parent->index)--;
569 else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
570 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
571 (parent->index)++;
572 (parent->index)--;
573 }
574
575 double dx = x - (parent->breaks)(parent->index);
576 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
577 for(int i=1;i<=((parent->order)-2);i++)
578 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
579 xSave=x;
580 ySave <<= yi;
581 return FromVecV<Ret>::cast(yi);
582 }
583 }
584
585 template<typename Ret, typename Arg>
586 void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
587 xercesc::DOMElement *e;
588 fmatvec::VecV x;
589 fmatvec::MatV y;
590 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
591 if (e) {
592 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
593 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
594 sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
595 }
596 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
597 if (e) {
598 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
599 setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
600 }
601 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
602 if(e) {
603 std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
604 str=str.substr(1,str.length()-2);
605 if(str=="cSplinePeriodic") interpolationMethod=cSplinePeriodic;
606 else if(str=="cSplineNatural") interpolationMethod=cSplineNatural;
607 else if(str=="piecewiseLinear") interpolationMethod=piecewiseLinear;
608 else interpolationMethod=useBreaksAndCoefs;
609 }
610
611 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"breaks");
612 if(e) {
613 // set breaks
614 setBreaks(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
615
616 // count number of columns = dimension of the vector returned by this function
617 int nrCols = 0;
618 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"coefficients");
619 while(e) {
620 nrCols++;
621 e=MBXMLUtils::E(e)->getNextElementSiblingNamed(MBSIM%"coefficients");
622 }
623
624 // read all coefficients element and convert to coefs
625 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"coefficients");
626 auto c=MBXMLUtils::E(e)->getText<fmatvec::Mat>();
627 std::vector<fmatvec::MatV> coefs(c.cols(), fmatvec::MatV(c.rows(),nrCols)); // init coefs with proper size
628 bool first=true;
629 int col = 0;
630 while(e) {
631 if(!first) // avoid double read of first coefficients element
632 c=MBXMLUtils::E(e)->getText<fmatvec::Mat>();
633 if(c.cols()!=static_cast<int>(coefs.size()))
634 this->throwError("The number of columns in the coefficients elements differ.");
635 if(c.rows()!=static_cast<int>(coefs[0].rows()))
636 this->throwError("The number of rows in the coefficients elements differ.");
637 for(int deg=0; deg<c.cols(); deg++)
638 coefs[deg].set(col, c.col(deg));
639 e=MBXMLUtils::E(e)->getNextElementSiblingNamed(MBSIM%"coefficients");
640 col++;
641 first=false;
642 }
643 setCoefficients(coefs);
644 interpolationMethod=useBreaksAndCoefs;
645 }
646
647 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"extrapolationMethod");
648 if(e) {
649 std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
650 str=str.substr(1,str.length()-2);
651 if(str=="error") extrapolationMethod=error;
652 else if(str=="continuePolynom") extrapolationMethod=continuePolynom;
653 else if(str=="linear") extrapolationMethod=linear;
654 else this->throwError("Unknown extrapolationMethod.");
655 }
656 }
657
658 }
659
660 #endif /* PPOLYNOM */
MBSim::PiecewisePolynomFunction< Ret(Arg)>::init
void init(Element::InitStage stage, const InitConfigSet &config)
plots time series header
Definition: piecewise_polynom_function.h:86
MBSim::PiecewisePolynomFunction< Ret(Arg)>::setCoefficients
void setCoefficients(const std::vector< fmatvec::MatV > &coefs_u)
set polynomial coefficients
Definition: piecewise_polynom_function.h:194
MBSim::PiecewisePolynomFunction< Ret(Arg)>::setBreaks
void setBreaks(const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:200
MBSim::PiecewisePolynomFunction< Ret(Arg)>::getBreaks
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:165
MBSim::PiecewisePolynomFunction< Ret(Arg)>::setInterpolationMethod
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:177
MBXMLUtils::X
MBSim::PiecewisePolynomFunction< Ret(Arg)>::index
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:232
MBSim::Element::InitStage
InitStage
The stages of the initialization.
Definition: element.h:62
MBSim::PiecewisePolynomFunction
Definition: piecewise_polynom_function.h:31
MBSim::PiecewisePolynomFunction< Ret(Arg)>::interpolationMethod
InterpolationMethod interpolationMethod
interpolation interpolationMethod
Definition: piecewise_polynom_function.h:240
MBSim::PiecewisePolynomFunction< Ret(Arg)>::order
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:227
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::PiecewisePolynomFunction< Ret(Arg)>::getCoefficients
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:160
MBSim::Element::preInit
Definition: element.h:64
MBSim::PiecewisePolynomFunction< Ret(Arg)>::breaks
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:217
MBSim::PiecewisePolynomFunction< Ret(Arg)>::nPoly
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:222
MBSim::PiecewisePolynomFunction< Ret(Arg)>::coefs
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:212
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30