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,
00029 *qq,
00030 *t,
00031 *k[6];
00032 const double err_tol;
00033 const double h_max;
00034 double h_cur;
00035
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
00070 double err = DBL_MAX, h = std::min(h_cur*1.1,std::min(h_max,h_lim));
00071 for (;;) {
00072
00073 for (int i = 0; i < this->sys->numVars(); i++) qq[i] = q[i];
00074
00075 err = trial_step(h);
00076
00077 if (err <= err_tol) {
00078 if (h_cur <= h_lim) h_cur = h;
00079 break;
00080 }
00081
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
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
00097 this->sys->der_func(qq,dq);
00098 for (int j = 0; j < this->sys->numVars(); j++) k[0][j] = step*dq[j];
00099
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
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
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
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
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
00123 double err = 0.0;
00124 for (int j = 0; j < this->sys->numVars(); j++)
00125 {
00126
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
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
00135 return err;
00136 }
00137
00138 }
00139 #endif
00140