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 {
76 int getArgSize()
const {
return 1; }
78 std::pair<int, int> getRetSize()
const {
return std::make_pair(y.cols(),1); }
81 Function<Ret(Arg)>::init(stage, config);
83 if(method!=useBreaksAndCoefs) {
84 if(y.rows() != x.size())
85 this->throwError(
"Dimension missmatch in size of x");
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 typename B::DRetDArg parDerDirDer(
const Arg &argDir,
const Arg &arg) {
return fdd(arg)*ToDouble<Arg>::cast(argDir); }
134 void setx(
const fmatvec::VecV &x_) { x <<= x_; }
135 void sety(
const fmatvec::MatV &y_) { y <<= y_; }
136 void setxy(
const fmatvec::MatV &xy) {
138 this->throwError(
"Dimension missmatch in size of xy");
140 y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
153 void setBreaks(
const fmatvec::VecV &breaks_u) { breaks <<= breaks_u; }
159 virtual void initializeUsingXML(xercesc::DOMElement *element);
200 void calculateSplinePeriodic();
207 void calculateSplineNatural();
216 void calculatePLinear();
221 class ZerothDerivative {
223 ZerothDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
225 void reset() { firstCall =
true; }
227 Ret operator()(
const Arg &x);
239 class FirstDerivative {
241 FirstDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
243 void reset() { firstCall =
true; }
245 Ret operator()(
const Arg& x);
257 class SecondDerivative {
259 SecondDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
261 void reset() { firstCall =
true; }
263 Ret operator()(
const Arg& x);
275 SecondDerivative fdd;
278 template<
typename Ret,
typename Arg>
282 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)");
283 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
284 fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
287 for(
int i=0; i<N-3;i++) {
291 C(i,i+1) = 2*(hi+hii);
293 rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
300 C(N-3,N-2)= 2*(hi+hii);
302 rs.set(N-3, 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi));
305 double h1 = x(1)-x(0);
306 double hN_1 = x(N-1)-x(N-2);
307 C(N-2,0) = 2*(h1+hN_1);
310 rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
313 fmatvec::Mat c = slvLU(C,rs);
314 fmatvec::Mat ctmp(N,y.cols());
315 ctmp.set(N-1, c.row(0));
316 ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
319 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
320 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
321 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
323 for(
int i=0; i<N-1; i++) {
326 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
327 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
339 template<
typename Ret,
typename Arg>
344 if(N<4) this->throwError(
"(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
345 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
346 fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
347 double hi = x(i+1)-x(i);
348 double hii = x(i+2)-x(i+1);
351 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
358 C(i,i) = 2*hii + 2*hi;
359 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
367 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
371 fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
372 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3), C);
373 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1), rs);
374 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));
375 fmatvec::Mat rs1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1));
376 fmatvec::Mat C1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3));
377 fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
378 for(i=N-3;i>=0 ;i--) {
379 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
381 for(
int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
382 c.set(i, (rs1.row(i) - sum_ciCi)/C1(i,i));
384 fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
385 ctmp.set(fmatvec::RangeV(1,N-2),fmatvec::RangeV(0,y.cols()-1), c);
388 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
389 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
390 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
392 for(i=0; i<N-1; i++) {
395 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
396 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
403 coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
408 template<
typename Ret,
typename Arg>
415 fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
416 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
417 for(
int i=1;i<N;i++) {
418 m.set(i-1, (y.row(i)-y.row(i-1))/(x(i)-x(i-1)));
419 a.set(i-1, y.row(i-1));
426 template<
typename Ret,
typename Arg>
427 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(
const Arg& x_) {
428 double x = ToDouble<Arg>::cast(x_);
429 if(x-1e-13>(parent->breaks)(parent->nPoly))
430 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+
", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
431 if(x+1e-13<(parent->breaks)(0))
432 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+
", lower bound= "+fmatvec::toString((parent->breaks)(0)));
434 if ((fabs(x-xSave)<macheps) && !firstCall)
435 return FromVecV<Ret>::cast(ySave);
438 if(x<(parent->breaks)(parent->index))
439 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
441 else if(x>(parent->breaks)((parent->index)+1)) {
442 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
447 const double dx = x - (parent->breaks)(parent->index);
448 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
449 for(
int i=1;i<=(parent->order);i++)
450 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
453 return FromVecV<Ret>::cast(yi);
457 template<
typename Ret,
typename Arg>
458 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(
const Arg& x_) {
459 double x = ToDouble<Arg>::cast(x_);
460 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)));
461 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)));
463 if ((fabs(x-xSave)<macheps) && !firstCall)
464 return FromVecV<Ret>::cast(ySave);
467 if(x<(parent->breaks)(parent->index))
468 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
470 else if(x>(parent->breaks)((parent->index)+1)) {
471 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
476 double dx = x - (parent->breaks)(parent->index);
477 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order);
478 for(
int i=1;i<parent->order;i++)
479 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i);
482 return FromVecV<Ret>::cast(yi);
486 template<
typename Ret,
typename Arg>
487 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(
const Arg& x_) {
488 double x = ToDouble<Arg>::cast(x_);
489 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)));
490 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)));
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 double dx = x - (parent->breaks)(parent->index);
506 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order)*double((parent->order)-1);
507 for(
int i=1;i<=((parent->order)-2);i++)
508 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i)*
double((parent->order)-i-1);
511 return FromVecV<Ret>::cast(yi);
515 template<
typename Ret,
typename Arg>
517 xercesc::DOMElement *e;
520 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"x");
522 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
523 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"y");
524 sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
526 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
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;
538 else method=useBreaksAndCoefs;
541 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"breaks");
543 setBreaks(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
544 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"coefficients");
545 auto c=MBXMLUtils::E(e)->getText<fmatvec::Mat>();
546 std::vector<fmatvec::MatV> coefs;
547 coefs.reserve(c.cols());
548 for(
int i=0; i<c.cols(); ++i)
549 coefs.emplace_back(c.col(i));
550 setCoefficients(coefs);
551 method=useBreaksAndCoefs;
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:147
void setBreaks(const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:153
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:193
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:120
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:132
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:185
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:180
Definition: contact_kinematics.h:32
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:115
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:170
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:175
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:165
namespace MBSim
Definition: bilateral_constraint.cc:29