adevs
|
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