00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
00237 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00238 virtual double operator()(const double& x, const double& y, const void * = NULL);
00239
00240
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
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
00484