00001
00031 #ifndef _adevs_hybrid_h_
00032 #define _adevs_hybrid_h_
00033 #include <algorithm>
00034 #include <cmath>
00035 #include "adevs_models.h"
00036
00037 namespace adevs
00038 {
00039
00045 template <typename X> class ode_system
00046 {
00047 public:
00049 ode_system(int N_vars, int M_event_funcs):
00050 N(N_vars),M(M_event_funcs){}
00052 int numVars() const { return N; }
00054 int numEvents() const { return M; }
00056 virtual void init(double* q) = 0;
00058 virtual void der_func(const double* q, double* dq) = 0;
00060 virtual void state_event_func(const double* q, double* z) = 0;
00062 virtual double time_event_func(const double* q) = 0;
00068 virtual void postStep(double* q){};
00070 virtual void internal_event(double* q,
00071 const bool* state_event) = 0;
00073 virtual void external_event(double* q, double e,
00074 const Bag<X>& xb) = 0;
00076 virtual void confluent_event(double *q, const bool* state_event,
00077 const Bag<X>& xb) = 0;
00079 virtual void output_func(const double *q, const bool* state_event,
00080 Bag<X>& yb) = 0;
00082 virtual void gc_output(Bag<X>& gb) = 0;
00084 virtual ~ode_system(){}
00085 private:
00086 const int N, M;
00087 };
00088
00102 template <typename X> class dae_se1_system:
00103 public ode_system<X>
00104 {
00105 public:
00113 dae_se1_system(int N_vars, int M_event_funcs, int A_alg_vars,
00114 double err_tol = 1E-10, int max_iters = 30, double alpha = -1.0):
00115 ode_system<X>(N_vars,M_event_funcs),
00116 A(A_alg_vars),
00117 max_iters(max_iters),
00118 err_tol(err_tol),
00119 alpha(alpha)
00120 {
00121 failed = 0;
00122 max_err = 0.0;
00123 a = new double[A];
00124 atmp = new double[A];
00125 d = new double[A];
00126 f[1] = new double[A];
00127 f[0] = new double[A];
00128 }
00130 int numAlgVars() const { return A; }
00132 double getAlgVar(int i) const { return a[i]; }
00137 virtual void init(double* q, double* a) = 0;
00144 virtual void alg_func(const double* q, const double* a, double* af) = 0;
00149 virtual void der_func(const double* q, const double* a, double* dq) = 0;
00153 virtual void state_event_func(const double* q, const double* a, double* z) = 0;
00155 virtual double time_event_func(const double* q, const double* a) = 0;
00161 virtual void postStep(double* q, double* a) = 0;
00163 virtual void internal_event(double* q, double* a,
00164 const bool* state_event) = 0;
00166 virtual void external_event(double* q, double* a,
00167 double e, const Bag<X>& xb) = 0;
00169 virtual void confluent_event(double *q, double* a,
00170 const bool* state_event, const Bag<X>& xb) = 0;
00172 virtual void output_func(const double *q, const double* a,
00173 const bool* state_event, Bag<X>& yb) = 0;
00175 virtual ~dae_se1_system()
00176 {
00177 delete [] d;
00178 delete [] a;
00179 delete [] atmp;
00180 delete [] f[1];
00181 delete [] f[0];
00182 }
00187 int getIterFailCount() const { return failed; }
00193 double getWorseError() const { return max_err; }
00195 void init(double* q)
00196 {
00197 init(q,a);
00198 }
00200 void der_func(const double* q, double* dq)
00201 {
00202 solve(q);
00203 der_func(q,a,dq);
00204 }
00206 void state_event_func(const double* q, double* z)
00207 {
00208 solve(q);
00209 state_event_func(q,a,z);
00210 }
00212 double time_event_func(const double* q)
00213 {
00214 solve(q);
00215 return time_event_func(q,a);
00216 }
00218 void postStep(double* q)
00219 {
00220 solve(q);
00221 postStep(q,a);
00222 }
00224 void internal_event(double* q, const bool* state_event)
00225 {
00226
00227 internal_event(q,a,state_event);
00228
00229 solve(q);
00230 postStep(q,a);
00231 }
00233 void external_event(double* q, double e, const Bag<X>& xb)
00234 {
00235
00236 external_event(q,a,e,xb);
00237
00238 solve(q);
00239 postStep(q,a);
00240 }
00242 void confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
00243 {
00244
00245 confluent_event(q,a,state_event,xb);
00246
00247 solve(q);
00248 postStep(q,a);
00249 }
00251 void output_func(const double *q, const bool* state_event, Bag<X>& yb)
00252 {
00253
00254 output_func(q,a,state_event,yb);
00255 }
00256 protected:
00264 void solve(const double* q);
00265 private:
00266 const int A, max_iters;
00267 const double err_tol, alpha;
00268
00269 double *a, *atmp;
00270
00271 double* f[2];
00272
00273 double* d;
00274
00275 double max_err;
00276
00277 int failed;
00278 };
00279
00280 template <typename X>
00281 void dae_se1_system<X>::solve(const double* q)
00282 {
00283 int iter_count = 0, alt, good;
00284 double prev_err, err = 0.0, ee, beta, g2, alpha_tmp = alpha;
00290 _adevs_dae_se_1_system_solve_try_it_again:
00291 alt = 0;
00292 good = 1;
00293 prev_err = DBL_MAX;
00294
00295 alg_func(q,a,f[alt]);
00296 for (int i = 0; i < A; i++)
00297 {
00298
00299 f[alt][i] -= a[i];
00300
00301 d[i] = -f[alt][i];
00302
00303 atmp[i] = a[i];
00304 a[i] += alpha_tmp*d[i];
00305 }
00306
00307
00308 while (iter_count < max_iters)
00309 {
00310 iter_count++;
00311 err = 0.0;
00312
00313 alg_func(q,a,f[good]);
00314
00315 for (int i = 0; i < A; i++)
00316 {
00317
00318 f[good][i] -= a[i];
00319
00320 ee = fabs(f[good][i]);
00321 if (ee > err) err = ee;
00322 }
00323
00324 if (err < err_tol) return;
00325
00326 if (err > prev_err)
00327 {
00328
00329 for (int i = 0; i < A; i++)
00330 a[i] = atmp[i];
00331
00332 if (alpha_tmp < 0.0) alpha_tmp = -alpha_tmp;
00333 else alpha_tmp *= -0.5;
00334 goto _adevs_dae_se_1_system_solve_try_it_again;
00335 }
00336 prev_err = err;
00337
00338
00339 beta = g2 = 0.0;
00340 for (int i = 0; i < A; i++)
00341 g2 += f[alt][i]*f[alt][i];
00342 for (int i = 0; i < A; i++)
00343 beta += f[good][i]*(f[good][i]-f[alt][i]);
00344 beta /= g2;
00345
00346 for (int i = 0; i < A; i++)
00347 {
00348 d[i] = beta*d[i]-f[good][i];
00349 atmp[i] = a[i];
00350 a[i] += alpha_tmp*d[i];
00351 }
00352
00353 good = alt;
00354 alt = (good+1)%2;
00355 }
00356
00357
00358 failed++;
00359 if (err > max_err)
00360 max_err = err;
00361 }
00362
00367 template <typename X> class ode_solver
00368 {
00369 public:
00374 ode_solver(ode_system<X>* sys):sys(sys){}
00380 virtual double integrate(double* q, double h_lim) = 0;
00384 virtual void advance(double* q, double h) = 0;
00386 virtual ~ode_solver(){}
00387 protected:
00388 ode_system<X>* sys;
00389 };
00390
00396 template <typename X> class event_locator
00397 {
00398 public:
00403 event_locator(ode_system<X>* sys):sys(sys){}
00414 virtual bool find_events(bool* events, const double *qstart,
00415 double* qend, ode_solver<X>* solver, double& h) = 0;
00417 virtual ~event_locator(){}
00418 protected:
00419 ode_system<X>* sys;
00420 };
00421
00430 template <typename X, class T = double> class Hybrid:
00431 public Atomic<X,T>
00432 {
00433 public:
00438 Hybrid(ode_system<X>* sys, ode_solver<X>* solver,
00439 event_locator<X>* event_finder):
00440 sys(sys),solver(solver),event_finder(event_finder),
00441 e_accum(0.0)
00442 {
00443 q = new double[sys->numVars()];
00444 q_trial = new double[sys->numVars()];
00445 event = new bool[sys->numEvents()+1];
00446 event_exists = false;
00447 sys->init(q_trial);
00448 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00449 tentative_step();
00450 }
00452 double getState(int k) const { return q[k]; }
00454 const double* getState() const { return q; }
00456 ode_system<X>* getSystem() { return sys; }
00458 bool eventHappened() const { return event_happened; }
00463 void delta_int()
00464 {
00465 if (!missedOutput.empty())
00466 {
00467 missedOutput.clear();
00468 return;
00469 }
00470 e_accum += ta();
00471
00472 event_happened = event_exists;
00473 if (event_exists)
00474 {
00475 sys->internal_event(q_trial,event);
00476 e_accum = 0.0;
00477 }
00478
00479 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00480 tentative_step();
00481 }
00486 void delta_ext(T e, const Bag<X>& xb)
00487 {
00488 bool state_event_exists = false;
00489 event_happened = true;
00490
00491 if (event_exists)
00492 {
00493 for (int i = 0; i < sys->numVars(); i++)
00494 q_trial[i] = q[i];
00495 solver->advance(q_trial,e);
00496 state_event_exists =
00497 event_finder->find_events(event,q,q_trial,solver,e);
00498
00499 if (state_event_exists)
00500 {
00501 output_func(missedOutput);
00502 sys->confluent_event(q_trial,event,xb);
00503 for (int i = 0; i < sys->numVars(); i++)
00504 q[i] = q_trial[i];
00505 }
00506 }
00507 if (!state_event_exists)
00508 {
00509 solver->advance(q,e);
00510
00511 sys->postStep(q);
00512
00513 sys->external_event(q,e+e_accum,xb);
00514 }
00515 e_accum = 0.0;
00516
00517 for (int i = 0; i < sys->numVars(); i++) q_trial[i] = q[i];
00518 tentative_step();
00519 }
00524 void delta_conf(const Bag<X>& xb)
00525 {
00526 if (!missedOutput.empty())
00527 {
00528 missedOutput.clear();
00529 if (sigma > 0.0) event_exists = false;
00530 }
00531
00532 event_happened = true;
00533 if (event_exists)
00534 sys->confluent_event(q_trial,event,xb);
00535 else sys->external_event(q_trial,e_accum+ta(),xb);
00536 e_accum = 0.0;
00537
00538 for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00539 tentative_step();
00540 }
00542 T ta()
00543 {
00544 if (missedOutput.empty()) return sigma;
00545 else return 0.0;
00546 }
00548 void output_func(Bag<X>& yb)
00549 {
00550 if (!missedOutput.empty())
00551 {
00552 typename Bag<X>::iterator iter = missedOutput.begin();
00553 for (; iter != missedOutput.end(); iter++)
00554 yb.insert(*iter);
00555 if (sigma == 0.0)
00556 sys->output_func(q_trial,event,yb);
00557 }
00558 else
00559 {
00560
00561 sys->postStep(q_trial);
00562 if (event_exists)
00563 sys->output_func(q_trial,event,yb);
00564 }
00565 }
00567 void gc_output(Bag<X>& gb) { sys->gc_output(gb); }
00569 virtual ~Hybrid()
00570 {
00571 delete [] q; delete [] q_trial; delete [] event;
00572 delete event_finder; delete solver; delete sys;
00573 }
00574 private:
00575 ode_system<X>* sys;
00576 ode_solver<X>* solver;
00577 event_locator<X>* event_finder;
00578 double sigma;
00579 double *q, *q_trial;
00580 bool* event;
00581 bool event_exists;
00582 bool event_happened;
00583 double e_accum;
00584 Bag<X> missedOutput;
00585
00586 void tentative_step()
00587 {
00588
00589 double time_event = sys->time_event_func(q);
00590
00591 double step_size = solver->integrate(q_trial,time_event);
00592
00593 bool state_event_exists =
00594 event_finder->find_events(event,q,q_trial,solver,step_size);
00595
00596 sigma = std::min(step_size,time_event);
00597 event[sys->numEvents()] = time_event <= sigma;
00598 event_exists = event[sys->numEvents()] || state_event_exists;
00599 }
00600 };
00601
00602 }
00603
00604 #endif