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 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        * \brief calculation of piecewise linear interpolation
00173        * \param interpolated arguments
00174        * \param interpolated function values
00175        *
00176        * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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     // Matrix C and vector rs C*c=rs
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     // last but one row
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     // last row
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     // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
00278     fmatvec::Mat c = slvLU(C,rs);
00279     fmatvec::Mat ctmp(N,f.cols());
00280     ctmp.row(N-1) = c.row(0); // CN = c1
00281     ctmp(0,0,N-2,f.cols()-1)= c;
00282 
00283     // vector ordering of the further coefficients
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     // first row
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     // last row
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     // solve C*c = rs with C tridiagonal
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; // C_rs=[C rs] for Gauss in matrix
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); // C_rs -> upper triangular matrix C1 -> C1 rs1
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--) { // backward substitution
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; // c1=cN=0 natural splines c=[ 0; c; 0]
00349 
00350     // vector ordering of the further coefficients
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(); // number of supporting points
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)); // slope
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)) // saved index still OK? otherwise search downwards
00399         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00400           (parent->index)--;
00401       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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); // local coordinate
00408       fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
00409       for(int i=1;i<=(parent->order);i++) // Horner scheme
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)) // saved index still OK? otherwise search downwards
00427         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00428           (parent->index)--;
00429       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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)) // saved index still OK? otherwise search downwards
00455         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00456           (parent->index)--;
00457       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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 /* PPOLYNOM */
00505