00001
00031 #ifndef _adevs_corrected_euler_h_
00032 #define _adevs_corrected_euler_h_
00033 #include <cmath>
00034 #include "adevs_hybrid.h"
00035
00036 namespace adevs
00037 {
00038
00043 template <typename X> class corrected_euler:
00044 public ode_solver<X>
00045 {
00046 public:
00051 corrected_euler(ode_system<X>* sys, double err_tol, double h_max);
00053 ~corrected_euler();
00054 double integrate(double* q, double h_lim);
00055 void advance(double* q, double h);
00056 private:
00057 double *dq,
00058 *qq,
00059 *t,
00060 *k[2];
00061 const double err_tol;
00062 const double h_max;
00063 double h_cur;
00064
00065 double trial_step(double h);
00066 };
00067
00068 template <typename X>
00069 corrected_euler<X>::corrected_euler(ode_system<X>* sys, double err_tol,
00070 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 < 2; i++) k[i] = new double[sys->numVars()];
00074 dq = new double[sys->numVars()];
00075 qq = new double[sys->numVars()];
00076 t = new double[sys->numVars()];
00077 }
00078
00079 template <typename X>
00080 corrected_euler<X>::~corrected_euler()
00081 {
00082 delete [] t; delete [] qq; delete [] dq;
00083 for (int i = 0; i < 2; i++) delete [] k[i];
00084 }
00085
00086 template <typename X>
00087 void corrected_euler<X>::advance(double* q, double h)
00088 {
00089 double dt;
00090 while ((dt = integrate(q,h)) < h) h -= dt;
00091 }
00092
00093 template <typename X>
00094 double corrected_euler<X>::integrate(double* q, double h_lim)
00095 {
00096
00097 double err = DBL_MAX, h = std::min(h_cur*1.1,std::min(h_max,h_lim));
00098 for (;;) {
00099
00100 for (int i = 0; i < this->sys->numVars(); i++) qq[i] = q[i];
00101
00102 err = trial_step(h);
00103
00104 if (err <= err_tol) {
00105 if (h_lim >= h_cur) h_cur = h;
00106 break;
00107 }
00108
00109 else {
00110 double h_guess = 0.8*err_tol*h/fabs(err);
00111 if (h < h_guess) h *= 0.8;
00112 else h = h_guess;
00113 }
00114 }
00115
00116 for (int i = 0; i < this->sys->numVars(); i++) q[i] = qq[i];
00117 return h;
00118 }
00119
00120 template <typename X>
00121 double corrected_euler<X>::trial_step(double step)
00122 {
00123 int j;
00124
00125 this->sys->der_func(qq,dq);
00126 for (j = 0; j < this->sys->numVars(); j++) k[0][j] = step*dq[j];
00127
00128 for (j = 0; j < this->sys->numVars(); j++) t[j] = qq[j] + 0.5*k[0][j];
00129 this->sys->der_func(t,dq);
00130 for (j = 0; j < this->sys->numVars(); j++) k[1][j] = step*dq[j];
00131
00132 double err = 0.0;
00133 for (j = 0; j < this->sys->numVars(); j++) {
00134 qq[j] += k[1][j];
00135 err = std::max(err,fabs(k[0][j]-k[1][j]));
00136 }
00137 return err;
00138 }
00139
00140 }
00141 #endif