adevs
|
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