adevs
|
00001 /*************** 00002 Copyright (C) 2000-2006 by James Nutaro 00003 00004 This library is free software; you can redistribute it and/or 00005 modify it under the terms of the GNU Lesser General Public 00006 License as published by the Free Software Foundation; either 00007 version 2 of the License, or (at your option) any later version. 00008 00009 This library is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00012 Lesser General Public License for more details. 00013 00014 You should have received a copy of the GNU Lesser General Public 00015 License along with this library; if not, write to the Free Software 00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00017 00018 Bugs, comments, and questions can be sent to nutaro@gmail.com 00019 ***************/ 00020 #ifndef _adevs_rk45_h_ 00021 #define _adevs_rk45_h_ 00022 #include "adevs_dess.h" 00023 #include <cmath> 00024 #include <algorithm> 00025 00026 namespace adevs 00027 { 00028 00039 template <class X> class rk45: public DESS<X> 00040 { 00041 public: 00049 rk45(int num_state_vars, double h_max, double err_tol, 00050 int zero_crossing_funcs, double event_tol = 1E-12); 00054 void init(int i, double q0) { q[i] = q0; } 00058 const double* getStateVars() const { return q; } 00062 int getNumStateVars() const { return num_state_vars; } 00067 virtual void der_func(const double* q, double* dq) = 0; 00074 virtual void state_event_func(const double* q, double* z) = 0; 00079 virtual double time_event_func(const double* q) = 0; 00087 virtual void discrete_action(double* q, const Bag<X>& xb) = 0; 00092 virtual void discrete_output(const double* q, Bag<X>& yb) = 0; 00101 virtual void state_changed(const double* q){} 00102 // Implementation of the DESS evolve_func method 00103 void evolve_func(double h); 00104 // Implementation of the DESS next_event_func method 00105 double next_event_func(bool& is_event); 00106 // Implementation of the DESS discrete_action_func method 00107 void discrete_action_func(const Bag<X>& xb); 00108 // Implementation of the DESS dscrete_output_func method 00109 void discrete_output_func(Bag<X>& yb); 00110 // Implementation of the DESS state changed method 00111 void state_changed() { state_changed(q); } 00113 ~rk45(); 00114 00115 private: 00116 // Maximum step size, trunc. err. tolerance, and 00117 // state event detection time tolerance 00118 const double h_max, err_tol, event_tol; 00119 // Scratch variables to implement the integration 00120 // and event detection scheme 00121 double *q, *dq, *t, *k[6], *q_tmp, *es, *en; 00122 // Current time step selection 00123 double h_cur; 00124 // Number of state variables and level crossing functions 00125 int num_state_vars, zero_funcs; 00126 /* 00127 * This makes an appropriate sized step and stores the 00128 * result in qq. The method returns truncation error 00129 * estimate. 00130 */ 00131 double ode_step(double *qq, double step); 00132 00133 static int sgn(double x) 00134 { 00135 if (x < 0.0) return -1; 00136 else if (x > 0.0) return 1; 00137 else return 0; 00138 } 00139 }; 00140 00141 template <class X> 00142 rk45<X>::rk45(int num_state_vars, double h_max, double err_tol, int zero_funcs, double event_tol): 00143 DESS<X>(), 00144 h_max(h_max), 00145 err_tol(err_tol), 00146 event_tol(event_tol), 00147 num_state_vars(num_state_vars), 00148 zero_funcs(zero_funcs) 00149 { 00150 h_cur = h_max; 00151 q = new double[num_state_vars]; 00152 dq = new double[num_state_vars]; 00153 t = new double[num_state_vars]; 00154 q_tmp = new double[num_state_vars]; 00155 for (int i = 0; i < 6; i++) 00156 k[i] = new double[num_state_vars]; 00157 en = new double[zero_funcs]; 00158 es = new double[zero_funcs]; 00159 } 00160 00161 template <class X> 00162 rk45<X>::~rk45() 00163 { 00164 delete [] q; 00165 delete [] dq; 00166 delete [] t; 00167 delete [] q_tmp; 00168 for (int i = 0; i < 6; i++) 00169 delete [] k[i]; 00170 delete [] es; 00171 delete [] en; 00172 } 00173 00174 template <class X> 00175 void rk45<X>::evolve_func(double h) 00176 { 00177 ode_step(q,h); 00178 } 00179 00180 template <class X> 00181 double rk45<X>::next_event_func(bool& is_event) 00182 { 00183 // Get the next time event 00184 double time_event = time_event_func(q); 00185 // Look for the next integrator step size 00186 h_cur = std::min(h_max,h_cur*1.1); // Try something a little bigger 00187 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i]; 00188 while (ode_step(q_tmp,h_cur) > err_tol) // Shrink until the tolerance is met 00189 { 00190 h_cur *= 0.9; 00191 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i]; 00192 } 00193 // Look for the next state event 00194 double sigma = h_cur; 00195 state_event_func(q,es); 00196 for (int i = 0; i < zero_funcs; i++) en[i] = es[i]; 00197 while (true) 00198 { 00199 // Look for an event in the next time step 00200 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i]; 00201 // This will update the h_cur variable to satisfy 00202 // the error tolerance. Therefore, the step size 00203 // selection for error will also be found during 00204 // the search for state events 00205 ode_step(q_tmp,sigma); 00206 state_event_func(q_tmp,en); 00207 // If no event, then return 00208 is_event = false; 00209 for (int i = 0; i < zero_funcs && !is_event; i++) 00210 { 00211 is_event = (sgn(es[i]) != sgn(en[i])); 00212 } 00213 if (!is_event) break; 00214 // Otherwise, halve the interval and try again 00215 sigma /= 2.0; 00216 // If t_event is small, then just return, its been found 00217 if (sigma < event_tol) 00218 { 00219 is_event = true; 00220 break; 00221 } 00222 } 00223 // Return the time until the next event 00224 if (sigma < time_event) 00225 { 00226 return sigma; 00227 } 00228 is_event = true; 00229 return time_event; 00230 } 00231 00232 template <class X> 00233 void rk45<X>::discrete_action_func(const Bag<X>& xb) 00234 { 00235 discrete_action(q,xb); 00236 } 00237 00238 template <class X> 00239 void rk45<X>::discrete_output_func(Bag<X>& yb) 00240 { 00241 discrete_output(q,yb); 00242 } 00243 00244 template <class X> 00245 double rk45<X>::ode_step(double*qq, double step) 00246 { 00247 if (step == 0.0) 00248 { 00249 return 0.0; 00250 } 00251 // Compute k1 00252 der_func(qq,dq); 00253 for (int j = 0; j < num_state_vars; j++) 00254 k[0][j] = step*dq[j]; 00255 // Compute k2 00256 for (int j = 0; j < num_state_vars; j++) 00257 t[j] = qq[j] + 0.5*k[0][j]; 00258 der_func(t,dq); 00259 for (int j = 0; j < num_state_vars; j++) 00260 k[1][j] = step*dq[j]; 00261 // Compute k3 00262 for (int j = 0; j < num_state_vars; j++) 00263 t[j] = qq[j] + 0.25*(k[0][j]+k[1][j]); 00264 der_func(t,dq); 00265 for (int j = 0; j < num_state_vars; j++) 00266 k[2][j] = step*dq[j]; 00267 // Compute k4 00268 for (int j = 0; j < num_state_vars; j++) 00269 t[j] = qq[j] - k[1][j] + 2.0*k[2][j]; 00270 der_func(t,dq); 00271 for (int j = 0; j < num_state_vars; j++) 00272 k[3][j] = step*dq[j]; 00273 // Compute k5 00274 for (int j = 0; j < num_state_vars; j++) 00275 t[j] = qq[j] + (7.0/27.0)*k[0][j] + (10.0/27.0)*k[1][j] + (1.0/27.0)*k[3][j]; 00276 der_func(t,dq); 00277 for (int j = 0; j < num_state_vars; j++) 00278 k[4][j] = step*dq[j]; 00279 // Compute k6 00280 for (int j = 0; j < num_state_vars; j++) 00281 t[j] = qq[j] + (28.0/625.0)*k[0][j] - 0.2*k[1][j] + (546.0/625.0)*k[2][j] 00282 + (54.0/625.0)*k[3][j] - (378.0/625.0)*k[4][j]; 00283 der_func(t,dq); 00284 for (int j = 0 ; j < num_state_vars; j++) 00285 k[5][j] = step*dq[j]; 00286 // Compute next state and maximum approx. error 00287 double err = 0.0; 00288 for (int j = 0; j < num_state_vars; j++) 00289 { 00290 qq[j] += (1.0/24.0)*k[0][j] + (5.0/48.0)*k[3][j] + 00291 (27.0/56.0)*k[4][j] + (125.0/336.0)*k[5][j]; 00292 err = std::max(err, 00293 fabs(k[0][j]/8.0+2.0*k[2][j]/3.0+k[3][j]/16.0-27.0*k[4][j]/56.0 00294 -125.0*k[5][j]/336.0)); 00295 } 00296 return err; 00297 } 00298 00299 } // end of namespace 00300 00301 #endif