00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef PIECEWISE_POLYNOM
00021 #define PIECEWISE_POLYNOM
00022
00023 #include "fmatvec/fmatvec.h"
00024 #include "fmatvec/function.h"
00025 #include "mbsim/element.h"
00026 #include "mbsim/utils/utils.h"
00027 #include "mbsim/utils/eps.h"
00028 #include "mbsim/mbsim_event.h"
00029
00030 namespace MBSim {
00031
00032 template<typename Sig> class PiecewisePolynomFunction;
00033
00063 template<typename Ret, typename Arg>
00064 class PiecewisePolynomFunction<Ret(Arg)> : public fmatvec::Function<Ret(Arg)> {
00065
00066 public:
00067 enum InterpolationMethod {
00068 cSplinePeriodic,
00069 cSplineNatural,
00070 piecewiseLinear
00071 };
00072
00073 PiecewisePolynomFunction() : f(this), fd(this), fdd(this) { }
00074
00075 Ret operator()(const Arg &x) { return f(x); }
00076 typename fmatvec::Der<Ret, Arg>::type parDer(const Arg &x) { return fd(x); }
00077 typename fmatvec::Der<typename fmatvec::Der<Ret, double>::type, double>::type parDerParDer(const double &x) { return fdd(x); }
00078
00091 void setXF(const fmatvec::VecV &x, const fmatvec::MatV &f, InterpolationMethod method=cSplineNatural) {
00092 assert(x.size() == f.rows());
00093
00094 if(method == cSplinePeriodic) {
00095 calculateSplinePeriodic(x,f);
00096 }
00097 else if(method == cSplineNatural) {
00098 calculateSplineNatural(x,f);
00099 }
00100 else if(method == piecewiseLinear) {
00101 calculatePLinear(x,f);
00102 }
00103 else throw MBSimError("(PiecewisePolynomFunction::setXF): No valid method to calculate pp-form");
00104
00105 index = 0;
00106 nPoly = x.size()-1;
00107 order = coefs.size()-1;
00108 }
00109
00113 std::vector<fmatvec::MatV> getCoefs() { return coefs; }
00114
00118 fmatvec::VecV getBreaks() { return breaks; }
00119
00125 void setPP(const std::vector<fmatvec::MatV> &coefs_u, const fmatvec::Vec &breaks_u) {
00126 coefs = coefs_u;
00127 breaks = breaks_u;
00128 index = 0;
00129 nPoly = (coefs[0]).rows();
00130 order = coefs.size()-1;
00131 }
00132
00137 virtual void initializeUsingXML(xercesc::DOMElement *element);
00138
00139 protected:
00143 std::vector<fmatvec::MatV> coefs;
00144
00148 fmatvec::Vec breaks;
00149
00153 int nPoly;
00154
00158 int order;
00159
00163 int index;
00164
00170 void calculateSplinePeriodic(const fmatvec::VecV &x, const fmatvec::MatV &f);
00171
00177 void calculateSplineNatural(const fmatvec::VecV &x, const fmatvec::MatV &f);
00178
00179
00180
00181
00182
00183
00184
00185
00186 void calculatePLinear(const fmatvec::VecV &x, const fmatvec::MatV &f);
00187
00191 class ZerothDerivative {
00192 public:
00193 ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00194 virtual ~ZerothDerivative() {}
00195
00196
00197 Ret operator()(const Arg &x);
00198
00199
00200 private:
00201 PiecewisePolynomFunction<Ret(Arg)> *parent;
00202 double xSave;
00203 fmatvec::VecV ySave;
00204 bool firstCall;
00205 };
00206
00210 class FirstDerivative {
00211 public:
00212 FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00213 virtual ~FirstDerivative() {}
00214
00215
00216 Ret operator()(const Arg& x);
00217
00218
00219 private:
00220 PiecewisePolynomFunction<Ret(Arg)> *parent;
00221 double xSave;
00222 fmatvec::VecV ySave;
00223 bool firstCall;
00224 };
00225
00229 class SecondDerivative {
00230 public:
00231 SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00232 virtual ~SecondDerivative() {}
00233
00234
00235 Ret operator()(const Arg& x);
00236
00237
00238 private:
00239 PiecewisePolynomFunction<Ret(Arg)> *parent;
00240 double xSave;
00241 fmatvec::VecV ySave;
00242 bool firstCall;
00243 };
00244
00245 private:
00246 ZerothDerivative f;
00247 FirstDerivative fd;
00248 SecondDerivative fdd;
00249 };
00250
00251 template<typename Ret, typename Arg>
00252 void PiecewisePolynomFunction<Ret(Arg)>::calculateSplinePeriodic(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00253 double hi, hii;
00254 int N = x.size();
00255 if(nrm2(f.row(0)-f.row(f.rows()-1))>epsroot()) throw MBSimError("(PiecewisePolynomFunction::calculateSplinePeriodic): f(0)= "+numtostr(f.row(0))+"!="+numtostr(f.row(f.rows()-1))+" =f(end)");
00256 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
00257 fmatvec::Mat rs(N-1,f.cols(),fmatvec::INIT,0.0);
00258
00259
00260 for(int i=0; i<N-3;i++) {
00261 hi = x(i+1) - x(i);
00262 hii = x(i+2)-x(i+1);
00263 C(i,i) = hi;
00264 C(i,i+1) = 2*(hi+hii);
00265 C(i,i+2) = hii;
00266 rs.row(i) = 3.*((f.row(i+2)-f.row(i+1))/hii - (f.row(i+1)-f.row(i))/hi);
00267 }
00268
00269
00270 hi = x(N-2)-x(N-3);
00271 hii = x(N-1)-x(N-2);
00272 C(N-3,N-3) = hi;
00273 C(N-3,N-2)= 2*(hi+hii);
00274 C(N-3,0)= hii;
00275 rs.row(N-3) = 3.*((f.row(N-1)-f.row(N-2))/hii - (f.row(N-2)-f.row(N-3))/hi);
00276
00277
00278 double h1 = x(1)-x(0);
00279 double hN_1 = x(N-1)-x(N-2);
00280 C(N-2,0) = 2*(h1+hN_1);
00281 C(N-2,1) = h1;
00282 C(N-2,N-2)= hN_1;
00283 rs.row(N-2) = 3.*((f.row(1)-f.row(0))/h1 - (f.row(0)-f.row(N-2))/hN_1);
00284
00285
00286 fmatvec::Mat c = slvLU(C,rs);
00287 fmatvec::Mat ctmp(N,f.cols());
00288 ctmp.row(N-1) = c.row(0);
00289 ctmp(0,0,N-2,f.cols()-1)= c;
00290
00291
00292 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00293 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00294 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00295
00296 for(int i=0; i<N-1; i++) {
00297 hi = x(i+1)-x(i);
00298 a.row(i) = f.row(i);
00299 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00300 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00301 }
00302
00303 breaks.resize(N);
00304 breaks = x;
00305 coefs.push_back(d);
00306 coefs.push_back(c);
00307 coefs.push_back(b);
00308 coefs.push_back(a);
00309 }
00310
00311 template<typename Ret, typename Arg>
00312 void PiecewisePolynomFunction<Ret(Arg)>::calculateSplineNatural(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00313
00314 int i=0;
00315 int N = x.size();
00316 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
00317 fmatvec::Mat rs(N-2,f.cols(),fmatvec::INIT,0.0);
00318 double hi = x(i+1)-x(i);
00319 double hii = x(i+2)-x(i+1);
00320 C(i,i) = 2*hi+2*hii;
00321 C(i,i+1) = hii;
00322 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00323
00324
00325 i = (N-3);
00326 hi = x(i+1)-x(i);
00327 hii = x(i+2)-x(i+1);
00328 C(i,i-1) = hi;
00329 C(i,i) = 2*hii + 2*hi;
00330 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00331
00332 for(i=1;i<N-3;i++) {
00333 hi = x(i+1)-x(i);
00334 hii = x(i+2)-x(i+1);
00335 C(i,i-1) = hi;
00336 C(i,i) = 2*(hi+hii);
00337 C(i,i+1) = hii;
00338 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00339 }
00340
00341
00342 fmatvec::Mat C_rs(N-2,N-1+f.cols(),fmatvec::INIT,0.0);
00343 C_rs(0,0,N-3,N-3) = C;
00344 C_rs(0,N-2,N-3,N-2+f.cols()-1) = rs;
00345 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);
00346 fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+f.cols()-1);
00347 fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
00348 fmatvec::Mat c(N-2,f.cols(),fmatvec::INIT,0.0);
00349 for(i=N-3;i>=0 ;i--) {
00350 fmatvec::RowVecV sum_ciCi(f.cols(),fmatvec::NONINIT);
00351 sum_ciCi.init(0.);
00352 for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
00353 c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
00354 }
00355 fmatvec::Mat ctmp(N,f.cols(),fmatvec::INIT,0.0);
00356 ctmp(1,0,N-2,f.cols()-1) = c;
00357
00358
00359 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00360 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00361 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00362
00363 for(i=0; i<N-1; i++) {
00364 hi = x(i+1)-x(i);
00365 a.row(i) = f.row(i);
00366 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00367 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00368 }
00369
00370 breaks.resize(N);
00371 breaks = x;
00372 coefs.push_back(d);
00373 coefs.push_back(ctmp(0,0,N-2,f.cols()-1));
00374 coefs.push_back(b);
00375 coefs.push_back(a);
00376 }
00377
00378 template<typename Ret, typename Arg>
00379 void PiecewisePolynomFunction<Ret(Arg)>::calculatePLinear(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00380 int N = x.size();
00381
00382 breaks.resize(N);
00383 breaks = x;
00384
00385 fmatvec::Mat m(N-1,f.cols(),fmatvec::INIT,0.0);
00386 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00387 for(int i=1;i<N;i++) {
00388 m.row(i-1) = (f.row(i)-f.row(i-1))/(x(i)-x(i-1));
00389 a.row(i-1) = f.row(i-1);
00390 }
00391 coefs.push_back(m);
00392 coefs.push_back(a);
00393 }
00394
00395 template<typename Ret, typename Arg>
00396 Ret PiecewisePolynomFunction<Ret(Arg)>::ZerothDerivative::operator()(const Arg& x_) {
00397 double x = ToDouble<Arg>::cast(x_);
00398 if(x>(parent->breaks)(parent->nPoly))
00399 throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00400 if(x<(parent->breaks)(0))
00401 throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", lower bound= "+numtostr((parent->breaks)(0)));
00402
00403 if ((fabs(x-xSave)<macheps()) && !firstCall)
00404 return FromVecV<Ret>::cast(ySave);
00405 else {
00406 firstCall = false;
00407 if(x<(parent->breaks)(parent->index))
00408 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00409 (parent->index)--;
00410 else if(x>(parent->breaks)((parent->index)+1)) {
00411 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00412 (parent->index)++;
00413 (parent->index)--;
00414 }
00415
00416 const double dx = x - (parent->breaks)(parent->index);
00417 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
00418 for(int i=1;i<=(parent->order);i++)
00419 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
00420 xSave=x;
00421 ySave=yi;
00422 return FromVecV<Ret>::cast(yi);
00423 }
00424 }
00425
00426 template<typename Ret, typename Arg>
00427 Ret PiecewisePolynomFunction<Ret(Arg)>::FirstDerivative::operator()(const Arg& x_) {
00428 double x = ToDouble<Arg>::cast(x_);
00429 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00430 if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00431
00432 if ((fabs(x-xSave)<macheps()) && !firstCall)
00433 return FromVecV<Ret>::cast(ySave);
00434 else {
00435 firstCall = false;
00436 if(x<(parent->breaks)(parent->index))
00437 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00438 (parent->index)--;
00439 else if(x>(parent->breaks)((parent->index)+1)) {
00440 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00441 (parent->index)++;
00442 (parent->index)--;
00443 }
00444
00445 double dx = x - (parent->breaks)(parent->index);
00446 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
00447 for(int i=1;i<parent->order;i++)
00448 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
00449 xSave=x;
00450 ySave=yi;
00451 return FromVecV<Ret>::cast(yi);
00452 }
00453 }
00454
00455 template<typename Ret, typename Arg>
00456 Ret PiecewisePolynomFunction<Ret(Arg)>::SecondDerivative::operator()(const Arg& x_) {
00457 double x = ToDouble<Arg>::cast(x_);
00458 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00459 if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00460
00461 if ((fabs(x-xSave)<macheps()) && !firstCall)
00462 return FromVecV<Ret>::cast(ySave);
00463 else {
00464 firstCall = false;
00465 if(x<(parent->breaks)(parent->index))
00466 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00467 (parent->index)--;
00468 else if(x>(parent->breaks)((parent->index)+1)) {
00469 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00470 (parent->index)++;
00471 (parent->index)--;
00472 }
00473
00474 double dx = x - (parent->breaks)(parent->index);
00475 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
00476 for(int i=1;i<=((parent->order)-2);i++)
00477 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
00478 xSave=x;
00479 ySave=yi;
00480 return FromVecV<Ret>::cast(yi);
00481 }
00482 }
00483
00484 template<typename Ret, typename Arg>
00485 void PiecewisePolynomFunction<Ret(Arg)>::initializeUsingXML(xercesc::DOMElement * element) {
00486 xercesc::DOMElement *e;
00487 fmatvec::VecV x;
00488 fmatvec::MatV y;
00489 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
00490 if (e) {
00491 x=Element::getVec(e);
00492 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
00493 y=Element::getMat(e, x.size(), 0);
00494 }
00495 else {
00496 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xy");
00497 fmatvec::MatV xy=Element::getMat(e);
00498 assert(xy.cols()>1);
00499 x=xy.col(0);
00500 y=xy(fmatvec::Index(0, xy.rows()-1), fmatvec::Index(1, xy.cols()-1));
00501 }
00502 InterpolationMethod method=cSplineNatural;
00503 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethod");
00504 if(e) {
00505 std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
00506 if(str=="\"cSplinePeriodic\"") method=cSplinePeriodic;
00507 else if(str=="\"cSplineNatural\"") method=cSplineNatural;
00508 else if(str=="\"piecewiseLinear\"") method=piecewiseLinear;
00509 }
00510 setXF(x, y, method);
00511 }
00512
00513
00514 }
00515
00516 #endif
00517