00001 #ifndef _adevs_hybrid_h_
00002 #define _adevs_hybrid_h_
00003 #include <algorithm>
00004 #include <cmath>
00005 #include "adevs_models.h"
00006
00007 namespace adevs
00008 {
00009
00015 template <typename X> class ode_system
00016 {
00017 public:
00019 ode_system(int N_vars, int M_event_funcs):
00020 N(N_vars),M(M_event_funcs){}
00022 int numVars() const { return N; }
00024 int numEvents() const { return M; }
00026 virtual void init(double* q) = 0;
00028 virtual void der_func(const double* q, double* dq) = 0;
00030 virtual void state_event_func(const double* q, double* z) = 0;
00032 virtual double time_event_func(const double* q) = 0;
00038 virtual void postStep(double* q){};
00040 virtual void internal_event(double* q,
00041 const bool* state_event) = 0;
00043 virtual void external_event(double* q, double e,
00044 const Bag<X>& xb) = 0;
00046 virtual void confluent_event(double *q, const bool* state_event,
00047 const Bag<X>& xb) = 0;
00049 virtual void output_func(const double *q, const bool* state_event,
00050 Bag<X>& yb) = 0;
00052 virtual void gc_output(Bag<X>& gb) = 0;
00054 virtual ~ode_system(){}
00055 private:
00056 const int N, M;
00057 };
00058
00072 template <typename X> class dae_se1_system:
00073 public ode_system<X>
00074 {
00075 public:
00083 dae_se1_system(int N_vars, int M_event_funcs, int A_alg_vars,
00084 double err_tol = 1E-10, int max_iters = 30, double alpha = -1.0):
00085 ode_system<X>(N_vars,M_event_funcs),
00086 A(A_alg_vars),
00087 max_iters(max_iters),
00088 err_tol(err_tol),
00089 alpha(alpha)
00090 {
00091 failed = 0;
00092 max_err = 0.0;
00093 a = new double[A];
00094 atmp = new double[A];
00095 d = new double[A];
00096 f[1] = new double[A];
00097 f[0] = new double[A];
00098 }
00100 int numAlgVars() const { return A; }
00102 double getAlgVar(int i) const { return a[i]; }
00107 virtual void init(double* q, double* a) = 0;
00114 virtual void alg_func(const double* q, const double* a, double* af) = 0;
00119 virtual void der_func(const double* q, const double* a, double* dq) = 0;
00123 virtual void state_event_func(const double* q, const double* a, double* z) = 0;
00125 virtual double time_event_func(const double* q, const double* a) = 0;
00131 virtual void postStep(double* q, double* a) = 0;
00133 virtual void internal_event(double* q, double* a,
00134 const bool* state_event) = 0;
00136 virtual void external_event(double* q, double* a,
00137 double e, const Bag<X>& xb) = 0;
00139 virtual void confluent_event(double *q, double* a,
00140 const bool* state_event, const Bag<X>& xb) = 0;
00142 virtual void output_func(const double *q, const double* a,
00143 const bool* state_event, Bag<X>& yb) = 0;
00145 virtual ~dae_se1_system()
00146 {
00147 delete [] d;
00148 delete [] a;
00149 delete [] atmp;
00150 delete [] f[1];
00151 delete [] f[0];
00152 }
00157 int getIterFailCount() const { return failed; }
00163 double getWorseError() const { return max_err; }
00165 void init(double* q)
00166 {
00167 init(q,a);
00168 }
00170 void der_func(const double* q, double* dq)
00171 {
00172 solve(q);
00173 der_func(q,a,dq);
00174 }
00176 void state_event_func(const double* q, double* z)
00177 {
00178 solve(q);
00179 state_event_func(q,a,z);
00180 }
00182 double time_event_func(const double* q)
00183 {
00184 solve(q);
00185 return time_event_func(q,a);
00186 }
00188 void postStep(double* q)
00189 {
00190 solve(q);
00191 postStep(q,a);
00192 }
00194 void internal_event(double* q, const bool* state_event)
00195 {
00196
00197 internal_event(q,a,state_event);
00198
00199 solve(q);
00200 postStep(q,a);
00201 }
00203 void external_event(double* q, double e, const Bag<X>& xb)
00204 {
00205
00206 external_event(q,a,e,xb);
00207
00208 solve(q);
00209 postStep(q,a);
00210 }
00212 void confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
00213 {
00214
00215 confluent_event(q,a,state_event,xb);
00216
00217 solve(q);
00218 postStep(q,a);
00219 }
00221 void output_func(const double *q, const bool* state_event, Bag<X>& yb)
00222 {
00223
00224 output_func(q,a,state_event,yb);
00225 }
00226 protected:
00234 void solve(const double* q);
00235 private:
00236 const int A, max_iters;
00237 const double err_tol, alpha;
00238
00239 double *a, *atmp;
00240
00241 double* f[2];
00242
00243 double* d;
00244
00245 double max_err;
00246
00247 int failed;
00248 };
00249
00250 template <typename X>
00251 void dae_se1_system<X>::solve(const double* q)
00252 {
00253 int iter_count = 0, alt, good;
00254 double prev_err, err = 0.0, ee, beta, g2, alpha_tmp = alpha;
00260 _adevs_dae_se_1_system_solve_try_it_again:
00261 alt = 0;
00262 good = 1;
00263 prev_err = DBL_MAX;
00264
00265 alg_func(q,a,f[alt]);
00266 for (int i = 0; i < A; i++)
00267 {
00268
00269 f[alt][i] -= a[i];
00270
00271 d[i] = -f[alt][i];
00272
00273 atmp[i] = a[i];
00274 a[i] += alpha_tmp*d[i];
00275 }
00276
00277
00278 while (iter_count < max_iters)
00279 {
00280 iter_count++;
00281 err = 0.0;
00282
00283 alg_func(q,a,f[good]);
00284
00285 for (int i = 0; i < A; i++)
00286 {
00287
00288 f[good][i] -= a[i];
00289
00290 ee = fabs(f[good][i]);
00291 if (ee > err) err = ee;
00292 }
00293
00294 if (err < err_tol) return;
00295
00296 if (err > prev_err)
00297 {
00298
00299 for (int i = 0; i < A; i++)
00300 a[i] = atmp[i];
00301
00302 if (alpha_tmp < 0.0) alpha_tmp = -alpha_tmp;
00303 else alpha_tmp *= -0.5;
00304 goto _adevs_dae_se_1_system_solve_try_it_again;
00305 }
00306 prev_err = err;
00307
00308
00309 beta = g2 = 0.0;
00310 for (int i = 0; i < A; i++)
00311 g2 += f[alt][i]*f[alt][i];
00312 for (int i = 0; i < A; i++)
00313 beta += f[good][i]*(f[good][i]-f[alt][i]);
00314 beta /= g2;
00315
00316 for (int i = 0; i < A; i++)
00317 {
00318 d[i] = beta*d[i]-f[good][i];
00319 atmp[i] = a[i];
00320 a[i] += alpha_tmp*d[i];
00321 }
00322
00323 good = alt;
00324 alt = (good+1)%2;
00325 }
00326
00327
00328 failed++;
00329 if (err > max_err)
00330 max_err = err;
00331 }
00332
00337 template <typename X> class ode_solver
00338 {
00339 public:
00344 ode_solver(ode_system<X>* sys):sys(sys){}
00350 virtual double integrate(double* q, double h_lim) = 0;
00354 virtual void advance(double* q, double h) = 0;
00356 virtual ~ode_solver(){}
00357 protected:
00358 ode_system<X>* sys;
00359 };
00360
00366 template <typename X> class event_locator
00367 {
00368 public:
00373 event_locator(ode_system<X>* sys):sys(sys){}
00384 virtual bool find_events(bool* events, const double *qstart,
00385 double* qend, ode_solver<X>* solver, double& h) = 0;
00387 virtual ~event_locator(){}
00388 protected:
00389 ode_system<X>* sys;
00390 };
00391
00400 template <typename X, class T = double> class Hybrid:
00401 public Atomic<X,T>
00402 {
00403 public:
00408 Hybrid(ode_system<X>* sys, ode_solver<X>* solver,
00409 event_locator<X>* event_finder):
00410 sys(sys),solver(solver),event_finder(event_finder),
00411 e_accum(0.0)
00412 {
00413 q = new double[sys->numVars()];
00414 q_trial = new double[sys->numVars()];
00415 event = new bool[sys->numEvents()+1];
00416 event_exists = false;
00417 sys->init(q_trial);
00418 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00419 tentative_step();
00420 }
00422 double getState(int k) const { return q[k]; }
00424 const double* getState() const { return q; }
00426 ode_system<X>* getSystem() { return sys; }
00428 bool eventHappened() const { return event_happened; }
00433 void delta_int()
00434 {
00435 if (!missedOutput.empty())
00436 {
00437 missedOutput.clear();
00438 return;
00439 }
00440 e_accum += ta();
00441
00442 event_happened = event_exists;
00443 if (event_exists)
00444 {
00445 sys->internal_event(q_trial,event);
00446 e_accum = 0.0;
00447 }
00448
00449 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00450 tentative_step();
00451 }
00456 void delta_ext(T e, const Bag<X>& xb)
00457 {
00458 bool state_event_exists = false;
00459 event_happened = true;
00460
00461 if (event_exists)
00462 {
00463 for (int i = 0; i < sys->numVars(); i++)
00464 q_trial[i] = q[i];
00465 solver->advance(q_trial,e);
00466 state_event_exists =
00467 event_finder->find_events(event,q,q_trial,solver,e);
00468
00469 if (state_event_exists)
00470 {
00471 output_func(missedOutput);
00472 sys->confluent_event(q_trial,event,xb);
00473 for (int i = 0; i < sys->numVars(); i++)
00474 q[i] = q_trial[i];
00475 }
00476 }
00477 if (!state_event_exists)
00478 {
00479 solver->advance(q,e);
00480
00481 sys->postStep(q);
00482
00483 sys->external_event(q,e+e_accum,xb);
00484 }
00485 e_accum = 0.0;
00486
00487 for (int i = 0; i < sys->numVars(); i++) q_trial[i] = q[i];
00488 tentative_step();
00489 }
00494 void delta_conf(const Bag<X>& xb)
00495 {
00496 if (!missedOutput.empty())
00497 {
00498 missedOutput.clear();
00499 if (sigma > 0.0) event_exists = false;
00500 }
00501
00502 event_happened = true;
00503 if (event_exists)
00504 sys->confluent_event(q_trial,event,xb);
00505 else sys->external_event(q_trial,e_accum+ta(),xb);
00506 e_accum = 0.0;
00507
00508 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00509 tentative_step();
00510 }
00512 T ta()
00513 {
00514 if (missedOutput.empty()) return sigma;
00515 else return 0.0;
00516 }
00518 void output_func(Bag<X>& yb)
00519 {
00520 if (!missedOutput.empty())
00521 {
00522 typename Bag<X>::iterator iter = missedOutput.begin();
00523 for (; iter != missedOutput.end(); iter++)
00524 yb.insert(*iter);
00525 if (sigma == 0.0)
00526 sys->output_func(q_trial,event,yb);
00527 }
00528 else
00529 {
00530
00531 sys->postStep(q_trial);
00532 if (event_exists)
00533 sys->output_func(q_trial,event,yb);
00534 }
00535 }
00537 void gc_output(Bag<X>& gb) { sys->gc_output(gb); }
00539 virtual ~Hybrid()
00540 {
00541 delete [] q; delete [] q_trial; delete [] event;
00542 delete event_finder; delete solver; delete sys;
00543 }
00544 private:
00545 ode_system<X>* sys;
00546 ode_solver<X>* solver;
00547 event_locator<X>* event_finder;
00548 double sigma;
00549 double *q, *q_trial;
00550 bool* event;
00551 bool event_exists;
00552 bool event_happened;
00553 double e_accum;
00554 Bag<X> missedOutput;
00555
00556 void tentative_step()
00557 {
00558
00559 double time_event = sys->time_event_func(q);
00560
00561 double step_size = solver->integrate(q_trial,time_event);
00562
00563 bool state_event_exists =
00564 event_finder->find_events(event,q,q_trial,solver,step_size);
00565
00566 sigma = std::min(step_size,time_event);
00567 event[sys->numEvents()] = time_event <= sigma;
00568 event_exists = event[sys->numEvents()] || state_event_exists;
00569 }
00570 };
00571
00572 }
00573
00574 #endif