MBSim

Project Page
function_library.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: markus.ms.schneider@gmail.com
00018  */
00019 
00020 #ifndef _FUNCTION_LIBRARY_H_
00021 #define _FUNCTION_LIBRARY_H_
00022 
00023 #include "mbsim/utils/function.h"
00024 #include "mbsim/objectfactory.h"
00025 
00026 namespace MBSim {
00027 
00028   class Function1_SS_from_VS : public Function1<double, double> {
00029     public:
00030       Function1_SS_from_VS() : fun(NULL) {}
00031       Function1_SS_from_VS(Function1<fmatvec::Vec, double> * fun_) : fun(fun_) {assert((*fun)(0).size()==1); }
00032       void setFunction(Function1<fmatvec::Vec, double> * fun_) {fun=fun_; assert((*fun)(0).size()==1); }
00033       double operator()(const double& x, const void * =NULL) {return (*fun)(x)(0); }
00034       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00035     private:
00036       Function1<fmatvec::Vec, double> * fun;
00037   };
00038 
00039   template<class Col>
00040   class Function1_VS_from_SS : public Function1<fmatvec::Vector<Col,double>, double> {
00041     public:
00042       Function1_VS_from_SS() : fun(NULL), vec(0) {}
00043       Function1_VS_from_SS(Function1<double, double> * fun_, fmatvec::Vector<Col,double> v) : fun(fun_), vec(v) {vec/=nrm2(v); }
00044       void setFunction(Function1<double, double> * fun_) {fun=fun_; }
00045       void setVector(fmatvec::Vector<Col,double> v) {vec=v; vec/=nrm2(v); }
00046       fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL) {return (*fun)(x)*vec; }
00047       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00048     private:
00049       Function1<double, double> * fun;
00050       fmatvec::Vector<Col,double> vec;
00051   };
00052 
00053 
00060   template<class Col>
00061   class QuadraticFunction1_VS : public DifferentiableFunction1<fmatvec::Vector<Col,double> > {
00062     public:
00063       QuadraticFunction1_VS();
00064       QuadraticFunction1_VS(fmatvec::Vector<Col,double> a0_, fmatvec::Vector<Col,double> a1_, fmatvec::Vector<Col,double> a2_);
00065       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00066 
00067       class ZerothDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00068          public:
00069           ZerothDerivative(QuadraticFunction1_VS *f) : Function1<fmatvec::Vector<Col,double>,double>(), parent(f) {}
00070           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00071         private:
00072           QuadraticFunction1_VS *parent;
00073       };
00074 
00075       class FirstDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00076          public:
00077           FirstDerivative(QuadraticFunction1_VS *f) : Function1<fmatvec::Vector<Col,double>,double>(), parent(f) {}
00078           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00079         private:
00080           QuadraticFunction1_VS *parent;
00081       };
00082       
00083       class SecondDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00084          public:
00085           SecondDerivative(QuadraticFunction1_VS *f) : Function1<fmatvec::Vector<Col,double>,double>(), parent(f) {}
00086           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00087         private:
00088           QuadraticFunction1_VS *parent;
00089       };
00090     protected:
00091       int ySize;
00092       fmatvec::Vector<Col,double> a0, a1, a2;
00093     private:
00094   };
00095 
00096 
00103   template<class Col>
00104   class SinusFunction1_VS : public DifferentiableFunction1<fmatvec::Vector<Col,double> > {
00105     public:
00106       SinusFunction1_VS();
00107       SinusFunction1_VS(fmatvec::Vector<Col,double>, fmatvec::Vector<Col,double> frequency_, fmatvec::Vector<Col,double> phase_, fmatvec::Vector<Col,double> offset_);
00108       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00109       MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00110       std::string getType() const { return "SinusFunction1_VS"; }
00111 
00112       class ZerothDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00113          public:
00114           ZerothDerivative(SinusFunction1_VS *sin) : Function1<fmatvec::Vector<Col,double>,double>(), parent(sin) {}
00115           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00116         private:
00117           SinusFunction1_VS *parent;
00118       };
00119 
00120       class FirstDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00121          public:
00122           FirstDerivative(SinusFunction1_VS *sin) : Function1<fmatvec::Vector<Col,double>,double>(), parent(sin) {}
00123           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00124         private:
00125           SinusFunction1_VS *parent;
00126       };
00127       
00128       class SecondDerivative : public Function1<fmatvec::Vector<Col,double>,double> {
00129          public:
00130           SecondDerivative(SinusFunction1_VS *sin) : Function1<fmatvec::Vector<Col,double>,double>(), parent(sin) {}
00131           fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL);
00132         private:
00133           SinusFunction1_VS *parent;
00134       };
00135     protected:
00136       int ySize;
00137       fmatvec::Vector<Col,double> amplitude, frequency, phase, offset;
00138     private:
00139       void check();
00140   };
00141 
00142 
00143   class PositiveSinusFunction1_VS : public SinusFunction1_VS<fmatvec::Ref> {
00144     public:
00145       PositiveSinusFunction1_VS() {}
00146       PositiveSinusFunction1_VS(fmatvec::Vec amplitude, fmatvec::Vec frequency, fmatvec::Vec phase, fmatvec::Vec offset) : SinusFunction1_VS<fmatvec::Ref>(amplitude, frequency, phase, offset) {}
00147       fmatvec::Vec operator()(const double& tVal, const void * =NULL);
00148       void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00149         SinusFunction1_VS<fmatvec::Ref>::initializeUsingXML(element);
00150       }
00151   };
00152 
00153 
00154   class StepFunction1_VS : public Function1<fmatvec::Vec, double> {
00155     public:
00156       StepFunction1_VS() {}
00157       StepFunction1_VS(fmatvec::Vec stepTime_, fmatvec::Vec stepSize_) : stepTime(stepTime_), stepSize(stepSize_) {
00158         check();
00159       }
00160       fmatvec::Vec operator()(const double& tVal, const void * =NULL);
00161       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00162     private:
00163       fmatvec::Vec stepTime, stepSize;
00164       int ySize;
00165       void check();
00166   };
00167 
00168 
00169   template<class Row, class Col>
00170   class TabularFunction1_VS : public Function1<fmatvec::Vector<Col,double>, double> {
00171     public:
00172       TabularFunction1_VS() : xIndexOld(0) {}
00173       TabularFunction1_VS(fmatvec::Vector<Row,double> x_, fmatvec::Matrix<fmatvec::General,Row,Col,double> y_) : x(x_), y(y_), xIndexOld(0) {
00174         check();
00175       }
00176       fmatvec::Vector<Col,double> operator()(const double& xVal, const void * =NULL);
00177       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00178     protected:
00179       fmatvec::Vector<Row,double> x;
00180       fmatvec::Matrix<fmatvec::General,Row,Col,double> y;
00181     private:
00182       int xIndexOld, xSize;
00183       void check();
00184   };
00185 
00186 
00187   class PeriodicTabularFunction1_VS : public TabularFunction1_VS<fmatvec::Ref,fmatvec::Ref> {
00188     public:
00189       PeriodicTabularFunction1_VS() {}
00190       PeriodicTabularFunction1_VS(fmatvec::Vec x_, fmatvec::Mat y_) : TabularFunction1_VS<fmatvec::Ref,fmatvec::Ref>(x_, y_) {
00191         check();
00192       }
00193       fmatvec::Vec operator()(const double& xVal, const void * =NULL);
00194       void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00195         TabularFunction1_VS<fmatvec::Ref,fmatvec::Ref>::initializeUsingXML(element);
00196         check();
00197       }
00198     private:
00199       double xMin, xMax, xDelta;
00200       void check() {
00201         xMin=x(0);
00202         xMax=x(x.size()-1);
00203         xDelta=xMax-xMin;
00204       }
00205   };
00206 
00207 
00208   class SummationFunction1_VS : public Function1<fmatvec::Vec, double> {
00209     public:
00210       SummationFunction1_VS() : ySize(0) {};
00211       void addFunction(Function1<fmatvec::Vec, double> * function, double factor=1.) {
00212         if (!ySize)
00213           ySize=((*function)(0)).size();
00214         else
00215           assert (((*function)(0)).size()==ySize);
00216         functions.push_back(function);
00217         factors.push_back(factor);
00218       }
00219       fmatvec::Vec operator()(const double& tVal, const void * =NULL) {
00220         fmatvec::Vec y=factors[0]*(*(functions[0]))(tVal);
00221         for (unsigned int i=1; i<functions.size(); i++)
00222           y+=factors[i]*(*(functions[i]))(tVal);
00223         return y;
00224       }
00225       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00226     private:
00227       std::vector<Function1<fmatvec::Vec, double> *> functions;
00228       std::vector<double> factors;
00229       int ySize;
00230   };
00231 
00232 
00233   class TabularFunction2_SSS: public MBSim::Function2<double,double,double> {
00234     public:
00235       TabularFunction2_SSS();
00236       /* INHERITED INTERFACE OF FUNCTION2 */
00237       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00238       virtual double operator()(const double& x, const double& y, const void * = NULL);
00239       /***************************************************/
00240       /* GETTER / SETTER */
00241       void setXValues(fmatvec::Vec x_);
00242       void setYValues(fmatvec::Vec y_);
00243       void setXYMat(fmatvec::Mat XY_);
00244       /***************************************************/
00245 
00246     private:
00247       fmatvec::Vec xVec;
00248       fmatvec::Vec yVec;
00249       fmatvec::Mat XY;
00250 
00251       int xSize;
00252       int ySize;
00253       int x0Index,x1Index;
00254       int y0Index,y1Index;
00255 
00256       fmatvec::Vec func_value;
00257       fmatvec::Vec xy;
00258       fmatvec::Vec XYval;
00259       fmatvec::Mat XYfac;
00260 
00261       void calcIndex(const double * x, fmatvec::Vec X, int * xSize, int * xIndexMinus, int * xIndexPlus);
00262   };
00263 
00264 
00265   class Polynom1_SS : public MBSim::DifferentiableFunction1<double> {
00266     public:
00267       Polynom1_SS() {}
00268       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00269 
00270       class Polynom1_SSEvaluation : public MBSim::Function1<double,double> {
00271         public:
00272           Polynom1_SSEvaluation(fmatvec::Vec a_) : MBSim::Function1<double,double>(), a(a_) {}
00273           double operator()(const double& x, const void * =NULL);
00274         private:
00275           fmatvec::Vec a;
00276       };
00277 
00278       void setCoefficients(fmatvec::Vec a);
00279   };
00280 
00281 
00282   // ---------------------------------- Implementations ---------------------------------- 
00283 
00284   template<class Col>
00285   QuadraticFunction1_VS<Col>::QuadraticFunction1_VS() : DifferentiableFunction1<fmatvec::Vector<Col,double> >(), ySize(0), a0(), a1(), a2() {
00286     this->addDerivative(new QuadraticFunction1_VS::ZerothDerivative(this));
00287     this->addDerivative(new QuadraticFunction1_VS::FirstDerivative(this));
00288     this->addDerivative(new QuadraticFunction1_VS::SecondDerivative(this));
00289   }
00290 
00291   template<class Col>
00292   QuadraticFunction1_VS<Col>::QuadraticFunction1_VS(fmatvec::Vector<Col,double> a0_, fmatvec::Vector<Col,double> a1_, fmatvec::Vector<Col,double> a2_) : DifferentiableFunction1<fmatvec::Vector<Col,double> >(), a0(a0_), a1(a1_), a2(a2_) {
00293     addDerivative(new QuadraticFunction1_VS::ZerothDerivative(this));
00294     addDerivative(new QuadraticFunction1_VS::FirstDerivative(this));
00295     addDerivative(new QuadraticFunction1_VS::SecondDerivative(this));
00296     ySize=a0.size();
00297   }
00298   
00299   template<class Col>
00300   fmatvec::Vector<Col,double> QuadraticFunction1_VS<Col>::ZerothDerivative::operator()(const double& tVal, const void *) {
00301     fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00302     for (int i=0; i<parent->ySize; i++)
00303       y(i)=parent->a0(i)+tVal*(parent->a1(i)+parent->a2(i)*tVal);
00304     return y;
00305   }
00306 
00307   template<class Col>
00308   fmatvec::Vector<Col,double> QuadraticFunction1_VS<Col>::FirstDerivative::operator()(const double& tVal, const void *) {
00309     fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00310     for (int i=0; i<parent->ySize; i++)
00311       y(i)=parent->a1(i)+2.*parent->a2(i)*tVal;
00312     return y;
00313   }
00314 
00315   template<class Col>
00316   fmatvec::Vector<Col,double> QuadraticFunction1_VS<Col>::SecondDerivative::operator()(const double& tVal, const void *) {
00317     fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00318     for (int i=0; i<parent->ySize; i++)
00319       y(i)=2.*parent->a2(i);
00320     return y;
00321   }
00322 
00323   template<class Col>
00324   void QuadraticFunction1_VS<Col>::initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00325     DifferentiableFunction1<fmatvec::Vector<Col,double> >::initializeUsingXML(element);
00326     MBXMLUtils::TiXmlElement *e=element->FirstChildElement(MBSIMNS"a0");
00327     a0=Element::getVec(e);
00328     ySize=a0.size();
00329     e=element->FirstChildElement(MBSIMNS"a1");
00330     a1=Element::getVec(e, ySize);
00331     e=element->FirstChildElement(MBSIMNS"a2");
00332     a2=Element::getVec(e, ySize);
00333     
00334   }
00335 
00336   template<class Col>
00337   void Function1_VS_from_SS<Col>::initializeUsingXML(MBXMLUtils::TiXmlElement * element) {
00338     MBXMLUtils::TiXmlElement * e;
00339     e=element->FirstChildElement(MBSIMNS"function");
00340     Function1<double, double> * f=ObjectFactory<Function>::create<Function1<double,double> >(e->FirstChildElement());
00341     f->initializeUsingXML(e->FirstChildElement());
00342     setFunction(f);
00343     e=element->FirstChildElement(MBSIMNS"direction");
00344     setVector(Element::getVec(e));
00345   }
00346 
00347   template<class Col>
00348     SinusFunction1_VS<Col>::SinusFunction1_VS() : DifferentiableFunction1<fmatvec::Vector<Col,double> >(), ySize(0), amplitude(), frequency(), phase(), offset() {
00349       this->addDerivative(new SinusFunction1_VS::ZerothDerivative(this));
00350       this->addDerivative(new SinusFunction1_VS::FirstDerivative(this));
00351       this->addDerivative(new SinusFunction1_VS::SecondDerivative(this));
00352     }
00353 
00354   template<class Col>
00355     SinusFunction1_VS<Col>::SinusFunction1_VS(fmatvec::Vector<Col,double> amplitude_, fmatvec::Vector<Col,double> frequency_, fmatvec::Vector<Col,double> phase_, fmatvec::Vector<Col,double> offset_) : DifferentiableFunction1<fmatvec::Vector<Col,double> >(), amplitude(amplitude_), frequency(frequency_), phase(phase_), offset(offset_) {
00356       this->addDerivative(new SinusFunction1_VS::ZerothDerivative(this));
00357       this->addDerivative(new SinusFunction1_VS::FirstDerivative(this));
00358       this->addDerivative(new SinusFunction1_VS::SecondDerivative(this));
00359       check();
00360     }
00361 
00362   template<class Col>
00363     fmatvec::Vector<Col,double> SinusFunction1_VS<Col>::ZerothDerivative::operator()(const double& tVal, const void *) {
00364       fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00365       for (int i=0; i<parent->ySize; i++)
00366         y(i)=parent->offset(i)+parent->amplitude(i)*sin(2.*M_PI*parent->frequency(i)*tVal+parent->phase(i));
00367       return y;
00368     }
00369 
00370   template<class Col>
00371     fmatvec::Vector<Col,double> SinusFunction1_VS<Col>::FirstDerivative::operator()(const double& tVal, const void *) {
00372       fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00373       for (int i=0; i<parent->ySize; i++)
00374         y(i)=parent->amplitude(i)*2.*M_PI*parent->frequency(i)*cos(2.*M_PI*parent->frequency(i)*tVal+parent->phase(i));
00375       return y;
00376     }
00377 
00378   template<class Col>
00379     fmatvec::Vector<Col,double> SinusFunction1_VS<Col>::SecondDerivative::operator()(const double& tVal, const void *) {
00380       fmatvec::Vector<Col,double> y(parent->ySize, fmatvec::NONINIT);
00381       for (int i=0; i<parent->ySize; i++)
00382         y(i)=-parent->amplitude(i)*2.*M_PI*parent->frequency(i)*2.*M_PI*parent->frequency(i)*sin(2.*M_PI*parent->frequency(i)*tVal+parent->phase(i));
00383       return y;
00384     }
00385 
00386   template<class Col>
00387     void SinusFunction1_VS<Col>::initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00388       DifferentiableFunction1<fmatvec::Vector<Col,double> >::initializeUsingXML(element);
00389       MBXMLUtils::TiXmlElement *e=element->FirstChildElement(MBSIMNS"amplitude");
00390       fmatvec::Vector<Col,double> amplitude_=Element::getVec(e);
00391       amplitude=amplitude_;
00392       e=element->FirstChildElement(MBSIMNS"frequency");
00393       fmatvec::Vector<Col,double> frequency_=Element::getVec(e, amplitude_.size());
00394       frequency=frequency_;
00395       e=element->FirstChildElement(MBSIMNS"phase");
00396       fmatvec::Vector<Col,double> phase_=Element::getVec(e, amplitude_.size());
00397       phase=phase_;
00398       e=element->FirstChildElement(MBSIMNS"offset");
00399       fmatvec::Vector<Col,double> offset_;
00400       if (e)
00401         offset_=Element::getVec(e, amplitude_.size());
00402       else
00403         offset_=fmatvec::Vector<Col,double>(amplitude_.size());
00404       offset=offset_;
00405       check();
00406     }
00407 
00408   template<class Col>
00409   MBXMLUtils::TiXmlElement* SinusFunction1_VS<Col>::writeXMLFile(MBXMLUtils::TiXmlNode *parent) {
00410     MBXMLUtils::TiXmlElement *ele0 = DifferentiableFunction1<fmatvec::Vector<Col,double> >::writeXMLFile(parent);
00411     addElementText(ele0,MBSIMNS"amplitude",amplitude);
00412     addElementText(ele0,MBSIMNS"frequency",frequency);
00413     addElementText(ele0,MBSIMNS"phase",phase);
00414     addElementText(ele0,MBSIMNS"offset",offset);
00415     return ele0;
00416   }
00417 
00418   template<class Col>
00419     void SinusFunction1_VS<Col>::check() {
00420       ySize=amplitude.size();
00421       assert(frequency.size()==ySize);
00422       assert(phase.size()==ySize);
00423       assert(offset.size()==ySize);
00424     }
00425 
00426   template<class Row, class Col>
00427     void TabularFunction1_VS<Row,Col>::initializeUsingXML(MBXMLUtils::TiXmlElement * element) {
00428       MBXMLUtils::TiXmlElement *e=element->FirstChildElement(MBSIMNS"x");
00429       if (e) {
00430         fmatvec::Vector<Row,double> x_=Element::getVec(e);
00431         x=x_;
00432         e=element->FirstChildElement(MBSIMNS"y");
00433         fmatvec::Matrix<fmatvec::General,Row,Col,double> y_=Element::getMat(e, x.size(), 0);
00434         y=y_;
00435       }
00436       e=element->FirstChildElement(MBSIMNS"xy");
00437       if (e) {
00438         fmatvec::MatV xy=Element::getMat(e);
00439         assert(xy.cols()>1);
00440         x=xy.col(0);
00441         y=xy(fmatvec::Range<fmatvec::Var,fmatvec::Var>(0, xy.rows()-1), fmatvec::Range<fmatvec::Var,fmatvec::Var>(1, xy.cols()-1));
00442       }
00443       check();
00444     }
00445 
00446   template<class Row, class Col>
00447     fmatvec::Vector<Col,double> TabularFunction1_VS<Row,Col>::operator()(const double& xVal, const void *) {
00448       int i=xIndexOld;
00449       if (xVal<=x(0)) {
00450         xIndexOld=0;
00451         return trans(y.row(0));
00452       }
00453       else if (xVal>=x(xSize-1)) {
00454         xIndexOld=xSize-1;
00455         return trans(y.row(xSize-1));
00456       }
00457       else if (xVal<=x(i)) {
00458         while (xVal<x(i))
00459           i--;
00460       }
00461       else {
00462         do
00463           i++;
00464         while (xVal>x(i));
00465         i--;
00466       }
00467       xIndexOld=i;
00468       fmatvec::RowVector<Col,double> m=(y.row(i+1)-y.row(i))/(x(i+1)-x(i));
00469       return trans(y.row(i)+(xVal-x(i))*m);
00470     }
00471 
00472   template<class Row, class Col>
00473     void TabularFunction1_VS<Row,Col>::check() {
00474       for (int i=1; i<x.size(); i++)
00475         assert(x(i)>x(i-1));
00476       assert(x.size()==y.rows());
00477       xSize=x.size();
00478     }
00479 
00480 
00481 }
00482 
00483 #endif /* _FUNCTION_LIBRARY_H_ */
00484