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
00061 template<class Ret>
00062 class PiecewisePolynomFunction : public fmatvec::Function<Ret(double)> {
00063
00064 public:
00065 PiecewisePolynomFunction() : f(this), fd(this), fdd(this) { }
00066
00067 Ret operator()(const double &x) { return f(x); }
00068 typename fmatvec::Der<Ret, double>::type parDer(const double &x) { return fd(x); }
00069 typename fmatvec::Der<typename fmatvec::Der<Ret, double>::type, double>::type parDerParDer(const double &x) { return fdd(x); }
00070
00083 void setXF(const fmatvec::VecV &x, const fmatvec::MatV &f, std::string InterpolationMethod) {
00084 assert(x.size() == f.rows());
00085
00086 if(InterpolationMethod == "csplinePer") {
00087 calculateSplinePeriodic(x,f);
00088 }
00089 else if(InterpolationMethod == "csplineNat") {
00090 calculateSplineNatural(x,f);
00091 }
00092 else if(InterpolationMethod == "plinear") {
00093 calculatePLinear(x,f);
00094 }
00095 else throw MBSimError("(PiecewisePolynomFunction::setXF): No valid method to calculate pp-form");
00096
00097 index = 0;
00098 nPoly = x.size()-1;
00099 order = coefs.size()-1;
00100 }
00101
00105 std::vector<fmatvec::MatV> getCoefs() { return coefs; }
00106
00110 fmatvec::VecV getBreaks() { return breaks; }
00111
00117 void setPP(const std::vector<fmatvec::MatV> &coefs_u, const fmatvec::Vec &breaks_u) {
00118 coefs = coefs_u;
00119 breaks = breaks_u;
00120 index = 0;
00121 nPoly = (coefs[0]).rows();
00122 order = coefs.size()-1;
00123 }
00124
00129 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00130
00131 protected:
00135 std::vector<fmatvec::MatV> coefs;
00136
00140 fmatvec::Vec breaks;
00141
00145 int nPoly;
00146
00150 int order;
00151
00155 int index;
00156
00162 void calculateSplinePeriodic(const fmatvec::VecV &x, const fmatvec::MatV &f);
00163
00169 void calculateSplineNatural(const fmatvec::VecV &x, const fmatvec::MatV &f);
00170
00171
00172
00173
00174
00175
00176
00177
00178 void calculatePLinear(const fmatvec::VecV &x, const fmatvec::MatV &f);
00179
00183 class ZerothDerivative {
00184 public:
00185 ZerothDerivative(PiecewisePolynomFunction<Ret> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00186 virtual ~ZerothDerivative() {}
00187
00188
00189 Ret operator()(const double &x);
00190
00191
00192 private:
00193 PiecewisePolynomFunction<Ret> *parent;
00194 double xSave;
00195 fmatvec::VecV ySave;
00196 bool firstCall;
00197 };
00198
00202 class FirstDerivative {
00203 public:
00204 FirstDerivative(PiecewisePolynomFunction<Ret> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00205 virtual ~FirstDerivative() {}
00206
00207
00208 Ret operator()(const double& x);
00209
00210
00211 private:
00212 PiecewisePolynomFunction<Ret> *parent;
00213 double xSave;
00214 fmatvec::VecV ySave;
00215 bool firstCall;
00216 };
00217
00221 class SecondDerivative {
00222 public:
00223 SecondDerivative(PiecewisePolynomFunction<Ret> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
00224 virtual ~SecondDerivative() {}
00225
00226
00227 Ret operator()(const double& x);
00228
00229
00230 private:
00231 PiecewisePolynomFunction<Ret> *parent;
00232 double xSave;
00233 fmatvec::VecV ySave;
00234 bool firstCall;
00235 };
00236
00237 private:
00238 ZerothDerivative f;
00239 FirstDerivative fd;
00240 SecondDerivative fdd;
00241 };
00242
00243 template<class Ret>
00244 void PiecewisePolynomFunction<Ret>::calculateSplinePeriodic(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00245 double hi, hii;
00246 int N = x.size();
00247 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)");
00248 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
00249 fmatvec::Mat rs(N-1,f.cols(),fmatvec::INIT,0.0);
00250
00251
00252 for(int i=0; i<N-3;i++) {
00253 hi = x(i+1) - x(i);
00254 hii = x(i+2)-x(i+1);
00255 C(i,i) = hi;
00256 C(i,i+1) = 2*(hi+hii);
00257 C(i,i+2) = hii;
00258 rs.row(i) = 3.*((f.row(i+2)-f.row(i+1))/hii - (f.row(i+1)-f.row(i))/hi);
00259 }
00260
00261
00262 hi = x(N-2)-x(N-3);
00263 hii = x(N-1)-x(N-2);
00264 C(N-3,N-3) = hi;
00265 C(N-3,N-2)= 2*(hi+hii);
00266 C(N-3,0)= hii;
00267 rs.row(N-3) = 3.*((f.row(N-1)-f.row(N-2))/hii - (f.row(N-2)-f.row(N-3))/hi);
00268
00269
00270 double h1 = x(1)-x(0);
00271 double hN_1 = x(N-1)-x(N-2);
00272 C(N-2,0) = 2*(h1+hN_1);
00273 C(N-2,1) = h1;
00274 C(N-2,N-2)= hN_1;
00275 rs.row(N-2) = 3.*((f.row(1)-f.row(0))/h1 - (f.row(0)-f.row(N-2))/hN_1);
00276
00277
00278 fmatvec::Mat c = slvLU(C,rs);
00279 fmatvec::Mat ctmp(N,f.cols());
00280 ctmp.row(N-1) = c.row(0);
00281 ctmp(0,0,N-2,f.cols()-1)= c;
00282
00283
00284 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00285 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00286 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00287
00288 for(int i=0; i<N-1; i++) {
00289 hi = x(i+1)-x(i);
00290 a.row(i) = f.row(i);
00291 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00292 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00293 }
00294
00295 breaks.resize(N);
00296 breaks = x;
00297 coefs.push_back(d);
00298 coefs.push_back(c);
00299 coefs.push_back(b);
00300 coefs.push_back(a);
00301 }
00302
00303 template<class Ret>
00304 void PiecewisePolynomFunction<Ret>::calculateSplineNatural(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00305
00306 int i=0;
00307 int N = x.size();
00308 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
00309 fmatvec::Mat rs(N-2,f.cols(),fmatvec::INIT,0.0);
00310 double hi = x(i+1)-x(i);
00311 double hii = x(i+2)-x(i+1);
00312 C(i,i) = 2*hi+2*hii;
00313 C(i,i+1) = hii;
00314 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00315
00316
00317 i = (N-3);
00318 hi = x(i+1)-x(i);
00319 hii = x(i+2)-x(i+1);
00320 C(i,i-1) = hi;
00321 C(i,i) = 2*hii + 2*hi;
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 for(i=1;i<N-3;i++) {
00325 hi = x(i+1)-x(i);
00326 hii = x(i+2)-x(i+1);
00327 C(i,i-1) = hi;
00328 C(i,i) = 2*(hi+hii);
00329 C(i,i+1) = hii;
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
00333
00334 fmatvec::Mat C_rs(N-2,N-1+f.cols(),fmatvec::INIT,0.0);
00335 C_rs(0,0,N-3,N-3) = C;
00336 C_rs(0,N-2,N-3,N-2+f.cols()-1) = rs;
00337 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);
00338 fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+f.cols()-1);
00339 fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
00340 fmatvec::Mat c(N-2,f.cols(),fmatvec::INIT,0.0);
00341 for(i=N-3;i>=0 ;i--) {
00342 fmatvec::RowVecV sum_ciCi(f.cols(),fmatvec::NONINIT);
00343 sum_ciCi.init(0.);
00344 for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
00345 c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
00346 }
00347 fmatvec::Mat ctmp(N,f.cols(),fmatvec::INIT,0.0);
00348 ctmp(1,0,N-2,f.cols()-1) = c;
00349
00350
00351 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00352 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00353 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00354
00355 for(i=0; i<N-1; i++) {
00356 hi = x(i+1)-x(i);
00357 a.row(i) = f.row(i);
00358 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00359 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00360 }
00361
00362 breaks.resize(N);
00363 breaks = x;
00364 coefs.push_back(d);
00365 coefs.push_back(ctmp(0,0,N-2,f.cols()-1));
00366 coefs.push_back(b);
00367 coefs.push_back(a);
00368 }
00369
00370 template<class Ret>
00371 void PiecewisePolynomFunction<Ret>::calculatePLinear(const fmatvec::VecV &x, const fmatvec::MatV &f) {
00372 int N = x.size();
00373
00374 breaks.resize(N);
00375 breaks = x;
00376
00377 fmatvec::Mat m(N-1,f.cols(),fmatvec::INIT,0.0);
00378 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00379 for(int i=1;i<N;i++) {
00380 m.row(i-1) = (f.row(i)-f.row(i-1))/(x(i)-x(i-1));
00381 a.row(i-1) = f.row(i-1);
00382 }
00383 coefs.push_back(m);
00384 coefs.push_back(a);
00385 }
00386
00387 template<class Ret>
00388 Ret PiecewisePolynomFunction<Ret>::ZerothDerivative::operator()(const double& x) {
00389 if(x>(parent->breaks)(parent->nPoly))
00390 throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00391 if(x<(parent->breaks)(0))
00392 throw MBSimError("(PiecewisePolynomFunction::operator()): x out of range! x= "+numtostr(x)+", lower bound= "+numtostr((parent->breaks)(0)));
00393
00394 if ((fabs(x-xSave)<macheps()) && !firstCall)
00395 return FromVecV<Ret>::cast(ySave);
00396 else {
00397 firstCall = false;
00398 if(x<(parent->breaks)(parent->index))
00399 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00400 (parent->index)--;
00401 else if(x>(parent->breaks)((parent->index)+1)) {
00402 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00403 (parent->index)++;
00404 (parent->index)--;
00405 }
00406
00407 const double dx = x - (parent->breaks)(parent->index);
00408 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
00409 for(int i=1;i<=(parent->order);i++)
00410 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
00411 xSave=x;
00412 ySave=yi;
00413 return FromVecV<Ret>::cast(yi);
00414 }
00415 }
00416
00417 template<class Ret>
00418 Ret PiecewisePolynomFunction<Ret>::FirstDerivative::operator()(const double& x) {
00419 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00420 if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff1): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00421
00422 if ((fabs(x-xSave)<macheps()) && !firstCall)
00423 return FromVecV<Ret>::cast(ySave);
00424 else {
00425 firstCall = false;
00426 if(x<(parent->breaks)(parent->index))
00427 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00428 (parent->index)--;
00429 else if(x>(parent->breaks)((parent->index)+1)) {
00430 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00431 (parent->index)++;
00432 (parent->index)--;
00433 }
00434
00435 double dx = x - (parent->breaks)(parent->index);
00436 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
00437 for(int i=1;i<parent->order;i++)
00438 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
00439 xSave=x;
00440 ySave=yi;
00441 return FromVecV<Ret>::cast(yi);
00442 }
00443 }
00444
00445 template<class Ret>
00446 Ret PiecewisePolynomFunction<Ret>::SecondDerivative::operator()(const double& x) {
00447 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00448 if(x<(parent->breaks)(0)) throw MBSimError("(PiecewisePolynomFunction::diff2): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00449
00450 if ((fabs(x-xSave)<macheps()) && !firstCall)
00451 return FromVecV<Ret>::cast(ySave);
00452 else {
00453 firstCall = false;
00454 if(x<(parent->breaks)(parent->index))
00455 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00456 (parent->index)--;
00457 else if(x>(parent->breaks)((parent->index)+1)) {
00458 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00459 (parent->index)++;
00460 (parent->index)--;
00461 }
00462
00463 double dx = x - (parent->breaks)(parent->index);
00464 fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order)*double((parent->order)-1);
00465 for(int i=1;i<=((parent->order)-2);i++)
00466 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i)*double((parent->order)-i-1);
00467 xSave=x;
00468 ySave=yi;
00469 return FromVecV<Ret>::cast(yi);
00470 }
00471 }
00472
00473 template<class Ret>
00474 void PiecewisePolynomFunction<Ret>::initializeUsingXML(MBXMLUtils::TiXmlElement * element) {
00475 MBXMLUtils::TiXmlElement *e;
00476 fmatvec::VecV x;
00477 fmatvec::MatV y;
00478 e=element->FirstChildElement(MBSIMNS"x");
00479 if (e) {
00480 x=Element::getVec(e);
00481 e=element->FirstChildElement(MBSIMNS"y");
00482 y=Element::getMat(e, x.size(), 0);
00483 }
00484 else {
00485 e=element->FirstChildElement(MBSIMNS"xy");
00486 fmatvec::MatV xy=Element::getMat(e);
00487 assert(xy.cols()>1);
00488 x=xy.col(0);
00489 y=xy(fmatvec::Index(0, xy.rows()-1), fmatvec::Index(1, xy.cols()-1));
00490 }
00491 std::string method;
00492 if (element->FirstChildElement(MBSIMNS"cSplinePeriodic"))
00493 method="csplinePer";
00494 else if (element->FirstChildElement(MBSIMNS"cSplineNatural"))
00495 method="csplineNat";
00496 else if (element->FirstChildElement(MBSIMNS"piecewiseLinear"))
00497 method="plinear";
00498 setXF(x, y, method);
00499 }
00500
00501
00502 }
00503
00504 #endif
00505