Back to index

kdeartwork  4.3.2
rkodesolver.h
Go to the documentation of this file.
00001 
00020 #ifndef RKODESOLVER_H
00021 #define RKODESOLVER_H
00022 
00023 #include <KDebug>
00024 
00025 /* vector and matrix classes */
00026 #include <Eigen/Core>
00027 /* import most common Eigen types */
00028 USING_PART_OF_NAMESPACE_EIGEN
00029 
00030 
00044 template<typename T, int D>
00045 class RkOdeSolver
00046 {
00047   public:
00056    RkOdeSolver(
00057       const T&             x,//   = 0.0,
00058       const Matrix<T,D,1>& y,//   = Matrix<T,D,1>::Zero(),
00059       const T&             dx  = 0.0,
00060       const T&             eps = 1e-6);
00061 
00063    virtual ~RkOdeSolver(void);
00064 
00072    void integrate(const T& dx);
00073 
00074    // Accessors
00075 
00078    const T& X(void) const;
00081    void X(const T& a);
00082 
00085    const Matrix<T,D,1>& Y(void) const;
00088    void Y(const Matrix<T,D,1>& a);
00089 
00092    const Matrix<T,D,1>& dYdX(void) const;
00093 
00096    const T& dX(void) const;
00099    void dX(const T& a);
00100 
00103    const T& Eps(void) const;
00106    void Eps(const T& a);
00107 
00108   protected:
00117    virtual Matrix<T,D,1>
00118       f(const T& x, const Matrix<T,D,1>& y) const = 0;
00119 
00120   private:
00130    bool rkStepCheck(const T& dx);
00131 
00140    Matrix<T,D,1> rkStep(
00141       const T& dx, Matrix<T,D,1>& yerr) const;
00142 
00144    T             m_x;
00146    Matrix<T,D,1> m_y;
00148    Matrix<T,D,1> m_dydx;
00149 
00151    T             m_eps;
00153    T             m_step;
00154 };
00155 
00156 // inline accessors
00157 
00158 template<typename T, int D>
00159 inline const T&
00160 RkOdeSolver<T, D>::X(void) const
00161 {
00162    return m_x;
00163 }
00164 
00165 template<typename T, int D>
00166 inline void
00167 RkOdeSolver<T, D>::X(const T &a)
00168 {
00169    m_x = a;
00170 }
00171 
00172 template<typename T, int D>
00173 inline const Matrix<T,D,1>&
00174 RkOdeSolver<T, D>::Y(void) const
00175 {
00176    return m_y;
00177 }
00178 
00179 template<typename T, int D>
00180 inline void
00181 RkOdeSolver<T, D>::Y(const Matrix<T,D,1>& a)
00182 {
00183    m_y = a;
00184 }
00185 
00186 template<typename T, int D>
00187 inline const Matrix<T,D,1>&
00188 RkOdeSolver<T, D>::dYdX(void) const
00189 {
00190    return m_dydx;
00191 }
00192 
00193 template<typename T, int D>
00194 inline const T&
00195 RkOdeSolver<T, D>::dX(void) const
00196 {
00197    return m_step;
00198 }
00199 
00200 template<typename T, int D>
00201 inline const T&
00202 RkOdeSolver<T, D>::Eps(void) const
00203 {
00204    return m_eps;
00205 }
00206 
00207 
00208 template<typename T, int D>
00209 RkOdeSolver<T, D>::RkOdeSolver(
00210    const T&             x,
00211    const Matrix<T,D,1>& y,
00212    const T&             dx,
00213    const T&             eps)
00214    : m_x(x)
00215 {
00216    Y(y);
00217    dX(dx);
00218    Eps(eps);
00219 }
00220 
00221 // virtual dtor
00222 template<typename T, int D>
00223 RkOdeSolver<T, D>::~RkOdeSolver(void)
00224 {
00225 }
00226 
00227 // accessors
00228 
00229 template<typename T, int D>
00230 void
00231 RkOdeSolver<T, D>::dX(const T& a)
00232 {
00233    if (a <= 0.0)
00234    {
00235       kError() << "RkOdeSolver: dx must be > 0";
00236       m_step = 0.001;            // a very arbitrary value
00237       return;
00238    }
00239 
00240    m_step = a;
00241 }
00242 
00243 template<typename T, int D>
00244 void
00245 RkOdeSolver<T, D>::Eps(const T& a)
00246 {
00247    if (a <= 0.0)
00248    {
00249       kError() << "RkOdeSolver: eps must be > 0";
00250       m_eps = 1e-5;              // a very arbitrary value
00251       return;
00252    }
00253 
00254    m_eps = a;
00255 }
00256 
00257 // public member functions
00258 
00259 template<typename T, int D>
00260 void
00261 RkOdeSolver<T, D>::integrate(const T& deltaX)
00262 {
00263    if (deltaX == 0)
00264    {
00265       return;                   // nothing to integrate
00266    }
00267 
00268    // init dydx
00269    m_dydx = f(m_x, m_y);
00270 
00271    static const unsigned int maxiter = 10000;
00272    const T x2 = m_x + deltaX;
00273 
00274    unsigned int iter;
00275    for (iter=0;
00276         iter<maxiter && !rkStepCheck(x2-m_x);
00277         ++iter)
00278    {
00279    }
00280 
00281    if (iter > maxiter)
00282    {
00283       kWarning()
00284          << "RkOdeSolver: More than " << maxiter
00285          << " iterations in RkOdeSolver::integrate" << endl;
00286    }
00287 }
00288 
00289 
00290 // private member functions
00291 
00292 template<typename T, int D>
00293 bool
00294 RkOdeSolver<T, D>::rkStepCheck(const T& dx_requested)
00295 {
00296    static const T safety =  0.9;
00297    static const T pshrnk = -0.25;
00298    static const T pgrow  = -0.2;
00299 
00300    // reduce step size by no more than a factor 10
00301    static const T shrinkLimit = 0.1;
00302    // enlarge step size by no more than a factor 5
00303    static const T growthLimit = 5.0;
00304    // errmax_sl = 6561.0
00305    static const T errmax_sl = std::pow(shrinkLimit/safety, 1.0/pshrnk);
00306    // errmax_gl = 1.89e-4
00307    static const T errmax_gl = std::pow(growthLimit/safety, 1.0/pgrow);
00308 
00309    static const unsigned int maxiter = 100;
00310 
00311    if (dx_requested == 0)
00312    {
00313       return true;              // integration done
00314    }
00315 
00316    Matrix<T,D,1> ytmp, yerr, t;
00317 
00318    bool stepSizeWasMaximal;
00319    T dx;
00320    if (std::abs(dx_requested) > m_step)
00321    {
00322       stepSizeWasMaximal = true;
00323       dx = dx_requested>0 ? m_step : -m_step;
00324    }
00325    else
00326    {
00327       stepSizeWasMaximal = false;
00328       dx = dx_requested;
00329    }
00330 
00331    // generic scaling factor
00332    // |y| + |dx * dy/dx| + 1e-15
00333    Matrix<T,D,1> yscal
00334       = (m_y.cwise().abs() + (dx*m_dydx).cwise().abs()).cwise()
00335       + 1e-15;
00336 
00337    unsigned int iter = 0;
00338    T errmax = 0;
00339    do
00340    {
00341       if (errmax >= 1.0)
00342       {
00343          // reduce step size
00344          dx *= errmax<errmax_sl ? safety * pow(errmax, pshrnk) : shrinkLimit;
00345          stepSizeWasMaximal = true;
00346          if (m_x == m_x + dx)
00347          {
00348             // stepsize below numerical resolution
00349             kError()
00350                << "RkOdeSolver: stepsize underflow in rkStepCheck"
00351                << endl;
00352          }
00353          // new dx -> update scaling vector
00354          yscal
00355             = (m_y.cwise().abs()
00356                + (dx*m_dydx).cwise().abs()).cwise()
00357             + 1e-15;
00358       }
00359 
00360       ytmp   = rkStep(dx, yerr); // try to make a step forward
00361       t      = (yerr.cwise() / yscal).cwise().abs(); // calc the error vector
00362       errmax = t.maxCoeff()/m_eps;    // calc the rel. maximal error
00363       ++iter;
00364    } while ((iter < maxiter) && (errmax >= 1.0));
00365 
00366    if (iter >= maxiter)
00367    {
00368       kError()
00369          << "RkOdeSolver: too many iterations in rkStepCheck()";
00370    }
00371 
00372    if (stepSizeWasMaximal)
00373    {
00374       // estimate next step size if used step size was maximal
00375       m_step =
00376          std::abs(dx)
00377          * (errmax>errmax_gl ? safety * pow(errmax, pgrow) : growthLimit);
00378    }
00379    m_x    += dx;                // make step forward
00380    m_y     = ytmp;              // save new function values
00381    m_dydx  = f(m_x,m_y);        // and update derivatives
00382 
00383    return (std::abs(dx) < std::abs(dx_requested));
00384 }
00385 
00386 template<typename T, int D>
00387 Matrix<T,D,1>
00388 RkOdeSolver<T, D>::rkStep(const T& dx, Matrix<T,D,1>& yerr) const
00389 {
00390    static const T
00391       a2=0.2, a3=0.3, a4=0.6, a5=1.0, a6=0.875,
00392       b21=0.2,
00393       b31=3.0/40.0,       b32=9.0/40.0,
00394       b41=0.3,            b42=-0.9,        b43=1.2,
00395       b51=-11.0/54.0,     b52=2.5,         b53=-70.0/27.0, b54=35.0/27.0,
00396       b61=1631.0/55296.0, b62=175.0/512.0, b63=575.0/13824.0,
00397       b64=44275.0/110592.0, b65=253.0/4096.0,
00398       c1=37.0/378.0, c3=250.0/621.0, c4=125.0/594.0, c6=512.0/1771.0,
00399       dc1=c1-2825.0/27648.0,  dc3=c3-18575.0/48384.0,
00400       dc4=c4-13525.0/55296.0, dc5=-277.0/14336.0, dc6=c6-0.25;
00401 
00402    Matrix<T,D,1> ak2 = f(m_x + a2*dx,
00403                          m_y + dx*b21*m_dydx);             // 2. step
00404    Matrix<T,D,1> ak3 = f(m_x + a3*dx,
00405                          m_y + dx*(b31*m_dydx + b32*ak2)); // 3.step
00406    Matrix<T,D,1> ak4 = f(m_x + a4*dx,
00407                          m_y + dx*(b41*m_dydx + b42*ak2
00408                                    + b43*ak3));           // 4.step
00409    Matrix<T,D,1> ak5 = f(m_x + a5*dx,
00410                          m_y + dx*(b51*m_dydx + b52*ak2
00411                                    + b53*ak3 + b54*ak4)); // 5.step
00412    Matrix<T,D,1> ak6 = f(m_x + a6*dx,
00413                          m_y + dx*(b61*m_dydx + b62*ak2
00414                                    + b63*ak3 + b64*ak4
00415                                    + b65*ak5));           // 6.step
00416    yerr       = dx*(dc1*m_dydx + dc3*ak3 + dc4*ak4 + dc5*ak5 + dc6*ak6);
00417    return m_y + dx*( c1*m_dydx +            c3*ak3 +  c4*ak4 +  c6*ak6);
00418 }
00419 
00420 #endif