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