MBSim

Project Page
differentiable_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: martin.o.foerg@googlemail.com
00018  */
00019 
00020 #ifndef DIFFERENTIABLE_FUNCTION_LIBRARY_H_
00021 #define DIFFERENTIABLE_FUNCTION_LIBRARY_H_
00022 
00023 #include <mbsim/utils/function.h>
00024 
00025 namespace MBSim {
00026 
00027   template<class Ret> class FirstDerivative;
00028   template<class Ret> class SecondDerivative;
00029 
00030   template<class Ret>
00031     class DifferentiableFunction : public Function1<Ret,double> {
00032       protected:
00033         DifferentiableFunction *firstDerivative, *secondDerivative;
00034       public:
00035         DifferentiableFunction() : firstDerivative(0), secondDerivative(0) {}
00036 
00037         virtual ~DifferentiableFunction() {
00038           delete firstDerivative;
00039           delete secondDerivative;
00040         }
00041 
00042         std::string getType() const { return "DifferentiableFunction"; }
00043 
00044         DifferentiableFunction* getFirstDerivative() {
00045           return firstDerivative ? firstDerivative : new FirstDerivative<Ret>(this);
00046         }
00047 
00048         DifferentiableFunction* getSecondDerivative() {
00049           return secondDerivative ? secondDerivative : new SecondDerivative<Ret>(this);
00050         }
00051 
00052         virtual Ret diff1(const double& x, const void * =NULL) {
00053           double dx = epsroot();
00054           return ((*this)(x+dx) - (*this)(x-dx))/(2.*dx);
00055         }
00056         virtual Ret diff2(const double& x, const void * =NULL) {
00057           double dx = epsroot();
00058           return ((*this).diff1(x+dx) - (*this).diff1(x-dx))/(2.*dx);
00059         }
00060     };
00061 
00062   template <class Ret>
00063     class FirstDerivative : public DifferentiableFunction<Ret> {
00064       protected:
00065         DifferentiableFunction<Ret> *f;
00066       public:
00067         FirstDerivative(DifferentiableFunction<Ret> *f_) : f(f_) {}
00068         Ret operator()(const double& x, const void * =NULL) {
00069           return (*f).diff1(x);
00070         }
00071         Ret diff1(const double& x, const void * =NULL) {
00072           return (*f).diff2(x);
00073         }
00074     };
00075 
00076   template <class Ret>
00077     class SecondDerivative : public DifferentiableFunction<Ret> {
00078       protected:
00079         DifferentiableFunction<Ret> *f;
00080       public:
00081         SecondDerivative(DifferentiableFunction<Ret> *f_) : f(f_) {}
00082         Ret operator()(const double& x, const void * =NULL) {
00083           return (*f).diff2(x);
00084         }
00085     };
00086 
00087   class Variable : public DifferentiableFunction<double> {
00088     public:
00089       Variable() {}
00090       double operator()(const double& x, const void * =NULL) {
00091         return x;
00092       }
00093       double diff1(const double& x, const void * =NULL) {
00094         return 1;
00095       }
00096       double diff2(const double& x, const void * =NULL) {
00097         return 0;
00098       }
00099   };
00100 
00101   class Zero : public DifferentiableFunction<double> {
00102     public:
00103       Zero() {}
00104       double operator()(const double& x, const void * =NULL) {
00105         return 0;
00106       }
00107       double diff1(const double& x, const void * =NULL) {
00108         return 0;
00109       }
00110       double diff2(const double& x, const void * =NULL) {
00111         return 0;
00112       }
00113   };
00114 
00115   class Constant : public DifferentiableFunction<double> {
00116     protected:
00117       double a;
00118     public:
00119       Constant(double a_) : a(a_) {}
00120       double operator()(const double& x, const void * =NULL) {
00121         return a;
00122       }
00123       double diff1(const double& x, const void * =NULL) {
00124         return 0;
00125       }
00126       double diff2(const double& x, const void * =NULL) {
00127         return 0;
00128       }
00129   };
00130 
00131   class Sinus : public DifferentiableFunction<double> {
00132     protected:
00133       DifferentiableFunction<double> *f;
00134     public:
00135       Sinus(DifferentiableFunction<double> *f_) : f(f_) {}
00136       double operator()(const double& x, const void * =NULL) {
00137         return sin((*f)(x));
00138       }
00139       double diff1(const double& x, const void * =NULL) {
00140         return cos((*f)(x))*(*f).diff1(x);
00141       }
00142       double diff2(const double& x, const void * =NULL) {
00143         return -sin((*f)(x))*(*f).diff1(x)*(*f).diff1(x) + cos((*f)(x))*(*f).diff2(x);
00144       }
00145   };
00146 
00147   class Product : public DifferentiableFunction<double> {
00148     protected:
00149       DifferentiableFunction<double> *f1, *f2;
00150     public:
00151       Product(DifferentiableFunction<double> *f1_, DifferentiableFunction<double> *f2_) : f1(f1_), f2(f2_){}
00152       double operator()(const double& x, const void * =NULL) {
00153         return (*f1)(x)*(*f2)(x);
00154       }
00155       double diff1(const double& x, const void * =NULL) {
00156         return (*f1).diff1(x)*(*f2)(x) + (*f1)(x)*(*f2).diff1(x);
00157       }
00158       double diff2(const double& x, const void * =NULL) {
00159         return (*f1).diff2(x)*(*f2)(x) + 2*(*f1).diff1(x)*(*f2).diff1(x) + (*f1)(x)*(*f2).diff2(x) ;
00160       }
00161   };
00162 
00163   class Sum : public DifferentiableFunction<double> {
00164     protected:
00165       DifferentiableFunction<double> *f1, *f2;
00166     public:
00167       Sum(DifferentiableFunction<double> *f1_, DifferentiableFunction<double> *f2_) : f1(f1_), f2(f2_){}
00168       double operator()(const double& x, const void * =NULL) {
00169         return (*f1)(x)+(*f2)(x);
00170       }
00171       double diff1(const double& x, const void * =NULL) {
00172         return (*f1).diff1(x)+(*f2).diff1(x);
00173       }
00174       double diff2(const double& x, const void * =NULL) {
00175         return (*f1).diff2(x)+(*f2).diff2(x);
00176       }
00177   };
00178 
00179   class LinearFunction : public DifferentiableFunction<double> {
00180     protected:
00181       double a, b;
00182     public:
00183       LinearFunction(double a_, double b_) : a(a_), b(b_) {}
00184       double operator()(const double& x, const void * =NULL) {
00185         return a*x+b;
00186       }
00187       double diff1(const double& x, const void * =NULL) {
00188         return a;
00189       }
00190       double diff2(const double& x, const void * =NULL) {
00191         return 0;
00192       }
00193   };
00194 
00195   template <class Col>
00196     class VectorFunction : public DifferentiableFunction<fmatvec::Vector<Col,double> > {
00197       protected:
00198         std::vector<DifferentiableFunction<double>*> f;
00199       public:
00200         VectorFunction() {}
00201         VectorFunction(std::vector<DifferentiableFunction<double>*> f_) : f(f_) {}
00202         void addFunction(DifferentiableFunction<double> *f_) {f.push_back(f_);}
00203         virtual fmatvec::Vector<Col,double> operator()(const double& x, const void * =NULL) {
00204           fmatvec::Vector<Col,double> r(f.size(),fmatvec::NONINIT);
00205           for(int i=0; i<r.size(); i++)
00206             r(i) = (*f[i])(x);
00207           return r;
00208         };
00209         virtual fmatvec::Vector<Col,double> diff1(const double& x, const void * =NULL) {
00210           fmatvec::Vector<Col,double> r(f.size(),fmatvec::NONINIT);
00211           for(int i=0; i<r.size(); i++)
00212             r(i) = (*f[i]).diff1(x);
00213           return r;
00214         }
00215         virtual fmatvec::Vector<Col,double> diff2(const double& x, const void * =NULL) {
00216           fmatvec::Vector<Col,double> r(f.size(),fmatvec::NONINIT);
00217           for(int i=0; i<r.size(); i++)
00218             r(i) = (*f[i]).diff2(x);
00219           return r;
00220         }
00221     };
00222 
00223 }
00224 
00225 
00226 #endif