MBSim

Project Page
ppolynom.h
00001 /* Copyright (C) 2004-2009 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
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        * \brief calculation of piecewise linear interpolation
00156        * \param interpolated arguments
00157        * \param interpolated function values
00158        *
00159        * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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     // Matrix C and vector rs C*c=rs
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     // last but one row
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     // last row
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     // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
00289     fmatvec::Mat c = slvLU(C,rs);
00290     fmatvec::Mat ctmp(N,f.cols());
00291     ctmp.row(N-1) = c.row(0); // CN = c1
00292     ctmp(0,0,N-2,f.cols()-1)= c;
00293 
00294     // vector ordering of the further coefficients
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     // first row
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     // last row
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     // solve C*c = rs with C tridiagonal
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; // C_rs=[C rs] for Gauss in matrix
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); // C_rs -> upper triangular matrix C1 -> C1 rs1
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--) { // backward substitution
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; // c1=cN=0 natural splines c=[ 0; c; 0]
00360 
00361     // vector ordering of the further coefficients
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(); // number of supporting points
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)); // slope
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)) // saved index still OK? otherwise search downwards
00410         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00411           (parent->index)--;
00412       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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); // local coordinate
00419       fmatvec::Vector<Row,double> yi = trans(((parent->coefs)[0]).row(parent->index));
00420       for(int i=1;i<=(parent->order);i++) // Horner scheme
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)) // saved index still OK? otherwise search downwards
00438         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00439           (parent->index)--;
00440       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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)) // saved index still OK? otherwise search downwards
00466         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00467           (parent->index)--;
00468       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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 /* PPOLYNOM */
00517