00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
00033
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
00699