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