adevs
/home/rotten/adevs-2.6/include/adevs_rk_45.h
00001 #ifndef _adevs_rk_45_h_
00002 #define _adevs_rk_45_h_
00003 #include "adevs_hybrid.h"
00004 #include <cmath>
00005 
00006 namespace adevs
00007 {
00008 
00013 template <typename X> class rk_45:
00014         public ode_solver<X>
00015 {
00016         public:
00022                 rk_45(ode_system<X>* sys, double err_tol, double h_max);
00024                 ~rk_45();
00025                 double integrate(double* q, double h_lim);
00026                 void advance(double* q, double h);
00027         private:
00028                 double *dq, // derivative
00029                            *qq, // trial solution
00030                            *t,  // temporary variables for computing stages
00031                            *k[6]; // the six RK stages
00032                 const double err_tol; // Error tolerance
00033                 const double h_max; // Maximum time step
00034                 double h_cur; // Previous successful step size
00035                 // Compute a trial step of size h, store the result in qq, and return the error
00036                 double trial_step(double h);
00037 };
00038 
00039 template <typename X>
00040 rk_45<X>::rk_45(ode_system<X>* sys, double err_tol, double h_max):
00041         ode_solver<X>(sys),err_tol(err_tol),h_max(h_max),h_cur(h_max)
00042 {
00043         for (int i = 0; i < 6; i++)
00044                 k[i] = new double[sys->numVars()];
00045         dq = new double[sys->numVars()];
00046         qq = new double[sys->numVars()];
00047         t = new double[sys->numVars()];
00048 }
00049 
00050 template <typename X>
00051 rk_45<X>::~rk_45()
00052 {
00053         delete [] dq;
00054         delete [] t;
00055         for (int i = 0; i < 6; i++)
00056                 delete [] k[i];
00057 }
00058 
00059 template <typename X>
00060 void rk_45<X>::advance(double* q, double h)
00061 {
00062         double dt;
00063         while ((dt = integrate(q,h)) < h) h -= dt;
00064 }
00065 
00066 template <typename X>
00067 double rk_45<X>::integrate(double* q, double h_lim)
00068 {
00069         // Initial error estimate and step size
00070         double err = DBL_MAX, h = std::min(h_cur*1.1,std::min(h_max,h_lim));
00071         for (;;) {
00072                 // Copy q to the trial vector
00073                 for (int i = 0; i < this->sys->numVars(); i++) qq[i] = q[i];
00074                 // Make the trial step which will be stored in qq
00075                 err = trial_step(h);
00076                 // If the error is ok, then we have found the proper step size
00077                 if (err <= err_tol) {
00078                         if (h_cur <= h_lim) h_cur = h;
00079                         break;
00080                 }
00081                 // Otherwise shrink the step size and try again
00082                 else {
00083                         double h_guess = 0.8*pow(err_tol*pow(h,4.0)/fabs(err),0.25);
00084                         if (h < h_guess) h *= 0.8;
00085                         else h = h_guess;
00086                 }
00087         }
00088         // Copy the trial solution to q and return the step size that was selected
00089         for (int i = 0; i < this->sys->numVars(); i++) q[i] = qq[i];
00090         return h;
00091 }
00092 
00093 template <typename X>
00094 double rk_45<X>::trial_step(double step)
00095 {
00096         // Compute k1
00097         this->sys->der_func(qq,dq); 
00098         for (int j = 0; j < this->sys->numVars(); j++) k[0][j] = step*dq[j];
00099         // Compute k2
00100         for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] + 0.5*k[0][j];
00101         this->sys->der_func(t,dq);
00102         for (int j = 0; j < this->sys->numVars(); j++) k[1][j] = step*dq[j];
00103         // Compute k3
00104         for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] + 0.25*(k[0][j]+k[1][j]);
00105         this->sys->der_func(t,dq);
00106         for (int j = 0; j < this->sys->numVars(); j++) k[2][j] = step*dq[j];
00107         // Compute k4
00108         for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] - k[1][j] + 2.0*k[2][j];
00109         this->sys->der_func(t,dq);
00110         for (int j = 0; j < this->sys->numVars(); j++) k[3][j] = step*dq[j];
00111         // Compute k5
00112         for (int j = 0; j < this->sys->numVars(); j++)
00113                 t[j] = qq[j] + (7.0/27.0)*k[0][j] + (10.0/27.0)*k[1][j] + (1.0/27.0)*k[3][j];
00114         this->sys->der_func(t,dq);
00115         for (int j = 0; j < this->sys->numVars(); j++) k[4][j] = step*dq[j];
00116         // Compute k6
00117         for (int j = 0; j < this->sys->numVars(); j++)
00118                 t[j] = qq[j] + (28.0/625.0)*k[0][j] - 0.2*k[1][j] + (546.0/625.0)*k[2][j]
00119                         + (54.0/625.0)*k[3][j] - (378.0/625.0)*k[4][j];
00120         this->sys->der_func(t,dq);
00121         for (int j = 0 ; j < this->sys->numVars(); j++) k[5][j] = step*dq[j];
00122         // Compute next state and the approximate error
00123         double err = 0.0;
00124         for (int j = 0; j < this->sys->numVars(); j++)
00125         {
00126                 // Next state
00127                 qq[j] += (1.0/24.0)*k[0][j] + (5.0/48.0)*k[3][j] + 
00128                         (27.0/56.0)*k[4][j] + (125.0/336.0)*k[5][j];
00129                 // Componennt wise maximum of the approximate error
00130                 err = std::max(err,
00131                                 fabs(k[0][j]/8.0+2.0*k[2][j]/3.0+k[3][j]/16.0-27.0*k[4][j]/56.0
00132                                         -125.0*k[5][j]/336.0));
00133         }
00134         // Return the error
00135         return err;
00136 }
00137 
00138 } // end of namespace
00139 #endif
00140