00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef PPOLYNOM
00021 #define PPOLYNOM
00022
00023 #include "fmatvec/fmatvec.h"
00024 #include "mbsim/utils/function.h"
00025 #include "mbsim/utils/utils.h"
00026 #include "mbsim/utils/eps.h"
00027 #include "mbsim/mbsim_event.h"
00028
00029 namespace MBSim {
00030
00060 template<class Row, class Col>
00061 class PPolynom : public DifferentiableFunction1<fmatvec::Vector<Col,double> > {
00062 public:
00066 PPolynom() : DifferentiableFunction1<fmatvec::Vector<Col,double> >() {
00067 this->addDerivative(new PPolynom<Row,Col>::ZerothDerivative(this));
00068 this->addDerivative(new PPolynom<Row,Col>::FirstDerivative(this));
00069 this->addDerivative(new PPolynom<Row,Col>::SecondDerivative(this));
00070 }
00071
00075 virtual ~PPolynom() {}
00076
00089 void setXF(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f, std::string InterpolationMethod);
00090
00094 std::vector<fmatvec::Matrix<fmatvec::General,Row,Col,double> > getCoefs();
00095
00099 fmatvec::Vector<Row,double> getBreaks();
00100
00106 void setPP(const std::vector<fmatvec::Matrix<fmatvec::General,Row,Col,double> > &coefs_u, const fmatvec::Vec &breaks_u);
00107
00112 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00113
00114 protected:
00118 std::vector<fmatvec::Matrix<fmatvec::General,Row,Col,double> > coefs;
00119
00123 fmatvec::Vec breaks;
00124
00128 int nPoly;
00129
00133 int order;
00134
00138 int index;
00139
00145 void calculateSplinePeriodic(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f);
00146
00152 void calculateSplineNatural(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f);
00153
00154
00155
00156
00157
00158
00159
00160
00161 void calculatePLinear(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f);
00162
00166 class ZerothDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00167 public:
00168 ZerothDerivative(PPolynom<Row,Col> *polynom) : Function1<fmatvec::Vector<Col,double>,double>(), parent(polynom), xSave(0), ySave(), firstCall(true) {}
00169 virtual ~ZerothDerivative() {}
00170
00171
00172 fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00173
00174
00175 private:
00176 PPolynom<Row,Col> *parent;
00177 double xSave;
00178 fmatvec::Vector<Col,double> ySave;
00179 bool firstCall;
00180 };
00181
00185 class FirstDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00186 public:
00187 FirstDerivative(PPolynom<Row,Col> *polynom) : Function1<fmatvec::Vector<Col,double>,double>(), parent(polynom), xSave(0), ySave(), firstCall(true) {}
00188 virtual ~FirstDerivative() {}
00189
00190
00191 fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00192
00193
00194 private:
00195 PPolynom<Row,Col> *parent;
00196 double xSave;
00197 fmatvec::Vector<Col,double> ySave;
00198 bool firstCall;
00199 };
00200
00204 class SecondDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00205 public:
00206 SecondDerivative(PPolynom<Row,Col> *polynom) : Function1<fmatvec::Vector<Col,double>,double>(), parent(polynom), xSave(0), ySave(), firstCall(true) {}
00207 virtual ~SecondDerivative() {}
00208
00209
00210 fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00211
00212
00213 private:
00214 PPolynom<Row,Col> *parent;
00215 double xSave;
00216 fmatvec::Vector<Col,double> ySave;
00217 bool firstCall;
00218 };
00219 };
00220
00221 template<class Row, class Col>
00222 inline std::vector<fmatvec::Matrix<fmatvec::General,Row,Col,double> > PPolynom<Row,Col>::getCoefs() { return coefs; }
00223 template<class Row, class Col>
00224 inline fmatvec::Vector<Row,double> PPolynom<Row,Col>::getBreaks() { return breaks; }
00225 template<class Row, class Col>
00226 inline void PPolynom<Row,Col>::setPP(const std::vector<fmatvec::Matrix<fmatvec::General,Row,Col,double> > &coefs_u, const fmatvec::Vec &breaks_u) {
00227 coefs = coefs_u;
00228 breaks = breaks_u;
00229 index = 0;
00230 nPoly = (coefs[0]).rows();
00231 order = coefs.size()-1;
00232 }
00233
00234 template<class Row, class Col>
00235 void PPolynom<Row,Col>::setXF(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f, std::string InterpolationMethod) {
00236 assert(x.size() == f.rows());
00237
00238 if(InterpolationMethod == "csplinePer") {
00239 calculateSplinePeriodic(x,f);
00240 }
00241 else if(InterpolationMethod == "csplineNat") {
00242 calculateSplineNatural(x,f);
00243 }
00244 else if(InterpolationMethod == "plinear") {
00245 calculatePLinear(x,f);
00246 }
00247 else throw MBSimError("ERROR (PPolynom::setXF): No valid method to calculate pp-form");
00248
00249 index = 0;
00250 nPoly = x.size()-1;
00251 order = coefs.size()-1;
00252 }
00253
00254 template<class Row, class Col>
00255 void PPolynom<Row,Col>::calculateSplinePeriodic(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f) {
00256 double hi, hii;
00257 int N = x.size();
00258 if(nrm2(f.row(0)-f.row(f.rows()-1))>epsroot()) throw MBSimError("ERROR (PPolynom::calculateSplinePeriodic): f(0)= "+numtostr(f.row(0))+"!="+numtostr(f.row(f.rows()-1))+" =f(end)");
00259 fmatvec::SqrMat C(N-1,fmatvec::INIT,0.0);
00260 fmatvec::Mat rs(N-1,f.cols(),fmatvec::INIT,0.0);
00261
00262
00263 for(int i=0; i<N-3;i++) {
00264 hi = x(i+1) - x(i);
00265 hii = x(i+2)-x(i+1);
00266 C(i,i) = hi;
00267 C(i,i+1) = 2*(hi+hii);
00268 C(i,i+2) = hii;
00269 rs.row(i) = 3.*((f.row(i+2)-f.row(i+1))/hii - (f.row(i+1)-f.row(i))/hi);
00270 }
00271
00272
00273 hi = x(N-2)-x(N-3);
00274 hii = x(N-1)-x(N-2);
00275 C(N-3,N-3) = hi;
00276 C(N-3,N-2)= 2*(hi+hii);
00277 C(N-3,0)= hii;
00278 rs.row(N-3) = 3.*((f.row(N-1)-f.row(N-2))/hii - (f.row(N-2)-f.row(N-3))/hi);
00279
00280
00281 double h1 = x(1)-x(0);
00282 double hN_1 = x(N-1)-x(N-2);
00283 C(N-2,0) = 2*(h1+hN_1);
00284 C(N-2,1) = h1;
00285 C(N-2,N-2)= hN_1;
00286 rs.row(N-2) = 3.*((f.row(1)-f.row(0))/h1 - (f.row(0)-f.row(N-2))/hN_1);
00287
00288
00289 fmatvec::Mat c = slvLU(C,rs);
00290 fmatvec::Mat ctmp(N,f.cols());
00291 ctmp.row(N-1) = c.row(0);
00292 ctmp(0,0,N-2,f.cols()-1)= c;
00293
00294
00295 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00296 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00297 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00298
00299 for(int i=0; i<N-1; i++) {
00300 hi = x(i+1)-x(i);
00301 a.row(i) = f.row(i);
00302 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00303 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00304 }
00305
00306 breaks.resize(N);
00307 breaks = x;
00308 coefs.push_back(d);
00309 coefs.push_back(c);
00310 coefs.push_back(b);
00311 coefs.push_back(a);
00312 }
00313
00314 template<class Row, class Col>
00315 void PPolynom<Row,Col>::calculateSplineNatural(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f) {
00316
00317 int i=0;
00318 int N = x.size();
00319 fmatvec::SqrMat C(N-2,fmatvec::INIT,0.0);
00320 fmatvec::Mat rs(N-2,f.cols(),fmatvec::INIT,0.0);
00321 double hi = x(i+1)-x(i);
00322 double hii = x(i+2)-x(i+1);
00323 C(i,i) = 2*hi+2*hii;
00324 C(i,i+1) = hii;
00325 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00326
00327
00328 i = (N-3);
00329 hi = x(i+1)-x(i);
00330 hii = x(i+2)-x(i+1);
00331 C(i,i-1) = hi;
00332 C(i,i) = 2*hii + 2*hi;
00333 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00334
00335 for(i=1;i<N-3;i++) {
00336 hi = x(i+1)-x(i);
00337 hii = x(i+2)-x(i+1);
00338 C(i,i-1) = hi;
00339 C(i,i) = 2*(hi+hii);
00340 C(i,i+1) = hii;
00341 rs.row(i) = 3.*(f.row(i+2)-f.row(i+1))/hii - 3.*(f.row(i+1)-f.row(i))/hi;
00342 }
00343
00344
00345 fmatvec::Mat C_rs(N-2,N-1+f.cols(),fmatvec::INIT,0.0);
00346 C_rs(0,0,N-3,N-3) = C;
00347 C_rs(0,N-2,N-3,N-2+f.cols()-1) = rs;
00348 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);
00349 fmatvec::Mat rs1 = C_rs(0,N-2,N-3,N-2+f.cols()-1);
00350 fmatvec::Mat C1 = C_rs(0,0,N-3,N-3);
00351 fmatvec::Mat c(N-2,f.cols(),fmatvec::INIT,0.0);
00352 for(i=N-3;i>=0 ;i--) {
00353 fmatvec::RowVector<Col,double> sum_ciCi(f.cols(),fmatvec::NONINIT);
00354 sum_ciCi.init(0.);
00355 for(int ii=i+1; ii<=N-3; ii++) sum_ciCi = sum_ciCi + C1(i,ii)*c.row(ii);
00356 c.row(i)= (rs1.row(i) - sum_ciCi)/C1(i,i);
00357 }
00358 fmatvec::Mat ctmp(N,f.cols(),fmatvec::INIT,0.0);
00359 ctmp(1,0,N-2,f.cols()-1) = c;
00360
00361
00362 fmatvec::Mat d(N-1,f.cols(),fmatvec::INIT,0.0);
00363 fmatvec::Mat b(N-1,f.cols(),fmatvec::INIT,0.0);
00364 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00365
00366 for(i=0; i<N-1; i++) {
00367 hi = x(i+1)-x(i);
00368 a.row(i) = f.row(i);
00369 d.row(i) = (ctmp.row(i+1) - ctmp.row(i) ) / 3. / hi;
00370 b.row(i) = (f.row(i+1)-f.row(i)) / hi - (ctmp.row(i+1) + 2.*ctmp.row(i) ) / 3. * hi;
00371 }
00372
00373 breaks.resize(N);
00374 breaks = x;
00375 coefs.push_back(d);
00376 coefs.push_back(ctmp(0,0,N-2,f.cols()-1));
00377 coefs.push_back(b);
00378 coefs.push_back(a);
00379 }
00380
00381 template<class Row, class Col>
00382 void PPolynom<Row,Col>::calculatePLinear(const fmatvec::Vector<Row,double> &x, const fmatvec::Matrix<fmatvec::General,Row,Col,double> &f) {
00383 int N = x.size();
00384
00385 breaks.resize(N);
00386 breaks = x;
00387
00388 fmatvec::Mat m(N-1,f.cols(),fmatvec::INIT,0.0);
00389 fmatvec::Mat a(N-1,f.cols(),fmatvec::INIT,0.0);
00390 for(int i=1;i<N;i++) {
00391 m.row(i-1) = (f.row(i)-f.row(i-1))/(x(i)-x(i-1));
00392 a.row(i-1) = f.row(i-1);
00393 }
00394 coefs.push_back(m);
00395 coefs.push_back(a);
00396 }
00397
00398 template<class Row, class Col>
00399 fmatvec::Vector<Col,double> PPolynom<Row,Col>::ZerothDerivative::operator()(const double& x, const void *) {
00400 if(x>(parent->breaks)(parent->nPoly))
00401 throw MBSimError("ERROR (PPolynom::operator()): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00402 if(x<(parent->breaks)(0))
00403 throw MBSimError("ERROR (PPolynom::operator()): x out of range! x= "+numtostr(x)+", lower bound= "+numtostr((parent->breaks)(0)));
00404
00405 if ((fabs(x-xSave)<macheps()) && !firstCall)
00406 return ySave;
00407 else {
00408 firstCall = false;
00409 if(x<(parent->breaks)(parent->index))
00410 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00411 (parent->index)--;
00412 else if(x>(parent->breaks)((parent->index)+1)) {
00413 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00414 (parent->index)++;
00415 (parent->index)--;
00416 }
00417
00418 const double dx = x - (parent->breaks)(parent->index);
00419 fmatvec::Vector<Row,double> yi = trans(((parent->coefs)[0]).row(parent->index));
00420 for(int i=1;i<=(parent->order);i++)
00421 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index));
00422 xSave=x;
00423 ySave=yi;
00424 return yi;
00425 }
00426 }
00427
00428 template<class Row, class Col>
00429 fmatvec::Vector<Col,double> PPolynom<Row,Col>::FirstDerivative::operator()(const double& x, const void *) {
00430 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("ERROR (PPolynom::diff1): x out of range! x= "+numtostr(x)+", upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00431 if(x<(parent->breaks)(0)) throw MBSimError("ERROR (PPolynom::diff1): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00432
00433 if ((fabs(x-xSave)<macheps()) && !firstCall)
00434 return ySave;
00435 else {
00436 firstCall = false;
00437 if(x<(parent->breaks)(parent->index))
00438 while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00439 (parent->index)--;
00440 else if(x>(parent->breaks)((parent->index)+1)) {
00441 while((parent->index) < (parent->nPoly) && (parent->breaks)(parent->index) <= x)
00442 (parent->index)++;
00443 (parent->index)--;
00444 }
00445
00446 double dx = x - (parent->breaks)(parent->index);
00447 fmatvec::Vector<Row,double> yi = trans(((parent->coefs)[0]).row(parent->index))*double(parent->order);
00448 for(int i=1;i<parent->order;i++)
00449 yi = yi*dx+trans(((parent->coefs)[i]).row(parent->index))*double((parent->order)-i);
00450 xSave=x;
00451 ySave=yi;
00452 return yi;
00453 }
00454 }
00455
00456 template<class Row, class Col>
00457 fmatvec::Vector<Col,double> PPolynom<Row,Col>::SecondDerivative::operator()(const double& x, const void *) {
00458 if(x>(parent->breaks)(parent->nPoly)) throw MBSimError("ERROR (PPolynom::diff2): x out of range! x= "+numtostr(x)+" upper bound= "+numtostr((parent->breaks)(parent->nPoly)));
00459 if(x<(parent->breaks)(0)) throw MBSimError("ERROR (PPolynom::diff2): x out of range! x= "+numtostr(x)+" lower bound= "+numtostr((parent->breaks)(0)));
00460
00461 if ((fabs(x-xSave)<macheps()) && !firstCall)
00462 return 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::Vector<Row,double> 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 yi;
00481 }
00482 }
00483
00484 template<class Row, class Col>
00485 void PPolynom<Row,Col>::initializeUsingXML(MBXMLUtils::TiXmlElement * element) {
00486 DifferentiableFunction1<fmatvec::Vector<Col,double> >::initializeUsingXML(element);
00487 MBXMLUtils::TiXmlElement * e;
00488 fmatvec::Vector<Row,double> x;
00489 fmatvec::Matrix<fmatvec::General,Row,Col,double> y;
00490 e=element->FirstChildElement(MBSIMNS"x");
00491 if (e) {
00492 x=Element::getVec(e);
00493 e=element->FirstChildElement(MBSIMNS"y");
00494 y=Element::getMat(e, x.size(), 0);
00495 }
00496 else {
00497 e=element->FirstChildElement(MBSIMNS"xy");
00498 fmatvec::Matrix<fmatvec::General,Row,Col,double> xy=Element::getMat(e);
00499 assert(xy.cols()>1);
00500 x=xy.col(0);
00501 y=xy(fmatvec::Index(0, xy.rows()-1), fmatvec::Index(1, xy.cols()-1));
00502 }
00503 std::string method;
00504 if (element->FirstChildElement(MBSIMNS"cSplinePeriodic"))
00505 method="csplinePer";
00506 else if (element->FirstChildElement(MBSIMNS"cSplineNatural"))
00507 method="csplineNat";
00508 else if (element->FirstChildElement(MBSIMNS"piecewiseLinear"))
00509 method="plinear";
00510 setXF(x, y, method);
00511 }
00512
00513
00514 }
00515
00516 #endif
00517