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){};
74  virtual void postTrialStep(double* q){};
76  virtual void internal_event(double* q,
77  const bool* state_event) = 0;
79  virtual void external_event(double* q, double e,
80  const Bag<X>& xb) = 0;
82  virtual void confluent_event(double *q, const bool* state_event,
83  const Bag<X>& xb) = 0;
85  virtual void output_func(const double *q, const bool* state_event,
86  Bag<X>& yb) = 0;
88  virtual void gc_output(Bag<X>& gb) = 0;
90  virtual ~ode_system(){}
91  private:
92  const int N, M;
93 };
94 
108 template <typename X> class dae_se1_system:
109  public ode_system<X>
110 {
111  public:
119  dae_se1_system(int N_vars, int M_event_funcs, int A_alg_vars,
120  double err_tol = 1E-10, int max_iters = 30, double alpha = -1.0):
121  ode_system<X>(N_vars,M_event_funcs),
122  A(A_alg_vars),
123  max_iters(max_iters),
124  err_tol(err_tol),
125  alpha(alpha)
126  {
127  failed = 0;
128  max_err = 0.0;
129  a = new double[A];
130  atmp = new double[A];
131  d = new double[A];
132  f[1] = new double[A];
133  f[0] = new double[A];
134  }
136  int numAlgVars() const { return A; }
138  double getAlgVar(int i) const { return a[i]; }
143  virtual void init(double* q, double* a) = 0;
150  virtual void alg_func(const double* q, const double* a, double* af) = 0;
155  virtual void der_func(const double* q, const double* a, double* dq) = 0;
159  virtual void state_event_func(const double* q, const double* a, double* z) = 0;
161  virtual double time_event_func(const double* q, const double* a) = 0;
167  virtual void postStep(double* q, double* a) = 0;
171  virtual void postTrialStep(double *q, double* a) { ode_system<X>::postTrialStep(q); }
173  virtual void internal_event(double* q, double* a,
174  const bool* state_event) = 0;
176  virtual void external_event(double* q, double* a,
177  double e, const Bag<X>& xb) = 0;
179  virtual void confluent_event(double *q, double* a,
180  const bool* state_event, const Bag<X>& xb) = 0;
182  virtual void output_func(const double *q, const double* a,
183  const bool* state_event, Bag<X>& yb) = 0;
185  virtual ~dae_se1_system()
186  {
187  delete [] d;
188  delete [] a;
189  delete [] atmp;
190  delete [] f[1];
191  delete [] f[0];
192  }
197  int getIterFailCount() const { return failed; }
203  double getWorseError() const { return max_err; }
205  void init(double* q)
206  {
207  init(q,a);
208  }
210  void der_func(const double* q, double* dq)
211  {
212  solve(q);
213  der_func(q,a,dq);
214  }
216  void state_event_func(const double* q, double* z)
217  {
218  solve(q);
219  state_event_func(q,a,z);
220  }
222  double time_event_func(const double* q)
223  {
224  solve(q);
225  return time_event_func(q,a);
226  }
228  void postStep(double* q)
229  {
230  solve(q);
231  postStep(q,a);
232  }
234  void internal_event(double* q, const bool* state_event)
235  {
236  // The variable a was solved for in the post step
237  internal_event(q,a,state_event);
238  // Make sure the algebraic variables are consistent with q
239  solve(q);
240  postStep(q,a);
241  }
243  void external_event(double* q, double e, const Bag<X>& xb)
244  {
245  // The variable a was solved for in the post step
246  external_event(q,a,e,xb);
247  // Make sure the algebraic variables are consistent with q
248  solve(q);
249  postStep(q,a);
250  }
252  void confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
253  {
254  // The variable a was solved for in the post step
255  confluent_event(q,a,state_event,xb);
256  // Make sure the algebraic variables are consistent with q
257  solve(q);
258  postStep(q,a);
259  }
261  void output_func(const double *q, const bool* state_event, Bag<X>& yb)
262  {
263  // The variable a was solved for in the post step
264  output_func(q,a,state_event,yb);
265  }
266  protected:
274  void solve(const double* q);
275  private:
276  const int A, max_iters;
277  const double err_tol, alpha;
278  // Guess at the algebraic solution
279  double *a, *atmp;
280  // Guesses at g(y)-y
281  double* f[2];
282  // Direction
283  double* d;
284  // Maximum error in the wake of a failure
285  double max_err;
286  // Number of failures
287  int failed;
288 };
289 
290 template <typename X>
291 void dae_se1_system<X>::solve(const double* q)
292 {
293  int iter_count = 0, alt, good;
294  double prev_err, err = 0.0, ee, beta, g2, alpha_tmp = alpha;
300  _adevs_dae_se_1_system_solve_try_it_again:
301  alt = 0;
302  good = 1;
303  prev_err = DBL_MAX;
304  // First step by steepest descent
305  alg_func(q,a,f[alt]);
306  for (int i = 0; i < A; i++)
307  {
308  // Calculate f(x,y)
309  f[alt][i] -= a[i];
310  // First direction
311  d[i] = -f[alt][i];
312  // Make the move
313  atmp[i] = a[i];
314  a[i] += alpha_tmp*d[i];
315  }
316  // Otherwise, first guess by steepest descent
317  // Finish search by conjugate gradiant
318  while (iter_count < max_iters)
319  {
320  iter_count++;
321  err = 0.0;
322  // Calculate y = g(x,y)
323  alg_func(q,a,f[good]);
324  // Check the quality of the solution
325  for (int i = 0; i < A; i++)
326  {
327  // Calculate f(x,y)
328  f[good][i] -= a[i];
329  // Get the largest error
330  ee = fabs(f[good][i]);
331  if (ee > err) err = ee;
332  }
333  // If the solution is good enough then return
334  if (err < err_tol) return;
335  // If the solution is not converging...
336  if (err > prev_err)
337  {
338  // Restore previous solution
339  for (int i = 0; i < A; i++)
340  a[i] = atmp[i];
341  // Restart with a new value for alpha
342  if (alpha_tmp < 0.0) alpha_tmp = -alpha_tmp;
343  else alpha_tmp *= -0.5;
344  goto _adevs_dae_se_1_system_solve_try_it_again;
345  }
346  prev_err = err;
347  // Calculate beta. See Strang's "Intro. to Applied Mathematics",
348  // pg. 379.
349  beta = g2 = 0.0;
350  for (int i = 0; i < A; i++)
351  g2 += f[alt][i]*f[alt][i];
352  for (int i = 0; i < A; i++)
353  beta += f[good][i]*(f[good][i]-f[alt][i]);
354  beta /= g2;
355  // Calculate a new guess at the solution
356  for (int i = 0; i < A; i++)
357  {
358  d[i] = beta*d[i]-f[good][i];
359  atmp[i] = a[i];
360  a[i] += alpha_tmp*d[i];
361  }
362  // Swap buffers
363  good = alt;
364  alt = (good+1)%2;
365  }
366  // Increment the failed count and worse case error if an
367  // acceptible solution was not found.
368  failed++;
369  if (err > max_err)
370  max_err = err;
371 }
372 
377 template <typename X> class ode_solver
378 {
379  public:
384  ode_solver(ode_system<X>* sys):sys(sys){}
390  virtual double integrate(double* q, double h_lim) = 0;
394  virtual void advance(double* q, double h) = 0;
396  virtual ~ode_solver(){}
397  protected:
398  ode_system<X>* sys;
399 };
400 
406 template <typename X> class event_locator
407 {
408  public:
413  event_locator(ode_system<X>* sys):sys(sys){}
424  virtual bool find_events(bool* events, const double *qstart,
425  double* qend, ode_solver<X>* solver, double& h) = 0;
427  virtual ~event_locator(){}
428  protected:
429  ode_system<X>* sys;
430 };
431 
440 template <typename X, class T = double> class Hybrid:
441  public Atomic<X,T>
442 {
443  public:
449  event_locator<X>* event_finder):
450  sys(sys),solver(solver),event_finder(event_finder),
451  e_accum(0.0)
452  {
453  q = new double[sys->numVars()];
454  q_trial = new double[sys->numVars()];
455  event = new bool[sys->numEvents()+1];
456  event_exists = false;
457  sys->init(q_trial); // Get the initial state of the model
458  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
459  tentative_step(); // Take the first tentative step
460  }
462  double getState(int k) const { return q[k]; }
464  const double* getState() const { return q; }
466  ode_system<X>* getSystem() { return sys; }
468  bool eventHappened() const { return event_happened; }
473  void delta_int()
474  {
475  if (!missedOutput.empty())
476  {
477  missedOutput.clear();
478  return;
479  }
480  e_accum += ta();
481  // Execute any discrete events
482  event_happened = event_exists;
483  if (event_exists) // Execute the internal event
484  {
485  sys->internal_event(q_trial,event);
486  e_accum = 0.0;
487  }
488  // Copy the new state vector to q
489  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
490  tentative_step(); // Take a tentative step
491  }
496  void delta_ext(T e, const Bag<X>& xb)
497  {
498  bool state_event_exists = false;
499  event_happened = true;
500  // Check that we have not missed a state event
501  if (event_exists)
502  {
503  for (int i = 0; i < sys->numVars(); i++)
504  q_trial[i] = q[i];
505  solver->advance(q_trial,e);
506  state_event_exists =
507  event_finder->find_events(event,q,q_trial,solver,e);
508  // We missed an event
509  if (state_event_exists)
510  {
511  output_func(missedOutput);
512  sys->confluent_event(q_trial,event,xb);
513  for (int i = 0; i < sys->numVars(); i++)
514  q[i] = q_trial[i];
515  }
516  }
517  if (!state_event_exists)// We didn't miss an event
518  {
519  solver->advance(q,e); // Advance the state q by e
520  // Let the model adjust algebraic variables, etc. for the new state
521  sys->postStep(q);
522  // Process the discrete input
523  sys->external_event(q,e+e_accum,xb);
524  }
525  e_accum = 0.0;
526  // Copy the new state to the trial solution
527  for (int i = 0; i < sys->numVars(); i++) q_trial[i] = q[i];
528  tentative_step(); // Take a tentative step
529  }
534  void delta_conf(const Bag<X>& xb)
535  {
536  if (!missedOutput.empty())
537  {
538  missedOutput.clear();
539  if (sigma > 0.0) event_exists = false;
540  }
541  // Execute any discrete events
542  event_happened = true;
543  if (event_exists)
544  sys->confluent_event(q_trial,event,xb);
545  else sys->external_event(q_trial,e_accum+ta(),xb);
546  e_accum = 0.0;
547  // Copy the new state vector to q
548  for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
549  tentative_step(); // Take a tentative step
550  }
552  T ta()
553  {
554  if (missedOutput.empty()) return sigma;
555  else return 0.0;
556  }
558  void output_func(Bag<X>& yb)
559  {
560  if (!missedOutput.empty())
561  {
562  typename Bag<X>::iterator iter = missedOutput.begin();
563  for (; iter != missedOutput.end(); iter++)
564  yb.insert(*iter);
565  if (sigma == 0.0) // Confluent event
566  sys->output_func(q_trial,event,yb);
567  }
568  else
569  {
570  // Let the model adjust algebraic variables, etc. for the new state
571  sys->postStep(q_trial);
572  if (event_exists)
573  sys->output_func(q_trial,event,yb);
574  }
575  }
577  void gc_output(Bag<X>& gb) { sys->gc_output(gb); }
579  virtual ~Hybrid()
580  {
581  delete [] q; delete [] q_trial; delete [] event;
582  delete event_finder; delete solver; delete sys;
583  }
584  private:
585  ode_system<X>* sys; // The ODE system
586  ode_solver<X>* solver; // Integrator for the ode set
587  event_locator<X>* event_finder; // Event locator
588  double sigma; // Time to the next internal event
589  double *q, *q_trial; // Current and tentative states
590  bool* event; // Flags indicating the encountered event surfaces
591  bool event_exists; // True if there is at least one event
592  bool event_happened; // True if a discrete event in the ode_system took place
593  double e_accum; // Accumlated time between discrete events
594  Bag<X> missedOutput; // Output missed at an external event
595  // Execute a tentative step and calculate the time advance function
596  void tentative_step()
597  {
598  // Check for a time event
599  double time_event = sys->time_event_func(q);
600  // Integrate up to that time at most
601  double step_size = solver->integrate(q_trial,time_event);
602  // Look for state events inside of the interval [0,step_size]
603  bool state_event_exists =
604  event_finder->find_events(event,q,q_trial,solver,step_size);
605  // Find the time advance and set the time event flag
606  sigma = std::min(step_size,time_event);
607  event[sys->numEvents()] = time_event <= sigma;
608  event_exists = event[sys->numEvents()] || state_event_exists;
609  sys->postTrialStep(q);
610  }
611 };
612 
613 } // end of namespace
614 
615 #endif
void gc_output(Bag< X > &gb)
Do not override. Invokes the ode_system gc_output method as needed.
Definition: adevs_hybrid.h:577
int numEvents() const
Get the number of state events.
Definition: adevs_hybrid.h:54
virtual ~Hybrid()
Destructor deletes everything.
Definition: adevs_hybrid.h:579
void output_func(Bag< X > &yb)
Do not override. Invokes the ode_system output function as needed.
Definition: adevs_hybrid.h:558
Hybrid(ode_system< X > *sys, ode_solver< X > *solver, event_locator< X > *event_finder)
Definition: adevs_hybrid.h:448
void der_func(const double *q, double *dq)
Do not override.
Definition: adevs_hybrid.h:210
Definition: adevs_hybrid.h:440
virtual ~ode_solver()
Destructor.
Definition: adevs_hybrid.h:396
void delta_conf(const Bag< X > &xb)
Definition: adevs_hybrid.h:534
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:203
virtual ~dae_se1_system()
Destructor.
Definition: adevs_hybrid.h:185
virtual ~ode_system()
Destructor.
Definition: adevs_hybrid.h:90
Definition: adevs_hybrid.h:406
ode_system< X > * getSystem()
Get the system that this solver is operating on.
Definition: adevs_hybrid.h:466
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:205
virtual double integrate(double *q, double h_lim)=0
virtual void postTrialStep(double *q, double *a)
Definition: adevs_hybrid.h:171
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:252
void internal_event(double *q, const bool *state_event)
Do not override.
Definition: adevs_hybrid.h:234
virtual void postTrialStep(double *q)
Definition: adevs_hybrid.h:74
void solve(const double *q)
Definition: adevs_hybrid.h:291
bool eventHappened() const
Did a discrete event occur at the last state transition?
Definition: adevs_hybrid.h:468
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:119
double getAlgVar(int i) const
Get the algebraic variables.
Definition: adevs_hybrid.h:138
int numVars() const
Get the number of state variables.
Definition: adevs_hybrid.h:52
void delta_ext(T e, const Bag< X > &xb)
Definition: adevs_hybrid.h:496
ode_solver(ode_system< X > *sys)
Definition: adevs_hybrid.h:384
Definition: adevs_fmi.h:56
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:222
const double * getState() const
Get the array of state variables.
Definition: adevs_hybrid.h:464
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:216
void delta_int()
Definition: adevs_hybrid.h:473
void postStep(double *q)
Do not override.
Definition: adevs_hybrid.h:228
virtual ~event_locator()
Destructor.
Definition: adevs_hybrid.h:427
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
void external_event(double *q, double e, const Bag< X > &xb)
Do not override.
Definition: adevs_hybrid.h:243
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:108
double getState(int k) const
Get the value of the kth continuous state variable.
Definition: adevs_hybrid.h:462
virtual double time_event_func(const double *q)=0
Compute the time event function using state q.
int getIterFailCount() const
Definition: adevs_hybrid.h:197
event_locator(ode_system< X > *sys)
Definition: adevs_hybrid.h:413
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:136
Definition: adevs_models.h:47
T ta()
Do not override.
Definition: adevs_hybrid.h:552
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:377
void output_func(const double *q, const bool *state_event, Bag< X > &yb)
Do not override.
Definition: adevs_hybrid.h:261
virtual void output_func(const double *q, const bool *state_event, Bag< X > &yb)=0
The output function.