NumericalDiff.h
Go to the documentation of this file.
00001 // -*- coding: utf-8
00002 // vim: set fileencoding=utf-8
00003 
00004 // This file is part of Eigen, a lightweight C++ template library
00005 // for linear algebra.
00006 //
00007 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
00008 //
00009 // This Source Code Form is subject to the terms of the Mozilla
00010 // Public License v. 2.0. If a copy of the MPL was not distributed
00011 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00012 
00013 #ifndef EIGEN_NUMERICAL_DIFF_H
00014 #define EIGEN_NUMERICAL_DIFF_H
00015 
00016 namespace Eigen { 
00017 
00018 enum NumericalDiffMode {
00019     Forward,
00020     Central
00021 };
00022 
00023 
00035 template<typename _Functor, NumericalDiffMode mode=Forward>
00036 class NumericalDiff : public _Functor
00037 {
00038 public:
00039     typedef _Functor Functor;
00040     typedef typename Functor::Scalar Scalar;
00041     typedef typename Functor::InputType InputType;
00042     typedef typename Functor::ValueType ValueType;
00043     typedef typename Functor::JacobianType JacobianType;
00044 
00045     NumericalDiff(Scalar _epsfcn=0.) : Functor(), epsfcn(_epsfcn) {}
00046     NumericalDiff(const Functor& f, Scalar _epsfcn=0.) : Functor(f), epsfcn(_epsfcn) {}
00047 
00048     // forward constructors
00049     template<typename T0>
00050         NumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {}
00051     template<typename T0, typename T1>
00052         NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {}
00053     template<typename T0, typename T1, typename T2>
00054         NumericalDiff(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2), epsfcn(0) {}
00055 
00056     enum {
00057         InputsAtCompileTime = Functor::InputsAtCompileTime,
00058         ValuesAtCompileTime = Functor::ValuesAtCompileTime
00059     };
00060 
00064     int df(const InputType& _x, JacobianType &jac) const
00065     {
00066         using std::sqrt;
00067         using std::abs;
00068         /* Local variables */
00069         Scalar h;
00070         int nfev=0;
00071         const typename InputType::Index n = _x.size();
00072         const Scalar eps = sqrt(((std::max)(epsfcn,NumTraits<Scalar>::epsilon() )));
00073         ValueType val1, val2;
00074         InputType x = _x;
00075         // TODO : we should do this only if the size is not already known
00076         val1.resize(Functor::values());
00077         val2.resize(Functor::values());
00078 
00079         // initialization
00080         switch(mode) {
00081             case Forward:
00082                 // compute f(x)
00083                 Functor::operator()(x, val1); nfev++;
00084                 break;
00085             case Central:
00086                 // do nothing
00087                 break;
00088             default:
00089                 eigen_assert(false);
00090         };
00091 
00092         // Function Body
00093         for (int j = 0; j < n; ++j) {
00094             h = eps * abs(x[j]);
00095             if (h == 0.) {
00096                 h = eps;
00097             }
00098             switch(mode) {
00099                 case Forward:
00100                     x[j] += h;
00101                     Functor::operator()(x, val2);
00102                     nfev++;
00103                     x[j] = _x[j];
00104                     jac.col(j) = (val2-val1)/h;
00105                     break;
00106                 case Central:
00107                     x[j] += h;
00108                     Functor::operator()(x, val2); nfev++;
00109                     x[j] -= 2*h;
00110                     Functor::operator()(x, val1); nfev++;
00111                     x[j] = _x[j];
00112                     jac.col(j) = (val2-val1)/(2*h);
00113                     break;
00114                 default:
00115                     eigen_assert(false);
00116             };
00117         }
00118         return nfev;
00119     }
00120 private:
00121     Scalar epsfcn;
00122 
00123     NumericalDiff& operator=(const NumericalDiff&);
00124 };
00125 
00126 } // end namespace Eigen
00127 
00128 //vim: ai ts=4 sts=4 et sw=4
00129 #endif // EIGEN_NUMERICAL_DIFF_H
00130 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:15