adevs
/home/rotten/adevs-2.6/include/adevs_hybrid.h
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(const 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(const double* q, const 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(const double* q)
00189                 {
00190                         solve(q);
00191                         postStep(q,a);
00192                 }
00194                 void internal_event(double* q, const bool* state_event)
00195                 {
00196                         // The variable a was solved for in the post step
00197                         internal_event(q,a,state_event);
00198                         // Make sure the algebraic variables are consistent with q
00199                         solve(q);
00200                         postStep(q,a);
00201                 }
00203                 void external_event(double* q, double e, const Bag<X>& xb)
00204                 {
00205                         // The variable a was solved for in the post step
00206                         external_event(q,a,e,xb);
00207                         // Make sure the algebraic variables are consistent with q
00208                         solve(q);
00209                         postStep(q,a);
00210                 }
00212                 void confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
00213                 {
00214                         // The variable a was solved for in the post step
00215                         confluent_event(q,a,state_event,xb);
00216                         // Make sure the algebraic variables are consistent with q
00217                         solve(q);
00218                         postStep(q,a);
00219                 }
00221                 void output_func(const double *q, const bool* state_event, Bag<X>& yb)
00222                 {
00223                         // The variable a was solved for in the post step
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                 // Guess at the algebraic solution
00239                 double *a, *atmp;
00240                 // Guesses at g(y)-y
00241                 double* f[2];
00242                 // Direction
00243                 double* d;
00244                 // Maximum error in the wake of a failure
00245                 double max_err;
00246                 // Number of failures
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         // First step by steepest descent
00265         alg_func(q,a,f[alt]);
00266         for (int i = 0; i < A; i++)
00267         {
00268                 // Calculate f(x,y)
00269                 f[alt][i] -= a[i];
00270                 // First direction
00271                 d[i] = -f[alt][i];
00272                 // Make the move
00273                 atmp[i] = a[i];
00274                 a[i] += alpha_tmp*d[i];
00275         }
00276         // Otherwise, first guess by steepest descent
00277         // Finish search by conjugate gradiant
00278         while (iter_count < max_iters)
00279         {
00280                 iter_count++;
00281                 err = 0.0;
00282                 // Calculate y = g(x,y) 
00283                 alg_func(q,a,f[good]);
00284                 // Check the quality of the solution
00285                 for (int i = 0; i < A; i++)
00286                 {
00287                         // Calculate f(x,y) 
00288                         f[good][i] -= a[i];
00289                         // Get the largest error 
00290                         ee = fabs(f[good][i]);
00291                         if (ee > err) err = ee;
00292                 }
00293                 // If the solution is good enough then return
00294                 if (err < err_tol) return;
00295                 // If the solution is not converging...
00296                 if (err > prev_err)
00297                 {
00298                         // Restore previous solution
00299                         for (int i = 0; i < A; i++)
00300                                 a[i] = atmp[i];
00301                         // Restart with a new value for alpha
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                 // Calculate beta. See Strang's "Intro. to Applied Mathematics",
00308                 // pg. 379.
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                 // Calculate a new guess at the solution
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                 // Swap buffers
00323                 good = alt;
00324                 alt = (good+1)%2;
00325         }
00326         // Increment the failed count and worse case error if an 
00327         // acceptible solution was not found.
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){}
00382                 virtual bool find_events(bool* events, const double *qstart, 
00383                                 double* qend, ode_solver<X>* solver, double& h) = 0;
00385                 virtual ~event_locator(){}
00386         protected:
00387                 ode_system<X>* sys;
00388 };
00389 
00398 template <typename X, class T = double> class Hybrid:
00399         public Atomic<X,T>
00400 {
00401         public:
00406                 Hybrid(ode_system<X>* sys, ode_solver<X>* solver,
00407                                 event_locator<X>* event_finder):
00408                         sys(sys),solver(solver),event_finder(event_finder),
00409                         e_accum(0.0)
00410                 {
00411                         q = new double[sys->numVars()];
00412                         q_trial = new double[sys->numVars()];
00413                         event = new bool[sys->numEvents()+1];
00414                         event_exists = false;
00415                         sys->init(q_trial); // Get the initial state of the model
00416                         for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00417                         tentative_step(); // Take the first tentative step
00418                 }
00420                 double getState(int k) const { return q[k]; }
00422                 const double* getState() const { return q; }
00424                 ode_system<X>* getSystem() { return sys; }
00426                 bool eventHappened() const { return event_happened; }
00431                 void delta_int()
00432                 {
00433                         e_accum += ta();
00434                         // Execute any discrete events
00435                         event_happened = event_exists;
00436                         if (event_exists) // Execute the internal event
00437                         {
00438                                 sys->internal_event(q_trial,event); 
00439                                 e_accum = 0.0;
00440                         }
00441                         // Copy the new state vector to q
00442                         for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00443                         tentative_step(); // Take a tentative step
00444                 }
00449                 void delta_ext(T e, const Bag<X>& xb)
00450                 {
00451                         event_happened = true;
00452                         solver->advance(q,e); // Advance the state q by e
00453                         // Let the model adjust algebraic variables, etc. for the new state
00454                         sys->postStep(q);
00455                         // Process the discrete input
00456                         sys->external_event(q,e+e_accum,xb);
00457                         e_accum = 0.0;
00458                         // Copy the new state to the trial solution 
00459                         for (int i = 0; i < sys->numVars(); i++) q_trial[i] = q[i];
00460                         tentative_step(); // Take a tentative step
00461                 }
00466                 void delta_conf(const Bag<X>& xb)
00467                 {
00468                         // Execute any discrete events
00469                         event_happened = true;
00470                         if (event_exists) // Execute the confluent or external event
00471                                 sys->confluent_event(q_trial,event,xb); 
00472                         else sys->external_event(q_trial,e_accum+ta(),xb);
00473                         e_accum = 0.0;
00474                         // Copy the new state vector to q
00475                         for (int i = 0; i < sys->numVars(); i++) q[i] = q_trial[i];
00476                         tentative_step(); // Take a tentative step 
00477                 }
00479                 T ta() { return sigma; }
00481                 void output_func(Bag<X>& yb)
00482                 {
00483                         // Let the model adjust algebraic variables, etc. for the new state
00484                         sys->postStep(q_trial);
00485                         if (event_exists)
00486                                 sys->output_func(q_trial,event,yb);
00487                 }
00489                 void gc_output(Bag<X>& gb) { sys->gc_output(gb); }
00491                 virtual ~Hybrid()
00492                 {
00493                         delete [] q; delete [] q_trial; delete [] event;
00494                         delete event_finder; delete solver; delete sys;
00495                 }
00496         private:
00497                 ode_system<X>* sys; // The ODE system
00498                 ode_solver<X>* solver; // Integrator for the ode set
00499                 event_locator<X>* event_finder; // Event locator
00500                 double sigma; // Time to the next internal event
00501                 double *q, *q_trial; // Current and tentative states
00502                 bool* event; // Flags indicating the encountered event surfaces
00503                 bool event_exists; // True if there is at least one event
00504                 bool event_happened; // True if a discrete event in the ode_system took place
00505                 double e_accum; // Accumlated time between discrete events
00506                 // Execute a tentative step and calculate the time advance function
00507                 void tentative_step()
00508                 {
00509                         // Check for a time event
00510                         double time_event = sys->time_event_func(q);
00511                         // Integrate up to that time at most
00512                         double step_size = solver->integrate(q_trial,time_event);
00513                         // Look for state events inside of the interval [0,step_size]
00514                         bool state_event_exists =
00515                                 event_finder->find_events(event,q,q_trial,solver,step_size);
00516                         // Find the time advance and set the time event flag
00517                         sigma = std::min(step_size,time_event);
00518                         event[sys->numEvents()] = time_event <= sigma;
00519                         event_exists = event[sys->numEvents()] || state_event_exists;
00520                 }
00521 };
00522 
00523 } // end of namespace
00524 
00525 #endif