20 #ifndef PIECEWISE_POLYNOM
21 #define PIECEWISE_POLYNOM
23 #include "fmatvec/fmatvec.h"
24 #include "mbsim/functions/function.h"
25 #include "mbsim/element.h"
26 #include "mbsim/utils/utils.h"
27 #include "mbsim/utils/eps.h"
28 #include "mbsim/mbsim_event.h"
63 template<
typename Ret,
typename Arg>
68 enum InterpolationMethod {
76 int getArgSize()
const {
return 1; }
81 if(y.rows() != x.size())
82 THROW_MBSIMERROR(
"Dimension missmatch in size of x");
83 if(x.size()) calculateSpline();
84 nPoly = (coefs[0]).rows();
85 order = coefs.size()-1;
89 void calculateSpline() {
90 if(method == cSplinePeriodic) calculateSplinePeriodic();
91 else if(method == cSplineNatural) calculateSplineNatural();
92 else if(method == piecewiseLinear) calculatePLinear();
93 else THROW_MBSIMERROR(
"(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
104 Ret operator()(
const Arg &x) {
return f(x); }
105 typename B::DRetDArg parDer(
const Arg &x) {
return fd(x); }
106 Ret parDerParDer(
const double &x) {
return fdd(x); }
107 typename B::DRetDArg parDerDirDer(
const Arg &argDir,
const Arg &arg) {
return fdd(arg)*ToDouble<Arg>::cast(argDir); }
131 void setx(
const fmatvec::VecV &x_) { x = x_; }
132 void sety(
const fmatvec::MatV &y_) { y = y_; }
133 void setxy(
const fmatvec::MatV &xy) {
135 THROW_MBSIMERROR(
"Dimension missmatch in size of xy");
137 y = xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
150 void setBreaks(
const std::vector<fmatvec::MatV> &coefs_u,
const fmatvec::VecV &breaks_u) { breaks = breaks_u; }
156 virtual void initializeUsingXML(xercesc::DOMElement *element);
197 void calculateSplinePeriodic();
204 void calculateSplineNatural();
213 void calculatePLinear();
218 class ZerothDerivative {
220 ZerothDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
222 void reset() { firstCall =
true; }
224 Ret operator()(
const Arg &x);
236 class FirstDerivative {
238 FirstDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
240 void reset() { firstCall =
true; }
242 Ret operator()(
const Arg& x);
254 class SecondDerivative {
256 SecondDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
258 void reset() { firstCall =
true; }
260 Ret operator()(
const Arg& x);
272 SecondDerivative fdd;
275 template<
typename Ret,
typename Arg>
279 if(nrm2(y.row(0)-y.row(y.rows()-1))>epsroot()) THROW_MBSIMERROR(
"(PiecewisePolynomFunction::calculateSplinePeriodic): f(0)= "+numtostr(y.row(0))+
"!="+numtostr(y.row(y.rows()-1))+
" =f(end)");
284 for(
int i=0; i<N-3;i++) {
288 C(i,i+1) = 2*(hi+hii);
290 rs.row(i) = 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi);
297 C(N-3,N-2)= 2*(hi+hii);
299 rs.row(N-3) = 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi);
302 double h1 = x(1)-x(0);
303 double hN_1 = x(N-1)-x(N-2);
304 C(N-2,0) = 2*(h1+hN_1);
307 rs.row(N-2) = 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1);
312 ctmp.row(N-1) = c.row(0);
313 ctmp(0,0,N-2,y.cols()-1)= c;
320 for(
int i=0; i<N-1; i++) {
323 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
324 b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
335 template<
typename Ret,
typename Arg>
342 double hi = x(i+1)-x(i);
343 double hii = x(i+2)-x(i+1);
346 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
353 C(i,i) = 2*hii + 2*hi;
354 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
362 rs.row(i) = 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi;
367 C_rs(0,0,N-3,N-3) = C;
368 C_rs(0,N-2,N-3,N-2+y.cols()-1) = rs;
369 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);
373 for(i=N-3;i>=0 ;i--) {
374 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
376 for(
int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
377 c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
380 ctmp(1,0,N-2,y.cols()-1) = c;
387 for(i=0; i<N-1; i++) {
390 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
391 b.row(i) = (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
397 coefs.push_back(ctmp(0,0,N-2,y.cols()-1));
402 template<
typename Ret,
typename Arg>
411 for(
int i=1;i<N;i++) {
412 m.row(i-1) = (y.row(i)-y.row(i-1))/(x(i)-x(i-1));
413 a.row(i-1) = y.row(i-1);
419 template<
typename Ret,
typename Arg>
420 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(
const Arg& x_) {
421 double x = ToDouble<Arg>::cast(x_);
422 if(x>(parent->breaks)(parent->nPoly))
423 throw MBSimError(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+
", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
424 if(x<(parent->breaks)(0))
425 throw MBSimError(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+
", lower bound= "+numtostr((parent->breaks)(0)));
427 if ((fabs(x-xSave)<macheps()) && !firstCall)
428 return FromVecV<Ret>::cast(ySave);
431 if(x<(parent->breaks)(parent->index))
432 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
434 else if(x>(parent->breaks)((parent->index)+1)) {
435 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
440 const double dx = x - (parent->breaks)(parent->index);
441 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index));
442 for(
int i=1;i<=(parent->order);i++)
443 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index));
446 return FromVecV<Ret>::cast(yi);
450 template<
typename Ret,
typename Arg>
451 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(
const Arg& x_) {
452 double x = ToDouble<Arg>::cast(x_);
453 if(x>(parent->breaks)(parent->nPoly))
throw MBSimError(
"(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+
", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
454 if(x<(parent->breaks)(0))
throw MBSimError(
"(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+
" lower bound= "+numtostr((parent->breaks)(0)));
456 if ((fabs(x-xSave)<macheps()) && !firstCall)
457 return FromVecV<Ret>::cast(ySave);
460 if(x<(parent->breaks)(parent->index))
461 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
463 else if(x>(parent->breaks)((parent->index)+1)) {
464 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
469 double dx = x - (parent->breaks)(parent->index);
470 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order);
471 for(
int i=1;i<parent->order;i++)
472 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i);
475 return FromVecV<Ret>::cast(yi);
479 template<
typename Ret,
typename Arg>
480 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(
const Arg& x_) {
481 double x = ToDouble<Arg>::cast(x_);
482 if(x>(parent->breaks)(parent->nPoly))
throw MBSimError(
"(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+
" upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
483 if(x<(parent->breaks)(0))
throw MBSimError(
"(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+
" lower bound= "+numtostr((parent->breaks)(0)));
485 if ((fabs(x-xSave)<macheps()) && !firstCall)
486 return FromVecV<Ret>::cast(ySave);
489 if(x<(parent->breaks)(parent->index))
490 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
492 else if(x>(parent->breaks)((parent->index)+1)) {
493 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
498 double dx = x - (parent->breaks)(parent->index);
499 fmatvec::VecV yi =
trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order)*double((parent->order)-1);
500 for(
int i=1;i<=((parent->order)-2);i++)
501 yi = yi*dx+
trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i)*
double((parent->order)-i-1);
504 return FromVecV<Ret>::cast(yi);
508 template<
typename Ret,
typename Arg>
510 xercesc::DOMElement *e;
513 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"x");
515 setx(Element::getVec(e));
516 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"y");
517 sety(Element::getMat(e, x.size(), 0));
520 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
521 setxy(Element::getMat(e));
523 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"interpolationMethod");
525 std::string str=
MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
526 str=str.substr(1,str.length()-2);
527 if(str==
"cSplinePeriodic") method=cSplinePeriodic;
528 else if(str==
"cSplineNatural") method=cSplineNatural;
529 else if(str==
"piecewiseLinear") method=piecewiseLinear;
void setCoefficients(const std::vector< fmatvec::MatV > &coefs_u)
set polynomial coefficients
Definition: piecewise_polynom_function.h:144
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:190
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:117
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:129
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:48
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:182
InitStage
The stages of the initialization.
Definition: element.h:97
Definition: piecewise_polynom_function.h:32
void setBreaks(const std::vector< fmatvec::MatV > &coefs_u, const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:150
int order
order of polynomial (3 for cubic polynomials)
Definition: piecewise_polynom_function.h:177
Definition: planar_contour.h:31
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:112
Definition: element.h:100
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:167
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:172
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:162
class for piecewise-polynomials and cubic spline interpolation
Definition: piecewise_polynom_function.h:64
RowVector< Ref, AT > trans(const Vector< Ref, AT > &x)
void init(Element::InitStage stage)
plots time series header
Definition: piecewise_polynom_function.h:78