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>
67 enum InterpolationMethod {
76 int getArgSize()
const {
return 1; }
78 std::pair<int, int> getRetSize()
const {
return std::make_pair(y.cols(),1); }
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;
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");
108 Ret operator()(
const Arg &x) {
return f(x); }
109 typename B::DRetDArg parDer(
const Arg &x) {
return fd(x); }
110 Ret parDerParDer(
const double &x) {
return fdd(x); }
111 typename B::DRetDArg parDerDirDer(
const Arg &argDir,
const Arg &arg) {
return fdd(arg)*ToDouble<Arg>::cast(argDir); }
135 void setx(
const fmatvec::VecV &x_) { x = x_; }
136 void sety(
const fmatvec::MatV &y_) { y = y_; }
137 void setxy(
const fmatvec::MatV &xy) {
139 this->throwError(
"Dimension missmatch in size of xy");
141 y = xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
154 void setBreaks(
const std::vector<fmatvec::MatV> &coefs_u,
const fmatvec::VecV &breaks_u) { breaks = breaks_u; }
160 virtual void initializeUsingXML(xercesc::DOMElement *element);
201 void calculateSplinePeriodic();
208 void calculateSplineNatural();
217 void calculatePLinear();
222 class ZerothDerivative {
224 ZerothDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
226 void reset() { firstCall =
true; }
228 Ret operator()(
const Arg &x);
240 class FirstDerivative {
242 FirstDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
244 void reset() { firstCall =
true; }
246 Ret operator()(
const Arg& x);
258 class SecondDerivative {
260 SecondDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
262 void reset() { firstCall =
true; }
264 Ret operator()(
const Arg& x);
276 SecondDerivative fdd;
279 template<
typename Ret,
typename Arg>
283 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)");
288 for(
int i=0; i<N-3;i++) {
292 C(i,i+1) = 2*(hi+hii);
294 rs.row(i) = 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi);
301 C(N-3,N-2)= 2*(hi+hii);
303 rs.row(N-3) = 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi);
306 double h1 = x(1)-x(0);
307 double hN_1 = x(N-1)-x(N-2);
308 C(N-2,0) = 2*(h1+hN_1);
311 rs.row(N-2) = 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1);
316 ctmp.row(N-1) = c.row(0);
317 ctmp(0,0,N-2,y.cols()-1)= c;
324 for(
int i=0; i<N-1; i++) {
327 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
328 b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
340 template<
typename Ret,
typename Arg>
345 if(N<4) this->throwError(
"(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
348 double hi = x(i+1)-x(i);
349 double hii = x(i+2)-x(i+1);
352 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
359 C(i,i) = 2*hii + 2*hi;
360 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
368 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
373 C_rs(0,0,N-3,N-3) = C;
374 C_rs(0,N-2,N-3,N-2+y.cols()-1) = rs;
375 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);
379 for(i=N-3;i>=0 ;i--) {
380 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
382 for(
int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
383 c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
386 ctmp(1,0,N-2,y.cols()-1) = c;
393 for(i=0; i<N-1; i++) {
396 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
397 b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
404 coefs.push_back(ctmp(0,0,N-2,y.cols()-1));
409 template<
typename Ret,
typename Arg>
418 for(
int i=1;i<N;i++) {
419 m.row(i-1) = (y.row(i)-y.row(i-1))/(x(i)-x(i-1));
420 a.row(i-1) = y.row(i-1);
427 template<
typename Ret,
typename Arg>
428 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(
const Arg& x_) {
429 double x = ToDouble<Arg>::cast(x_);
430 if(x-1e-13>(parent->breaks)(parent->nPoly))
431 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+
fmatvec::toString(x)+
", upper bound= "+
fmatvec::toString((parent->breaks)(parent->nPoly)));
432 if(x+1e-13<(parent->breaks)(0))
433 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+
fmatvec::toString(x)+
", lower bound= "+
fmatvec::toString((parent->breaks)(0)));
435 if ((fabs(x-xSave)<macheps) && !firstCall)
436 return FromVecV<Ret>::cast(ySave);
439 if(x<(parent->breaks)(parent->index))
440 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
442 else if(x>(parent->breaks)((parent->index)+1)) {
443 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
448 const double dx = x - (parent->breaks)(parent->index);
449 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index));
450 for(
int i=1;i<=(parent->order);i++)
451 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index));
454 return FromVecV<Ret>::cast(yi);
458 template<
typename Ret,
typename Arg>
459 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(
const Arg& x_) {
460 double x = ToDouble<Arg>::cast(x_);
461 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)));
462 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)));
464 if ((fabs(x-xSave)<macheps) && !firstCall)
465 return FromVecV<Ret>::cast(ySave);
468 if(x<(parent->breaks)(parent->index))
469 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
471 else if(x>(parent->breaks)((parent->index)+1)) {
472 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
477 double dx = x - (parent->breaks)(parent->index);
478 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order);
479 for(
int i=1;i<parent->order;i++)
480 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i);
483 return FromVecV<Ret>::cast(yi);
487 template<
typename Ret,
typename Arg>
488 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(
const Arg& x_) {
489 double x = ToDouble<Arg>::cast(x_);
490 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)));
491 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)));
493 if ((fabs(x-xSave)<macheps) && !firstCall)
494 return FromVecV<Ret>::cast(ySave);
497 if(x<(parent->breaks)(parent->index))
498 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
500 else if(x>(parent->breaks)((parent->index)+1)) {
501 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
506 double dx = x - (parent->breaks)(parent->index);
507 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order)*double((parent->order)-1);
508 for(
int i=1;i<=((parent->order)-2);i++)
509 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i)*
double((parent->order)-i-1);
512 return FromVecV<Ret>::cast(yi);
516 template<
typename Ret,
typename Arg>
518 xercesc::DOMElement *e;
521 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"x");
523 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
524 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"y");
525 sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
528 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
529 setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
531 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"interpolationMethod");
533 std::string str=
MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
534 str=str.substr(1,str.length()-2);
535 if(str==
"cSplinePeriodic") method=cSplinePeriodic;
536 else if(str==
"cSplineNatural") method=cSplineNatural;
537 else if(str==
"piecewiseLinear") method=piecewiseLinear;
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:148
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:194
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:121
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:133
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:93
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:186
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:154
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:181
Definition: planar_contour.h:30
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:116
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:171
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:176
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:166
class for piecewise-polynomials and cubic spline interpolation
Definition: piecewise_polynom_function.h:63
RowVector< Ref, AT > trans(const Vector< Ref, AT > &x)
std::string toString(const T &value, int precision=0)