MBSim

Project Page
piecewise_polynom_function.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 "mbsim/functions/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 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        * \brief calculation of piecewise linear interpolation
00181        * \param interpolated arguments
00182        * \param interpolated function values
00183        *
00184        * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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           /* INHERITED INTERFACE OF FUNCTION */
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     // Matrix C and vector rs C*c=rs
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     // last but one row
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     // last row
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     // solve C*c = rs -> TODO BETTER: RANK-1-MODIFICATION FOR LINEAR EFFORT (Simeon - Numerik 1)
00286     fmatvec::Mat c = slvLU(C,rs);
00287     fmatvec::Mat ctmp(N,f.cols());
00288     ctmp.row(N-1) = c.row(0); // CN = c1
00289     ctmp(0,0,N-2,f.cols()-1)= c;
00290 
00291     // vector ordering of the further coefficients
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     // first row
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     // last row
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     // solve C*c = rs with C tridiagonal
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; // C_rs=[C rs] for Gauss in matrix
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); // C_rs -> upper triangular matrix C1 -> C1 rs1
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--) { // backward substitution
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; // c1=cN=0 natural splines c=[ 0; c; 0]
00357 
00358     // vector ordering of the further coefficients
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(); // number of supporting points
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)); // slope
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)) // saved index still OK? otherwise search downwards
00408         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00409           (parent->index)--;
00410       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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); // local coordinate
00417       fmatvec::VecV yi = trans(((parent->coefs)[0]).row(parent->index));
00418       for(int i=1;i<=(parent->order);i++) // Horner scheme
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)) // saved index still OK? otherwise search downwards
00437         while((parent->index) > 0 && (parent->breaks)(parent->index) > x)
00438           (parent->index)--;
00439       else if(x>(parent->breaks)((parent->index)+1)) { // saved index still OK? otherwise search upwards
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)) // 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::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       str=str.substr(1,str.length()-2);
00507       if(str=="cSplinePeriodic") method=cSplinePeriodic;
00508       else if(str=="cSplineNatural") method=cSplineNatural;
00509       else if(str=="piecewiseLinear") method=piecewiseLinear;
00510     }
00511     setXF(x, y, method);
00512   }
00513 
00514 
00515 }
00516 
00517 #endif /* PPOLYNOM */
00518