00001
00031 #ifndef _adevs_rk_45_h_
00032 #define _adevs_rk_45_h_
00033 #include "adevs_hybrid.h"
00034 #include <cmath>
00035
00036 namespace adevs
00037 {
00038
00043 template <typename X> class rk_45:
00044 public ode_solver<X>
00045 {
00046 public:
00052 rk_45(ode_system<X>* sys, double err_tol, double h_max);
00054 ~rk_45();
00055 double integrate(double* q, double h_lim);
00056 void advance(double* q, double h);
00057 private:
00058 double *dq,
00059 *qq,
00060 *t,
00061 *k[6];
00062 const double err_tol;
00063 const double h_max;
00064 double h_cur;
00065
00066 double trial_step(double h);
00067 };
00068
00069 template <typename X>
00070 rk_45<X>::rk_45(ode_system<X>* sys, double err_tol, double h_max):
00071 ode_solver<X>(sys),err_tol(err_tol),h_max(h_max),h_cur(h_max)
00072 {
00073 for (int i = 0; i < 6; i++)
00074 k[i] = new double[sys->numVars()];
00075 dq = new double[sys->numVars()];
00076 qq = new double[sys->numVars()];
00077 t = new double[sys->numVars()];
00078 }
00079
00080 template <typename X>
00081 rk_45<X>::~rk_45()
00082 {
00083 delete [] dq;
00084 delete [] t;
00085 for (int i = 0; i < 6; i++)
00086 delete [] k[i];
00087 }
00088
00089 template <typename X>
00090 void rk_45<X>::advance(double* q, double h)
00091 {
00092 double dt;
00093 while ((dt = integrate(q,h)) < h) h -= dt;
00094 }
00095
00096 template <typename X>
00097 double rk_45<X>::integrate(double* q, double h_lim)
00098 {
00099
00100 double err = DBL_MAX, h = std::min(h_cur*1.1,std::min(h_max,h_lim));
00101 for (;;) {
00102
00103 for (int i = 0; i < this->sys->numVars(); i++) qq[i] = q[i];
00104
00105 err = trial_step(h);
00106
00107 if (err <= err_tol) {
00108 if (h_cur <= h_lim) h_cur = h;
00109 break;
00110 }
00111
00112 else {
00113 double h_guess = 0.8*pow(err_tol*pow(h,4.0)/fabs(err),0.25);
00114 if (h < h_guess) h *= 0.8;
00115 else h = h_guess;
00116 }
00117 }
00118
00119 for (int i = 0; i < this->sys->numVars(); i++) q[i] = qq[i];
00120 return h;
00121 }
00122
00123 template <typename X>
00124 double rk_45<X>::trial_step(double step)
00125 {
00126
00127 this->sys->der_func(qq,dq);
00128 for (int j = 0; j < this->sys->numVars(); j++) k[0][j] = step*dq[j];
00129
00130 for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] + 0.5*k[0][j];
00131 this->sys->der_func(t,dq);
00132 for (int j = 0; j < this->sys->numVars(); j++) k[1][j] = step*dq[j];
00133
00134 for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] + 0.25*(k[0][j]+k[1][j]);
00135 this->sys->der_func(t,dq);
00136 for (int j = 0; j < this->sys->numVars(); j++) k[2][j] = step*dq[j];
00137
00138 for (int j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] - k[1][j] + 2.0*k[2][j];
00139 this->sys->der_func(t,dq);
00140 for (int j = 0; j < this->sys->numVars(); j++) k[3][j] = step*dq[j];
00141
00142 for (int j = 0; j < this->sys->numVars(); j++)
00143 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];
00144 this->sys->der_func(t,dq);
00145 for (int j = 0; j < this->sys->numVars(); j++) k[4][j] = step*dq[j];
00146
00147 for (int j = 0; j < this->sys->numVars(); j++)
00148 t[j] = qq[j] + (28.0/625.0)*k[0][j] - 0.2*k[1][j] + (546.0/625.0)*k[2][j]
00149 + (54.0/625.0)*k[3][j] - (378.0/625.0)*k[4][j];
00150 this->sys->der_func(t,dq);
00151 for (int j = 0 ; j < this->sys->numVars(); j++) k[5][j] = step*dq[j];
00152
00153 double err = 0.0;
00154 for (int j = 0; j < this->sys->numVars(); j++)
00155 {
00156
00157 qq[j] += (1.0/24.0)*k[0][j] + (5.0/48.0)*k[3][j] +
00158 (27.0/56.0)*k[4][j] + (125.0/336.0)*k[5][j];
00159
00160 err = std::max(err,
00161 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
00162 -125.0*k[5][j]/336.0));
00163 }
00164
00165 return err;
00166 }
00167
00168 }
00169 #endif
00170