00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _adevs_rk45_h_
00021 #define _adevs_rk45_h_
00022 #include "adevs_dess.h"
00023 #include <cmath>
00024 #include <algorithm>
00025
00026 namespace adevs
00027 {
00028
00039 template <class X> class rk45: public DESS<X>
00040 {
00041 public:
00049 rk45(int num_state_vars, double h_max, double err_tol,
00050 int zero_crossing_funcs, double event_tol = 1E-12);
00054 void init(int i, double q0) { q[i] = q0; }
00058 const double* getStateVars() const { return q; }
00062 int getNumStateVars() const { return num_state_vars; }
00067 virtual void der_func(const double* q, double* dq) = 0;
00074 virtual void state_event_func(const double* q, double* z) = 0;
00079 virtual double time_event_func(const double* q) = 0;
00087 virtual void discrete_action(double* q, const Bag<X>& xb) = 0;
00092 virtual void discrete_output(const double* q, Bag<X>& yb) = 0;
00101 virtual void state_changed(const double* q){}
00102
00103 void evolve_func(double h);
00104
00105 double next_event_func(bool& is_event);
00106
00107 void discrete_action_func(const Bag<X>& xb);
00108
00109 void discrete_output_func(Bag<X>& yb);
00110
00111 void state_changed() { state_changed(q); }
00113 ~rk45();
00114
00115 private:
00116
00117
00118 const double h_max, err_tol, event_tol;
00119
00120
00121 double *q, *dq, *t, *k[6], *q_tmp, *es, *en;
00122
00123 double h_cur;
00124
00125 int num_state_vars, zero_funcs;
00126
00127
00128
00129
00130
00131 double ode_step(double *qq, double step);
00132
00133 static int sgn(double x)
00134 {
00135 if (x < 0.0) return -1;
00136 else if (x > 0.0) return 1;
00137 else return 0;
00138 }
00139 };
00140
00141 template <class X>
00142 rk45<X>::rk45(int num_state_vars, double h_max, double err_tol, int zero_funcs, double event_tol):
00143 DESS<X>(),
00144 h_max(h_max),
00145 err_tol(err_tol),
00146 event_tol(event_tol),
00147 num_state_vars(num_state_vars),
00148 zero_funcs(zero_funcs)
00149 {
00150 h_cur = h_max;
00151 q = new double[num_state_vars];
00152 dq = new double[num_state_vars];
00153 t = new double[num_state_vars];
00154 q_tmp = new double[num_state_vars];
00155 for (int i = 0; i < 6; i++)
00156 k[i] = new double[num_state_vars];
00157 en = new double[zero_funcs];
00158 es = new double[zero_funcs];
00159 }
00160
00161 template <class X>
00162 rk45<X>::~rk45()
00163 {
00164 delete [] q;
00165 delete [] dq;
00166 delete [] t;
00167 delete [] q_tmp;
00168 for (int i = 0; i < 6; i++)
00169 delete [] k[i];
00170 delete [] es;
00171 delete [] en;
00172 }
00173
00174 template <class X>
00175 void rk45<X>::evolve_func(double h)
00176 {
00177 ode_step(q,h);
00178 }
00179
00180 template <class X>
00181 double rk45<X>::next_event_func(bool& is_event)
00182 {
00183
00184 double time_event = time_event_func(q);
00185
00186 h_cur = std::min(h_max,h_cur*1.1);
00187 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00188 while (ode_step(q_tmp,h_cur) > err_tol)
00189 {
00190 h_cur *= 0.9;
00191 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00192 }
00193
00194 double sigma = h_cur;
00195 state_event_func(q,es);
00196 for (int i = 0; i < zero_funcs; i++) en[i] = es[i];
00197 while (true)
00198 {
00199
00200 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00201
00202
00203
00204
00205 ode_step(q_tmp,sigma);
00206 state_event_func(q_tmp,en);
00207
00208 is_event = false;
00209 for (int i = 0; i < zero_funcs && !is_event; i++)
00210 {
00211 is_event = (sgn(es[i]) != sgn(en[i]));
00212 }
00213 if (!is_event) break;
00214
00215 sigma /= 2.0;
00216
00217 if (sigma < event_tol)
00218 {
00219 is_event = true;
00220 break;
00221 }
00222 }
00223
00224 if (sigma < time_event)
00225 {
00226 return sigma;
00227 }
00228 is_event = true;
00229 return time_event;
00230 }
00231
00232 template <class X>
00233 void rk45<X>::discrete_action_func(const Bag<X>& xb)
00234 {
00235 discrete_action(q,xb);
00236 }
00237
00238 template <class X>
00239 void rk45<X>::discrete_output_func(Bag<X>& yb)
00240 {
00241 discrete_output(q,yb);
00242 }
00243
00244 template <class X>
00245 double rk45<X>::ode_step(double*qq, double step)
00246 {
00247 if (step == 0.0)
00248 {
00249 return 0.0;
00250 }
00251
00252 der_func(qq,dq);
00253 for (int j = 0; j < num_state_vars; j++)
00254 k[0][j] = step*dq[j];
00255
00256 for (int j = 0; j < num_state_vars; j++)
00257 t[j] = qq[j] + 0.5*k[0][j];
00258 der_func(t,dq);
00259 for (int j = 0; j < num_state_vars; j++)
00260 k[1][j] = step*dq[j];
00261
00262 for (int j = 0; j < num_state_vars; j++)
00263 t[j] = qq[j] + 0.25*(k[0][j]+k[1][j]);
00264 der_func(t,dq);
00265 for (int j = 0; j < num_state_vars; j++)
00266 k[2][j] = step*dq[j];
00267
00268 for (int j = 0; j < num_state_vars; j++)
00269 t[j] = qq[j] - k[1][j] + 2.0*k[2][j];
00270 der_func(t,dq);
00271 for (int j = 0; j < num_state_vars; j++)
00272 k[3][j] = step*dq[j];
00273
00274 for (int j = 0; j < num_state_vars; j++)
00275 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];
00276 der_func(t,dq);
00277 for (int j = 0; j < num_state_vars; j++)
00278 k[4][j] = step*dq[j];
00279
00280 for (int j = 0; j < num_state_vars; j++)
00281 t[j] = qq[j] + (28.0/625.0)*k[0][j] - 0.2*k[1][j] + (546.0/625.0)*k[2][j]
00282 + (54.0/625.0)*k[3][j] - (378.0/625.0)*k[4][j];
00283 der_func(t,dq);
00284 for (int j = 0 ; j < num_state_vars; j++)
00285 k[5][j] = step*dq[j];
00286
00287 double err = 0.0;
00288 for (int j = 0; j < num_state_vars; j++)
00289 {
00290 qq[j] += (1.0/24.0)*k[0][j] + (5.0/48.0)*k[3][j] +
00291 (27.0/56.0)*k[4][j] + (125.0/336.0)*k[5][j];
00292 err = std::max(err,
00293 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
00294 -125.0*k[5][j]/336.0));
00295 }
00296 return err;
00297 }
00298
00299 }
00300
00301 #endif