20 #ifndef PIECEWISE_POLYNOM
21 #define PIECEWISE_POLYNOM
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"
62 template<
typename Ret,
typename Arg>
64 using B = fmatvec::Function<Ret(Arg)>;
67 enum InterpolationMethod {
74 enum ExtrapolationMethod {
80 PiecewisePolynomFunction() : index(0), interpolationMethod(cSplineNatural), extrapolationMethod(error), f(
this), fd(
this), fdd(
this) { }
82 int getArgSize()
const {
return 1; }
84 std::pair<int, int> getRetSize()
const {
return std::make_pair(coefs[0].cols(),1); }
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++)
94 this->throwError(
"Values of x must be strictly monotonic increasing!");
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!");
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());
119 c.resize(coefsOld[0].rows()-removeIdx.size(),coefsOld[0].cols());
121 for(i=0; i<breaksOld.size()-1; ++i) {
122 if(removeIdx.find(i)!=removeIdx.end())
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));
129 breaks(ii)=breaksOld(i);
133 nPoly = (coefs[0]).rows();
134 order = coefs.size()-1;
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");
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); }
179 void setExtrapolationMethod(ExtrapolationMethod method_) { extrapolationMethod = method_; }
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) {
185 this->throwError(
"Dimension missmatch in size of xy");
187 y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
200 void setBreaks(
const fmatvec::VecV &breaks_u) { breaks <<= breaks_u; }
206 virtual void initializeUsingXML(xercesc::DOMElement *element);
242 ExtrapolationMethod extrapolationMethod;
249 void calculateSplinePeriodic();
256 void calculateSplineNatural();
265 void calculatePLinear();
270 class ZerothDerivative {
272 ZerothDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
274 void reset() { firstCall =
true; }
276 Ret operator()(
const Arg &x);
288 class FirstDerivative {
290 FirstDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
292 void reset() { firstCall =
true; }
294 Ret operator()(
const Arg& x);
306 class SecondDerivative {
308 SecondDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
310 void reset() { firstCall =
true; }
312 Ret operator()(
const Arg& x);
324 SecondDerivative fdd;
327 template<
typename Ret,
typename Arg>
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);
336 for(
int i=0; i<N-3;i++) {
340 C(i,i+1) = 2*(hi+hii);
342 rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
349 C(N-3,N-2)= 2*(hi+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));
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);
359 rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
362 fmatvec::Mat c = slvLU(C,rs);
363 fmatvec::Mat ctmp(N,y.cols());
364 ctmp.set(N-1, c.row(0));
365 ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
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);
372 for(
int i=0; i<N-1; 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);
388 template<
typename Ret,
typename Arg>
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);
400 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/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);
416 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
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);
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));
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--) {
428 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
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));
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);
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);
441 for(i=0; i<N-1; 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);
452 coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
457 template<
typename Ret,
typename Arg>
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)));
468 a.set(i-1, y.row(i-1));
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)));
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));
492 if ((fabs(x-xSave)<macheps) && !firstCall)
493 return FromVecV<Ret>::cast(ySave);
496 if(x<(parent->breaks)(parent->index))
497 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
499 else if(x>(parent->breaks)((parent->index)+1)) {
500 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
505 const double dx = x - (parent->breaks)(parent->index);
506 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
507 for(
int i=1;i<=(parent->order);i++)
508 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
511 return FromVecV<Ret>::cast(yi);
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)));
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);
529 if ((fabs(x-xSave)<macheps) && !firstCall)
530 return FromVecV<Ret>::cast(ySave);
533 if(x<(parent->breaks)(parent->index))
534 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
536 else if(x>(parent->breaks)((parent->index)+1)) {
537 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
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);
548 return FromVecV<Ret>::cast(yi);
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)));
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));
562 if ((fabs(x-xSave)<macheps) && !firstCall)
563 return FromVecV<Ret>::cast(ySave);
566 if(x<(parent->breaks)(parent->index))
567 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
569 else if(x>(parent->breaks)((parent->index)+1)) {
570 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
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);
581 return FromVecV<Ret>::cast(yi);
585 template<
typename Ret,
typename Arg>
587 xercesc::DOMElement *e;
590 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"x");
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));
596 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
598 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
599 setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
601 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"interpolationMethod");
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;
611 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"breaks");
614 setBreaks(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
618 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"coefficients");
621 e=MBXMLUtils::E(e)->getNextElementSiblingNamed(MBSIM%
"coefficients");
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));
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");
643 setCoefficients(coefs);
644 interpolationMethod=useBreaksAndCoefs;
647 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"extrapolationMethod");
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.");
void init(Element::InitStage stage, const InitConfigSet &config)
plots time series header
Definition: piecewise_polynom_function.h:86
void setCoefficients(const std::vector< fmatvec::MatV > &coefs_u)
set polynomial coefficients
Definition: piecewise_polynom_function.h:194
void setBreaks(const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:200
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:165
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:177
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:232
InitStage
The stages of the initialization.
Definition: element.h:62
Definition: piecewise_polynom_function.h:31
InterpolationMethod interpolationMethod
interpolation interpolationMethod
Definition: piecewise_polynom_function.h:240
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:227
Definition: contact_kinematics.h:32
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:160
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:217
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:222
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:212
class for piecewise-polynomials and cubic spline interpolation
Definition: piecewise_polynom_function.h:63