adevs
adevs_hybrid.h
1 
31 #ifndef _adevs_hybrid_h_
32 #define _adevs_hybrid_h_
33 #include <algorithm>
34 #include <cmath>
35 #include "adevs_models.h"
36 
37 namespace adevs
38 {
39 
45 template <typename X> class ode_system
46 {
47  public:
49  ode_system(int N_vars, int M_event_funcs):
50  N(N_vars),M(M_event_funcs){}
52  int numVars() const { return N; }
54  int numEvents() const { return M; }
56  virtual void init(double* q) = 0;
58  virtual void der_func(const double* q, double* dq) = 0;
60  virtual void state_event_func(const double* q, double* z) = 0;
62  virtual double time_event_func(const double* q) = 0;
68  virtual void postStep(double* q){};
70  virtual void internal_event(double* q,
71  const bool* state_event) = 0;
73  virtual void external_event(double* q, double e,
74  const Bag<X>& xb) = 0;
76  virtual void confluent_event(double *q, const bool* state_event,
77  const Bag<X>& xb) = 0;
79  virtual void output_func(const double *q, const bool* state_event,
80  Bag<X>& yb) = 0;
82  virtual void gc_output(Bag<X>& gb) = 0;
84  virtual ~ode_system(){}
85  private:
86  const int N, M;
87 };
88 
102 template <typename X> class dae_se1_system:
103  public ode_system<X>
104 {
105  public:
113  dae_se1_system(int N_vars, int M_event_funcs, int A_alg_vars,
114  double err_tol = 1E-10, int max_iters = 30, double alpha = -1.0):
115  ode_system<X>(N_vars,M_event_funcs),
116  A(A_alg_vars),
117  max_iters(max_iters),
118  err_tol(err_tol),
119  alpha(alpha)
120  {
121  failed = 0;
122  max_err = 0.0;
123  a = new double[A];
124  atmp = new double[A];
125  d = new double[A];
126  f[1] = new double[A];
127  f[0] = new double[A];
128  }
130  int numAlgVars() const { return A; }
132  double getAlgVar(int i) const { return a[i]; }
137  virtual void init(double* q, double* a) = 0;
144  virtual void alg_func(const double* q, const double* a, double* af) = 0;
149  virtual void der_func(const double* q, const double* a, double* dq) = 0;
153  virtual void state_event_func(const double* q, const double* a, double* z) = 0;
155  virtual double time_event_func(const double* q, const double* a) = 0;
161  virtual void postStep(double* q, double* a) = 0;
163  virtual void internal_event(double* q, double* a,
164  const bool* state_event) = 0;
166  virtual void external_event(double* q, double* a,
167  double e, const Bag<X>& xb) = 0;
169  virtual void confluent_event(double *q, double* a,
170  const bool* state_event, const Bag<X>& xb) = 0;
172  virtual void output_func(const double *q, const double* a,
173  const bool* state_event, Bag<X>& yb) = 0;
175  virtual ~dae_se1_system()
176  {
177  delete [] d;
178  delete [] a;
179  delete [] atmp;
180  delete [] f[1];
181  delete [] f[0];
182  }
187  int getIterFailCount() const { return failed; }
193  double getWorseError() const { return max_err; }
195  void init(double* q)
196  {
197  init(q,a);
198  }
200  void der_func(const double* q, double* dq)
201  {
202  solve(q);
203  der_func(q,a,dq);
204  }
206  void state_event_func(const double* q, double* z)
207  {
208  solve(q);
209  state_event_func(q,a,z);
210  }
212  double time_event_func(const double* q)
213  {
214  solve(q);
215  return time_event_func(q,a);
216  }
218  void postStep(double* q)
219  {
220  solve(q);
221  postStep(q,a);
222  }
224  void internal_event(double* q, const bool* state_event)
225  {
226  // The variable a was solved for in the post step
227  internal_event(q,a,state_event);
228  // Make sure the algebraic variables are consistent with q
229  solve(q);
230  postStep(q,a);
231  }
233  void external_event(double* q, double e, const Bag<X>& xb)
234  {
235  // The variable a was solved for in the post step
236  external_event(q,a,e,xb);
237  // Make sure the algebraic variables are consistent with q
238  solve(q);
239  postStep(q,a);
240  }
242  void confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
243  {
244  // The variable a was solved for in the post step
245  confluent_event(q,a,state_event,xb);
246  // Make sure the algebraic variables are consistent with q
247  solve(q);
248  postStep(q,a);
249  }
251  void output_func(const double *q, const bool* state_event, Bag<X>& yb)
252  {
253  // The variable a was solved for in the post step
254  output_func(q,a,state_event,yb);
255  }
256  protected:
264  void solve(const double* q);
265  private:
266  const int A, max_iters;
267  const double err_tol, alpha;
268  // Guess at the algebraic solution
269  double *a, *atmp;
270  // Guesses at g(y)-y
271  double* f[2];
272  // Direction
273  double* d;
274  // Maximum error in the wake of a failure
275  double max_err;
276  // Number of failures
277  int failed;
278 };
279 
280 template <typename X>
281 void dae_se1_system<X>::solve(const double* q)
282 {
283  int iter_count = 0, alt, good;
284  double prev_err, err = 0.0, ee, beta, g2, alpha_tmp = alpha;
290  _adevs_dae_se_1_system_solve_try_it_again:
291  alt = 0;
292  good = 1;
293  prev_err = DBL_MAX;
294  // First step by steepest descent
295  alg_func(q,a,f[alt]);
296  for (int i = 0; i < A; i++)
297  {
298  // Calculate f(x,y)
299  f[alt][i] -= a[i];
300  // First direction
301  d[i] = -f[alt][i];
302  // Make the move
303  atmp[i] = a[i];
304  a[i] += alpha_tmp*d[i];
305  }
306  // Otherwise, first guess by steepest descent
307  // Finish search by conjugate gradiant
308  while (iter_count < max_iters)
309  {
310  iter_count++;
311  err = 0.0;
312  // Calculate y = g(x,y)
313  alg_func(q,a,f[good]);
314  // Check the quality of the solution
315  for (int i = 0; i < A; i++)
316  {
317  // Calculate f(x,y)
318  f[good][i] -= a[i];
319  // Get the largest error
320  ee = fabs(f[good][i]);
321  if (ee > err) err = ee;
322  }
323  // If the solution is good enough then return
324  if (err < err_tol) return;
325  // If the solution is not converging...
326  if (err > prev_err)
327  {
328  // Restore previous solution
329  for (int i = 0; i < A; i++)
330  a[i] = atmp[i];
331  // Restart with a new value for alpha
332  if (alpha_tmp < 0.0) alpha_tmp = -alpha_tmp;
333  else alpha_tmp *= -0.5;
334  goto _adevs_dae_se_1_system_solve_try_it_again;
335  }
336  prev_err = err;
337  // Calculate beta. See Strang's "Intro. to Applied Mathematics",
338  // pg. 379.
339  beta = g2 = 0.0;
340  for (int i = 0; i < A; i++)
341  g2 += f[alt][i]*f[alt][i];
342  for (int i = 0; i < A; i++)
343  beta += f[good][i]*(f[good][i]-f[alt][i]);
344  beta /= g2;
345  // Calculate a new guess at the solution
346  for (int i = 0; i < A; i++)
347  {
348  d[i] = beta*d[i]-f[good][i];
349  atmp[i] = a[i];
350  a[i] += alpha_tmp*d[i];
351  }
352  // Swap buffers
353  good = alt;
354  alt = (good+1)%2;
355  }
356  // Increment the failed count and worse case error if an
357  // acceptible solution was not found.
358  failed++;
359  if (err > max_err)
360  max_err = err;
361 }
362 
367 template <typename X> class ode_solver
368 {
369  public:
374  ode_solver(ode_system<X>* sys):sys(sys){}
380  virtual double integrate(double* q, double h_lim) = 0;
384  virtual void advance(double* q, double h) = 0;
386  virtual ~ode_solver(){}
387  protected:
388  ode_system<X>* sys;
389 };
390 
396 template <typename X> class event_locator
397 {
398  public:
403  event_locator(ode_system<X>* sys):sys(sys){}
414  virtual bool find_events(bool* events, const double *qstart,
415  double* qend, ode_solver<X>* solver, double& h) = 0;
417  virtual ~event_locator(){}
418  protected:
419  ode_system<X>* sys;
420 };
421 
430 template <typename X, class T = double> class Hybrid:
431  public Atomic<X,T>
432 {
433  public:
439  event_locator<X>* event_finder):
440  sys(sys),solver(solver),event_finder(event_finder),
441  e_accum(0.0)
442  {
443  q = new double[sys->numVars()];
444  q_trial = new double[sys->numVars()];
445  event = new bool[sys->numEvents()+1];
446  event_exists = false;
447  sys->init(q_trial); // Get the initial state of the model
448  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
449  tentative_step(); // Take the first tentative step
450  }
452  double getState(int k) const { return q[k]; }
454  const double* getState() const { return q; }
456  ode_system<X>* getSystem() { return sys; }
458  bool eventHappened() const { return event_happened; }
463  void delta_int()
464  {
465  if (!missedOutput.empty())
466  {
467  missedOutput.clear();
468  return;
469  }
470  e_accum += ta();
471  // Execute any discrete events
472  event_happened = event_exists;
473  if (event_exists) // Execute the internal event
474  {
475  sys->internal_event(q_trial,event);
476  e_accum = 0.0;
477  }
478  // Copy the new state vector to q
479  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
480  tentative_step(); // Take a tentative step
481  }
486  void delta_ext(T e, const Bag<X>& xb)
487  {
488  bool state_event_exists = false;
489  event_happened = true;
490  // Check that we have not missed a state event
491  if (event_exists)
492  {
493  for (int i = 0; i < sys->numVars(); i++)
494  q_trial[i] = q[i];
495  solver->advance(q_trial,e);
496  state_event_exists =
497  event_finder->find_events(event,q,q_trial,solver,e);
498  // We missed an event
499  if (state_event_exists)
500  {
501  output_func(missedOutput);
502  sys->confluent_event(q_trial,event,xb);
503  for (int i = 0; i < sys->numVars(); i++)
504  q[i] = q_trial[i];
505  }
506  }
507  if (!state_event_exists)// We didn't miss an event
508  {
509  solver->advance(q,e); // Advance the state q by e
510  // Let the model adjust algebraic variables, etc. for the new state
511  sys->postStep(q);
512  // Process the discrete input
513  sys->external_event(q,e+e_accum,xb);
514  }
515  e_accum = 0.0;
516  // Copy the new state to the trial solution
517  for (int i = 0; i < sys->numVars(); i++) q_trial[i] = q[i];
518  tentative_step(); // Take a tentative step
519  }
524  void delta_conf(const Bag<X>& xb)
525  {
526  if (!missedOutput.empty())
527  {
528  missedOutput.clear();
529  if (sigma > 0.0) event_exists = false;
530  }
531  // Execute any discrete events
532  event_happened = true;
533  if (event_exists)
534  sys->confluent_event(q_trial,event,xb);
535  else sys->external_event(q_trial,e_accum+ta(),xb);
536  e_accum = 0.0;
537  // Copy the new state vector to q
538  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
539  tentative_step(); // Take a tentative step
540  }
542  T ta()
543  {
544  if (missedOutput.empty()) return sigma;
545  else return 0.0;
546  }
548  void output_func(Bag<X>& yb)
549  {
550  if (!missedOutput.empty())
551  {
552  typename Bag<X>::iterator iter = missedOutput.begin();
553  for (; iter != missedOutput.end(); iter++)
554  yb.insert(*iter);
555  if (sigma == 0.0) // Confluent event
556  sys->output_func(q_trial,event,yb);
557  }
558  else
559  {
560  // Let the model adjust algebraic variables, etc. for the new state
561  sys->postStep(q_trial);
562  if (event_exists)
563  sys->output_func(q_trial,event,yb);
564  }
565  }
567  void gc_output(Bag<X>& gb) { sys->gc_output(gb); }
569  virtual ~Hybrid()
570  {
571  delete [] q; delete [] q_trial; delete [] event;
572  delete event_finder; delete solver; delete sys;
573  }
574  private:
575  ode_system<X>* sys; // The ODE system
576  ode_solver<X>* solver; // Integrator for the ode set
577  event_locator<X>* event_finder; // Event locator
578  double sigma; // Time to the next internal event
579  double *q, *q_trial; // Current and tentative states
580  bool* event; // Flags indicating the encountered event surfaces
581  bool event_exists; // True if there is at least one event
582  bool event_happened; // True if a discrete event in the ode_system took place
583  double e_accum; // Accumlated time between discrete events
584  Bag<X> missedOutput; // Output missed at an external event
585  // Execute a tentative step and calculate the time advance function
586  void tentative_step()
587  {
588  // Check for a time event
589  double time_event = sys->time_event_func(q);
590  // Integrate up to that time at most
591  double step_size = solver->integrate(q_trial,time_event);
592  // Look for state events inside of the interval [0,step_size]
593  bool state_event_exists =
594  event_finder->find_events(event,q,q_trial,solver,step_size);
595  // Find the time advance and set the time event flag
596  sigma = std::min(step_size,time_event);
597  event[sys->numEvents()] = time_event <= sigma;
598  event_exists = event[sys->numEvents()] || state_event_exists;
599  }
600 };
601 
602 } // end of namespace
603 
604 #endif
void gc_output(Bag< X > &gb)
Do not override. Invokes the ode_system gc_output method as needed.
Definition: adevs_hybrid.h:567
virtual void der_func(const double *q, const double *a, double *dq)=0
int numEvents() const
Get the number of state events.
Definition: adevs_hybrid.h:54
bool empty() const
Same as size()==0.
Definition: adevs_bag.h:126
virtual ~Hybrid()
Destructor deletes everything.
Definition: adevs_hybrid.h:569
void output_func(Bag< X > &yb)
Do not override. Invokes the ode_system output function as needed.
Definition: adevs_hybrid.h:548
virtual void internal_event(double *q, double *a, const bool *state_event)=0
The internal transition function.
Hybrid(ode_system< X > *sys, ode_solver< X > *solver, event_locator< X > *event_finder)
Definition: adevs_hybrid.h:438
void der_func(const double *q, double *dq)
Do not override.
Definition: adevs_hybrid.h:200
Definition: adevs_hybrid.h:430
virtual void init(double *q, double *a)=0
virtual ~ode_solver()
Destructor.
Definition: adevs_hybrid.h:386
void delta_conf(const Bag< X > &xb)
Definition: adevs_hybrid.h:524
virtual void gc_output(Bag< X > &gb)=0
Garbage collection function. This works just like the Atomic gc_output method.
double getWorseError() const
Definition: adevs_hybrid.h:193
virtual ~dae_se1_system()
Destructor.
Definition: adevs_hybrid.h:175
virtual ~ode_system()
Destructor.
Definition: adevs_hybrid.h:84
Definition: adevs_hybrid.h:396
void clear()
Remove all of the elements from the bag.
Definition: adevs_bag.h:144
ode_system< X > * getSystem()
Get the system that this solver is operating on.
Definition: adevs_hybrid.h:456
virtual void der_func(const double *q, double *dq)=0
Compute the derivative for state q and put it in dq.
void init(double *q)
Do not override.
Definition: adevs_hybrid.h:195
virtual double integrate(double *q, double h_lim)=0
iterator begin() const
Get an iterator pointing to the first element in the bag.
Definition: adevs_bag.h:128
virtual void external_event(double *q, double e, const Bag< X > &xb)=0
The external transition function.
void confluent_event(double *q, const bool *state_event, const Bag< X > &xb)
Do not override.
Definition: adevs_hybrid.h:242
void internal_event(double *q, const bool *state_event)
Do not override.
Definition: adevs_hybrid.h:224
void solve(const double *q)
Definition: adevs_hybrid.h:281
bool eventHappened() const
Did a discrete event occur at the last state transition?
Definition: adevs_hybrid.h:458
iterator end() const
Get an iterator to the end of the bag (i.e., just after the last element)
Definition: adevs_bag.h:130
dae_se1_system(int N_vars, int M_event_funcs, int A_alg_vars, double err_tol=1E-10, int max_iters=30, double alpha=-1.0)
Definition: adevs_hybrid.h:113
double getAlgVar(int i) const
Get the algebraic variables.
Definition: adevs_hybrid.h:132
int numVars() const
Get the number of state variables.
Definition: adevs_hybrid.h:52
virtual double time_event_func(const double *q, const double *a)=0
Compute the time event function using state q and algebraic variables a.
void delta_ext(T e, const Bag< X > &xb)
Definition: adevs_hybrid.h:486
virtual void alg_func(const double *q, const double *a, double *af)=0
ode_solver(ode_system< X > *sys)
Definition: adevs_hybrid.h:374
virtual void external_event(double *q, double *a, double e, const Bag< X > &xb)=0
The external transition function.
virtual void postStep(double *q)
Definition: adevs_hybrid.h:68
double time_event_func(const double *q)
Override only if you have no time events.
Definition: adevs_hybrid.h:212
const double * getState() const
Get the array of state variables.
Definition: adevs_hybrid.h:454
virtual void output_func(const double *q, const double *a, const bool *state_event, Bag< X > &yb)=0
The output function.
virtual void internal_event(double *q, const bool *state_event)=0
The internal transition function.
void state_event_func(const double *q, double *z)
Override only if you have no state event functions.
Definition: adevs_hybrid.h:206
void delta_int()
Definition: adevs_hybrid.h:463
void postStep(double *q)
Do not override.
Definition: adevs_hybrid.h:218
virtual void advance(double *q, double h)=0
virtual ~event_locator()
Destructor.
Definition: adevs_hybrid.h:417
ode_system(int N_vars, int M_event_funcs)
Make a system with N state variables and M state event functions.
Definition: adevs_hybrid.h:49
virtual void state_event_func(const double *q, const double *a, double *z)=0
virtual void confluent_event(double *q, double *a, const bool *state_event, const Bag< X > &xb)=0
The confluent transition function.
void external_event(double *q, double e, const Bag< X > &xb)
Do not override.
Definition: adevs_hybrid.h:233
virtual void state_event_func(const double *q, double *z)=0
Compute the state event functions for state q and put them in z.
Definition: adevs_hybrid.h:102
double getState(int k) const
Get the value of the kth continuous state variable.
Definition: adevs_hybrid.h:452
virtual double time_event_func(const double *q)=0
Compute the time event function using state q.
int getIterFailCount() const
Definition: adevs_hybrid.h:187
event_locator(ode_system< X > *sys)
Definition: adevs_hybrid.h:403
void insert(const T &t)
Put t into the bag.
Definition: adevs_bag.h:153
int numAlgVars() const
Get the number of algebraic variables.
Definition: adevs_hybrid.h:130
Definition: adevs_models.h:47
virtual void postStep(double *q, double *a)=0
T ta()
Do not override.
Definition: adevs_hybrid.h:542
virtual bool find_events(bool *events, const double *qstart, double *qend, ode_solver< X > *solver, double &h)=0
virtual void init(double *q)=0
Copy the initial state of the model to q.
Definition: adevs_hybrid.h:45
virtual void confluent_event(double *q, const bool *state_event, const Bag< X > &xb)=0
The confluent transition function.
Definition: adevs_hybrid.h:367
void output_func(const double *q, const bool *state_event, Bag< X > &yb)
Do not override.
Definition: adevs_hybrid.h:251
virtual void output_func(const double *q, const bool *state_event, Bag< X > &yb)=0
The output function.