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");
86 for(
int i=1; i<x.size(); i++)
88 this->throwError(
"Values of x must be strictly monotonic increasing!");
92 for(
int i=1; i<breaks.size(); i++)
93 if(breaks(i) <= breaks(i-1))
94 this->throwError(
"Values of breaks must be strictly monotonic increasing!");
96 nPoly = (coefs[0]).rows();
97 order = coefs.size()-1;
101 void calculateSpline() {
102 if(method == cSplinePeriodic) calculateSplinePeriodic();
103 else if(method == cSplineNatural) calculateSplineNatural();
104 else if(method == piecewiseLinear) calculatePLinear();
105 else this->throwError(
"(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
116 Ret operator()(
const Arg &x) {
return f(x); }
117 typename B::DRetDArg parDer(
const Arg &x) {
return fd(x); }
118 typename B::DRetDArg parDerDirDer(
const Arg &argDir,
const Arg &arg) {
return fdd(arg)*ToDouble<Arg>::cast(argDir); }
142 void setx(
const fmatvec::VecV &x_) { x <<= x_; }
143 void sety(
const fmatvec::MatV &y_) { y <<= y_; }
144 void setxy(
const fmatvec::MatV &xy) {
146 this->throwError(
"Dimension missmatch in size of xy");
148 y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
161 void setBreaks(
const fmatvec::VecV &breaks_u) { breaks <<= breaks_u; }
167 virtual void initializeUsingXML(xercesc::DOMElement *element);
208 void calculateSplinePeriodic();
215 void calculateSplineNatural();
224 void calculatePLinear();
229 class ZerothDerivative {
231 ZerothDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
233 void reset() { firstCall =
true; }
235 Ret operator()(
const Arg &x);
247 class FirstDerivative {
249 FirstDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
251 void reset() { firstCall =
true; }
253 Ret operator()(
const Arg& x);
265 class SecondDerivative {
267 SecondDerivative(
PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(
true) {}
269 void reset() { firstCall =
true; }
271 Ret operator()(
const Arg& x);
283 SecondDerivative fdd;
286 template<
typename Ret,
typename Arg>
290 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)");
291 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
292 fmatvec::Mat rs(N-1,y.cols(),fmatvec::INIT,0.0);
295 for(
int i=0; i<N-3;i++) {
299 C(i,i+1) = 2*(hi+hii);
301 rs.set(i, 3.*((y.row(i+2)-y.row(i+1))/hii - (y.row(i+1)-y.row(i))/hi));
308 C(N-3,N-2)= 2*(hi+hii);
310 rs.set(N-3, 3.*((y.row(N-1)-y.row(N-2))/hii - (y.row(N-2)-y.row(N-3))/hi));
313 double h1 = x(1)-x(0);
314 double hN_1 = x(N-1)-x(N-2);
315 C(N-2,0) = 2*(h1+hN_1);
318 rs.set(N-2, 3.*((y.row(1)-y.row(0))/h1 - (y.row(0)-y.row(N-2))/hN_1));
321 fmatvec::Mat c = slvLU(C,rs);
322 fmatvec::Mat ctmp(N,y.cols());
323 ctmp.set(N-1, c.row(0));
324 ctmp.set(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1), c);
327 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
328 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
329 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
331 for(
int i=0; i<N-1; i++) {
334 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
335 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
347 template<
typename Ret,
typename Arg>
352 if(N<4) this->throwError(
"(PiecewisePolynomFunction::calculateSplineNatural): number of datapoints does not match, must be greater 3");
353 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
354 fmatvec::Mat rs(N-2,y.cols(),fmatvec::INIT,0.0);
355 double hi = x(i+1)-x(i);
356 double hii = x(i+2)-x(i+1);
359 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
366 C(i,i) = 2*hii + 2*hi;
367 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
375 rs.set(i, 3.*(y.row(i+2)-y.row(i+1))/hii - 3.*(y.row(i+1)-y.row(i))/hi);
379 fmatvec::Mat C_rs(N-2,N-1+y.cols(),fmatvec::INIT,0.0);
380 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3), C);
381 C_rs.set(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1), rs);
382 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));
383 fmatvec::Mat rs1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(N-2,N-2+y.cols()-1));
384 fmatvec::Mat C1 = C_rs(fmatvec::RangeV(0,N-3),fmatvec::RangeV(0,N-3));
385 fmatvec::Mat c(N-2,y.cols(),fmatvec::INIT,0.0);
386 for(i=N-3;i>=0 ;i--) {
387 fmatvec::RowVecV sum_ciCi(y.cols(),fmatvec::NONINIT);
389 for(
int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
390 c.set(i, (rs1.row(i) - sum_ciCi)/C1(i,i));
392 fmatvec::Mat ctmp(N,y.cols(),fmatvec::INIT,0.0);
393 ctmp.set(fmatvec::RangeV(1,N-2),fmatvec::RangeV(0,y.cols()-1), c);
396 fmatvec::Mat d(N-1,y.cols(),fmatvec::INIT,0.0);
397 fmatvec::Mat b(N-1,y.cols(),fmatvec::INIT,0.0);
398 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
400 for(i=0; i<N-1; i++) {
403 d.set(i, (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi);
404 b.set(i, (y.row(i+1)-y.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi);
411 coefs.push_back(ctmp(fmatvec::RangeV(0,N-2),fmatvec::RangeV(0,y.cols()-1)));
416 template<
typename Ret,
typename Arg>
423 fmatvec::Mat m(N-1,y.cols(),fmatvec::INIT,0.0);
424 fmatvec::Mat a(N-1,y.cols(),fmatvec::INIT,0.0);
425 for(
int i=1;i<N;i++) {
426 m.set(i-1, (y.row(i)-y.row(i-1))/(x(i)-x(i-1)));
427 a.set(i-1, y.row(i-1));
434 template<
typename Ret,
typename Arg>
435 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(
const Arg& x_) {
436 double x = ToDouble<Arg>::cast(x_);
437 if(x-1e-13>(parent->breaks)(parent->nPoly))
438 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+
", upper bound= "+fmatvec::toString((parent->breaks)(parent->nPoly)));
439 if(x+1e-13<(parent->breaks)(0))
440 throw std::runtime_error(
"(PiecewisePolynomFunction::operator()): x out of range! x= "+fmatvec::toString(x)+
", lower bound= "+fmatvec::toString((parent->breaks)(0)));
442 if ((fabs(x-xSave)<macheps) && !firstCall)
443 return FromVecV<Ret>::cast(ySave);
446 if(x<(parent->breaks)(parent->index))
447 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
449 else if(x>(parent->breaks)((parent->index)+1)) {
450 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
455 const double dx = x - (parent->breaks)(parent->index);
456 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
457 for(
int i=1;i<=(parent->order);i++)
458 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
461 return FromVecV<Ret>::cast(yi);
465 template<
typename Ret,
typename Arg>
466 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(
const Arg& x_) {
467 double x = ToDouble<Arg>::cast(x_);
468 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)));
469 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)));
471 if ((fabs(x-xSave)<macheps) && !firstCall)
472 return FromVecV<Ret>::cast(ySave);
475 if(x<(parent->breaks)(parent->index))
476 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
478 else if(x>(parent->breaks)((parent->index)+1)) {
479 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
484 double dx = x - (parent->breaks)(parent->index);
485 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order);
486 for(
int i=1;i<parent->order;i++)
487 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i);
490 return FromVecV<Ret>::cast(yi);
494 template<
typename Ret,
typename Arg>
495 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(
const Arg& x_) {
496 double x = ToDouble<Arg>::cast(x_);
497 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)));
498 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)));
500 if ((fabs(x-xSave)<macheps) && !firstCall)
501 return FromVecV<Ret>::cast(ySave);
504 if(x<(parent->breaks)(parent->index))
505 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
507 else if(x>(parent->breaks)((parent->index)+1)) {
508 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
513 double dx = x - (parent->breaks)(parent->index);
514 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*
double(parent->order)*double((parent->order)-1);
515 for(
int i=1;i<=((parent->order)-2);i++)
516 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*
double((parent->order)-i)*
double((parent->order)-i-1);
519 return FromVecV<Ret>::cast(yi);
523 template<
typename Ret,
typename Arg>
525 xercesc::DOMElement *e;
528 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"x");
530 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
531 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"y");
532 sety(MBXMLUtils::E(e)->getText<fmatvec::Mat>(x.size(), 0));
534 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
536 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"xy");
537 setxy(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
539 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"interpolationMethod");
541 std::string str=
MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
542 str=str.substr(1,str.length()-2);
543 if(str==
"cSplinePeriodic") method=cSplinePeriodic;
544 else if(str==
"cSplineNatural") method=cSplineNatural;
545 else if(str==
"piecewiseLinear") method=piecewiseLinear;
546 else method=useBreaksAndCoefs;
549 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"breaks");
551 setBreaks(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
552 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"coefficients");
553 auto c=MBXMLUtils::E(e)->getText<fmatvec::Mat>();
554 std::vector<fmatvec::MatV> coefs;
555 coefs.reserve(c.cols());
556 for(
int i=0; i<c.cols(); ++i)
557 coefs.emplace_back(c.col(i));
558 setCoefficients(coefs);
559 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:155
void setBreaks(const fmatvec::VecV &breaks_u)
set interval boundaries
Definition: piecewise_polynom_function.h:161
InterpolationMethod method
interpolation method
Definition: piecewise_polynom_function.h:201
fmatvec::VecV getBreaks()
Definition: piecewise_polynom_function.h:128
void setInterpolationMethod(InterpolationMethod method_)
set interpolation
Definition: piecewise_polynom_function.h:140
int index
for internal use in ppeval functions
Definition: piecewise_polynom_function.h:193
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:188
Definition: contact_kinematics.h:32
std::vector< fmatvec::MatV > getCoefficients()
Definition: piecewise_polynom_function.h:123
fmatvec::VecV breaks
vector of breaks (interval boundaries)
Definition: piecewise_polynom_function.h:178
int nPoly
number of defined piecewise polynomials
Definition: piecewise_polynom_function.h:183
std::vector< fmatvec::MatV > coefs
vector of polynomial coefficents
Definition: piecewise_polynom_function.h:173
namespace MBSim
Definition: bilateral_constraint.cc:30