MBSim

Project Page
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 FUNCTION_H_
00021 #define FUNCTION_H_
00022 
00023 #include <mbxmlutilstinyxml/tinyxml.h>
00024 #include <mbsim/element.h>
00025 #include <mbsim/utils/utils.h>
00026 #include <mbsim/utils/eps.h>
00027 
00028 #include <fmatvec/fmatvec.h>
00029 
00030 namespace MBSim {
00031 
00032   // abstract base class for all Functions
00033   // (required to be able to case Function of different type dynamically (in ObjectFactory))
00034   class Function {
00035     public:
00036       virtual ~Function() {}
00037   };
00038 
00044   template<class Ret, class Arg>
00045     class Function1 : public Function {
00046       public:
00050         virtual ~Function1() {}
00051 
00052         /* INTERFACE FOR DERIVED CLASSES */
00058         virtual Ret operator()(const Arg& x, const void * =NULL)=0;
00059 
00064         virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00065         virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { 
00066           MBXMLUtils::TiXmlElement *ele0=new MBXMLUtils::TiXmlElement(MBSIMNS+getType());
00067           parent->LinkEndChild(ele0);
00068           return ele0;
00069         }
00070 
00074         virtual std::string getType() const { return "Function1"; }
00075         /***************************************************/
00076     };
00077 
00083   template<class Ret, class Arg1, class Arg2>
00084     class Function2 : public Function {
00085       public:
00089         virtual ~Function2() {}
00090 
00091         /* INTERFACE FOR DERIVED CLASSES */
00097         virtual Ret operator()(const Arg1& p1, const Arg2& p2, const void * =NULL)=0;
00098 
00103         virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00104         virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { 
00105           MBXMLUtils::TiXmlElement *ele0=new MBXMLUtils::TiXmlElement(MBSIMNS+getType());
00106           parent->LinkEndChild(ele0);
00107           return ele0;
00108         }
00109 
00113         virtual std::string getType() const { return "Function2"; }
00114         /***************************************************/
00115     };
00116 
00122   template<class Ret, class Arg1, class Arg2, class Arg3>
00123     class Function3 : public Function {
00124       public:
00128         virtual ~Function3() {}
00129 
00130         /* INTERFACE FOR DERIVED CLASSES */
00136         virtual Ret operator()(const Arg1& p1, const Arg2& p2, const Arg3& p3, const void * =NULL)=0;
00137 
00142         virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00143         /***************************************************/
00144     };
00145 
00152   template<class Ret>
00153     class DifferentiableFunction1 : public Function1<Ret,double> {
00154       public:
00158         DifferentiableFunction1() : Function1<Ret,double>(), order(0) {}
00159 
00163         virtual ~DifferentiableFunction1() { delete derivatives[0]; derivatives.erase(derivatives.begin()); }
00164 
00165         /* INHERITED INTERFACE OF FUNCTION1 */
00166         virtual Ret operator()(const double& x, const void * =NULL) {
00167           assert(derivatives.size()>0); return (getDerivative(order))(x); }
00168         /***************************************************/
00169 
00174         const Function1<Ret,double>& getDerivative(int degree) const { return *(derivatives[degree]); }
00175 
00180         Function1<Ret,double>& getDerivative(int degree) { return *(derivatives[degree]); }
00181 
00185         void addDerivative(Function1<Ret,double> *diff) { derivatives.push_back(diff); }
00186 
00191         void setDerivative(Function1<Ret,double> *diff,size_t degree) { derivatives.resize(max(derivatives.size(),degree+1)); derivatives[degree]=diff; }
00192 
00196         void setOrderOfDerivative(int i) { order=i; }
00197 
00202         virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00203           Function1<Ret,double>::initializeUsingXML(element);
00204           MBXMLUtils::TiXmlElement * e;
00205           e=element->FirstChildElement(MBSIMNS"orderOfDerivative");
00206           if (e)
00207             setOrderOfDerivative(atoi(e->GetText()));
00208         }
00209         MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) {
00210           MBXMLUtils::TiXmlElement *ele0 = Function1<Ret,double>::writeXMLFile(parent);
00211           addElementText(ele0,MBSIMNS"orderOfDerivative",order);
00212           return ele0;
00213         }
00214         std::string getType() const { return "DifferentiableFunction1"; }
00215         /***************************************************/
00216 
00217       protected:
00221         std::vector<Function1<Ret,double>* > derivatives;
00222         int order;
00223     };
00224 
00230   template<class Ret, class Arg>
00231     class ConstantFunction1 : public Function1<Ret,Arg> {
00232       public:
00236         ConstantFunction1() {}
00237 
00242         ConstantFunction1(Ret c_) : c(c_) {}
00243 
00244         /* INHERITED INTERFACE OF FUNCTION1 */
00245         virtual Ret operator()(const Arg& p, const void * =NULL) { return c; }
00246         void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00247           Function1<Ret,Arg>::initializeUsingXML(element);
00248           MBXMLUtils::TiXmlElement *e;
00249           e=element->FirstChildElement(MBSIMNS"value");
00250           // TODO: use c=fromMatStr<Ret>(e->GetText()) and remove ConstantFunction1<double, Arg>
00251           c=Ret(e->GetText());
00252         }
00253         virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) {
00254           MBXMLUtils::TiXmlElement *ele0 = Function1<Ret,Arg>::writeXMLFile(parent);
00255           addElementText(ele0,MBSIMNS"value",c);
00256           return ele0;
00257         } 
00258 
00259         inline std::string getType() const { return std::string("ConstantFunction1_")+funcExt<Ret>()+funcExt<Arg>(); }
00260         /***************************************************/
00261 
00262         /* GETTER / SETTER */
00263         void setValue(Ret c_) { c=c_; }
00264         /***************************************************/
00265 
00266       protected:
00270         Ret c;
00271     };
00272 
00278   template<class Arg>
00279     class ConstantFunction1<double, Arg> : public Function1<double,Arg> {
00280       public:
00284         ConstantFunction1() {}
00285 
00290         ConstantFunction1(double c_) : c(c_) {}
00291 
00292         /* INHERITED INTERFACE OF FUNCTION1 */
00293         virtual double operator()(const Arg& p, const void * =NULL) { return c; }
00294         void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00295           Function1<double,Arg>::initializeUsingXML(element);
00296           MBXMLUtils::TiXmlElement *e;
00297           e=element->FirstChildElement(MBSIMNS"value");
00298           c=Element::getDouble(e);
00299         }
00300         virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) {
00301           MBXMLUtils::TiXmlElement *ele0 = Function1<double,Arg>::writeXMLFile(parent);
00302           addElementText(ele0,MBSIMNS"value",c);
00303           return ele0;
00304         } 
00305         inline std::string getType() const { return std::string("ConstantFunction1_")+funcExt<double>()+funcExt<Arg>(); }
00306         /***************************************************/
00307 
00308         /* GETTER / SETTER */
00309         void setValue(double c_) { c=c_; }
00310         /***************************************************/
00311 
00312       protected:
00316         double c;
00317     };
00318 
00324   template<class Ret, class Arg1, class Arg2>
00325     class ConstantFunction2 : public Function2<Ret,Arg1,Arg2> {
00326       public:
00330         ConstantFunction2() {}
00331 
00336         ConstantFunction2(Ret c_) : c(c_) {}
00337 
00338         /* INHERITED INTERFACE OF FUNCTION2 */
00339         virtual Ret operator()(const Arg1& p1, const Arg2& p2, const void * =NULL) { return c; }
00340         void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00341           Function2<Ret,Arg1,Arg2>::initializeUsingXML(element);
00342           MBXMLUtils::TiXmlElement *e;
00343           e=element->FirstChildElement(MBSIMNS"value");
00344           c=Ret(e->GetText());
00345         }
00346         /***************************************************/
00347 
00348         /* GETTER / SETTER */
00349         void setValue(Ret c_) { c=c_; }
00350         /***************************************************/
00351 
00352       protected:
00356         Ret c;
00357     };
00358 
00365   class LinearSpringDamperForce : public Function2<double,double,double> {
00366     public:
00370       LinearSpringDamperForce() {}
00371 
00378       LinearSpringDamperForce(double c_, double d_, double l0_) : c(c_), d(d_), l0(l0_) {}
00379 
00380       /* INHERITED INTERFACE OF FUNCTION2 */
00381       virtual double operator()(const double& g, const double& gd, const void * =NULL) { return c*(g-l0) + d*gd; }
00382       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00383       /***************************************************/
00384 
00385       /* GETTER / SETTER */
00386       void setParameters(double c_, double d_, double l0_) { c=c_; d=d_; l0=l0_; }
00387       /***************************************************/
00388 
00389     protected:
00393       double c, d, l0;
00394   };
00395 
00402   class NonlinearSpringDamperForce : public Function2<double,double,double> {
00403     public:
00407       NonlinearSpringDamperForce() {}
00408 
00414       NonlinearSpringDamperForce(Function1<fmatvec::Vec, double> * gForceFun_, Function1<fmatvec::Vec, double> * gdForceFun_) : gForceFun(gForceFun_), gdForceFun(gdForceFun_) {}
00415 
00416       /* INHERITED INTERFACE OF FUNCTION2 */
00417       virtual double operator()(const double& g, const double& gd, const void * =NULL) { return (*gForceFun)(g)(0) + (*gdForceFun)(gd)(0); }
00418       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00419       /***************************************************/
00420 
00421       /* GETTER / SETTER */
00422       void setParameters(Function1<fmatvec::Vec, double> * gForceFun_, Function1<fmatvec::Vec, double> * gdForceFun_) { gForceFun=gForceFun_; gdForceFun=gdForceFun_; }
00423       /***************************************************/
00424 
00425     protected:
00429       Function1<fmatvec::Vec, double> * gForceFun;
00430 
00434       Function1<fmatvec::Vec, double> * gdForceFun;
00435   };
00436 
00442   template<class Arg1, class Arg2>
00443     class ConstantFunction2<double, Arg1, Arg2> : public Function2<double,Arg1,Arg2> {
00444       public:
00448         ConstantFunction2() {}
00453         ConstantFunction2(double c_) : c(c_) {}
00454 
00455         /* INHERITED INTERFACE OF FUNCTION2 */
00456         virtual double operator()(const Arg1& p1, const Arg2& p2, const void * =NULL) { return c; }
00457         void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00458           Function2<double,Arg1,Arg2>::initializeUsingXML(element);
00459           MBXMLUtils::TiXmlElement *e;
00460           e=element->FirstChildElement(MBSIMNS"value");
00461           c=Element::getDouble(e);
00462         }
00463         /***************************************************/
00464 
00465         /* GETTER / SETTER */
00466         void setValue(double c_) { c=c_; }
00467         /***************************************************/
00468 
00469       protected:
00470         double c;
00471     };
00472 
00479   class LinearRegularizedUnilateralConstraint: public Function2<double,double,double> {
00480     public:
00484       LinearRegularizedUnilateralConstraint() : c(0), d(0) {}
00485 
00491       LinearRegularizedUnilateralConstraint(double c_, double d_) : c(c_), d(d_) {}
00492 
00493       /* INHERITED INTERFACE OF FUNCTION2 */
00494       virtual double operator()(const double& g, const double& gd, const void * =NULL) { 
00495         if(g>0)
00496           return 0;
00497         else if(gd<0) 
00498           return -c*g - d*gd;
00499         else
00500           return -c*g;
00501       }
00502       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00503       /***************************************************/
00504 
00505       /* GETTER / SETTER */
00506       void setParameter(double c_, double d_) { c=c_; d=d_; }
00507       /***************************************************/
00508 
00509     private:
00513       double c, d;
00514   };
00515 
00522   class LinearRegularizedBilateralConstraint: public Function2<double,double,double> {
00523     public:
00527       LinearRegularizedBilateralConstraint() : c(0), d(0) {}
00528 
00534       LinearRegularizedBilateralConstraint(double c_, double d_) : c(c_), d(d_) {}
00535 
00536       /* INHERITED INTERFACE OF FUNCTION2 */
00537       virtual double operator()(const double& g, const double& gd, const void * =NULL) { 
00538         return -c*g - d*gd;
00539       }
00540       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00541       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00542 
00543       virtual std::string getType() const { return "LinearRegularizedBilateralConstraint"; }
00544       /***************************************************/
00545 
00546       /* GETTER / SETTER */
00547       void setParameter(double c_, double d_) { c=c_; d=d_; }
00548       /***************************************************/
00549 
00550     private:
00554       double c, d;
00555   };
00556 
00564   class LinearRegularizedCoulombFriction : public Function2<fmatvec::Vec,fmatvec::Vec,double> {
00565     public:
00569       LinearRegularizedCoulombFriction() : mu(0), gdLim(0.01) {}
00570 
00576       LinearRegularizedCoulombFriction(double mu_, double gdLim_=0.01) : mu(mu_), gdLim(gdLim_) {}
00577 
00578       /* INHERITED INTERFACE OF FUNCTION2 */
00579       virtual fmatvec::Vec operator()(const fmatvec::Vec &gd, const double& laN, const void * =NULL);
00580       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00581       /***************************************************/
00582 
00583       /* GETTER / SETTER */
00584       void setFrictionCoefficient(double mu_) { mu=mu_; }
00585       void setMarginalVelocity(double gdLim_) { gdLim=gdLim_; }
00586       /***************************************************/
00587 
00588     protected:
00592       double mu, gdLim;
00593   };
00594 
00602   class LinearRegularizedStribeckFriction : public Function2<fmatvec::Vec,fmatvec::Vec,double> {
00603     public:
00607       LinearRegularizedStribeckFriction() : fmu(NULL), gdLim(0.01) {}
00608 
00614       LinearRegularizedStribeckFriction(Function1<double,double> *fmu_, double gdLim_=0.01) : fmu(fmu_), gdLim(gdLim_) {}
00615 
00616       /* INHERITED INTERFACE OF FUNCTION2 */
00617       virtual fmatvec::Vec operator()(const fmatvec::Vec &gd, const double& laN, const void * =NULL);
00618       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00619       /***************************************************/
00620 
00621       /* GETTER / SETTER */
00622       void setFrictionFunction(Function1<double,double> *fmu_) { fmu=fmu_; }
00623       void setMarginalVelocity(double gdLim_) { gdLim=gdLim_; }
00624       /***************************************************/
00625 
00626     protected:
00630       Function1<double,double> *fmu;
00631       
00635       double gdLim;
00636   };
00637 
00641   class InfluenceFunction : public Function2<double, fmatvec::Vec2,  fmatvec::Vec2 > {
00642     public:
00643       InfluenceFunction(){}
00644       /* INHERITED INTERFACE OF FUNCTION2 */
00645       virtual double operator()(const fmatvec::Vec2& firstContourLagrangeParameter, const fmatvec::Vec2& secondContourLagrangeParameter, const void * = NULL)=0;
00646       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00647       /***************************************************/
00648   };
00649 
00650   /*
00651    * \brief Influence function for flexibility of contour with no influence to other contours (or contour points)
00652    */
00653   class FlexibilityInfluenceFunction : public InfluenceFunction {
00654     public:
00655       FlexibilityInfluenceFunction() : flexibility(0) {
00656       }
00657       FlexibilityInfluenceFunction(const std::string& ContourName_, const double & flexibility_) :
00658           flexibility(flexibility_) {
00659       }
00660       virtual ~FlexibilityInfluenceFunction() {}
00661       /* INHERITED INTERFACE OF FUNCTION2 */
00662       virtual double operator()(const fmatvec::Vec2& firstContourLagrangeParameter, const fmatvec::Vec2& secondContourLagrangeParameter, const void * = NULL) {
00663         if(nrm2(firstContourLagrangeParameter - secondContourLagrangeParameter) < macheps())
00664           return flexibility;
00665         else
00666           return 0;
00667       }
00668       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00669       /***************************************************/
00670 
00671     protected:
00672       double flexibility;
00673   };
00674 
00675   /*
00676    * \brief a class for Influence-Functions with constant coupling
00677    */
00678   class ConstantInfluenceFunction : public InfluenceFunction {
00679     public:
00680       ConstantInfluenceFunction() : couplingValue(0) {
00681     }
00682       ConstantInfluenceFunction(const double & couplingValue_) :
00683           couplingValue(couplingValue_) {
00684       }
00685       virtual ~ConstantInfluenceFunction() {}
00686       /* INHERITED INTERFACE OF FUNCTION2 */
00687       virtual double operator()(const fmatvec::Vec2& firstContourLagrangeParameter, const fmatvec::Vec2& secondContourLagrangeParameter, const void * = NULL) {
00688         return couplingValue;
00689       }
00690       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00691       /***************************************************/
00692 
00693     protected:
00694       double couplingValue;
00695   };
00696 }
00697 
00698 #endif /* FUNCTION_H_ */
00699