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_rk4_h_ 00021 #define _adevs_rk4_h_ 00022 #include "adevs_dess.h" 00023 00024 namespace adevs 00025 { 00026 00034 template <class X> class rk4: public DESS<X> 00035 { 00036 public: 00043 rk4(int num_state_vars, double h_max, int zero_crossing_funcs = 1); 00047 void init(int i, double q0) { q[i] = q0; } 00051 const double* getStateVars() const { return q; } 00055 int getNumStateVars() const { return num_state_vars; } 00060 virtual void der_func(const double* q, double* dq) = 0; 00067 virtual void state_event_func(const double* q, double* z) = 0; 00072 virtual double time_event_func(const double* q) = 0; 00080 virtual void discrete_action(double* q, const Bag<X>& xb) = 0; 00085 virtual void discrete_output(const double* q, Bag<X>& yb) = 0; 00091 virtual void state_changed(const double* q){}; 00092 // Implementation of the DESS evolve_func method 00093 void evolve_func(double h); 00094 // Implementation of the DESS next_event_func method 00095 double next_event_func(bool& is_event); 00096 // Implementation of the DESS discrete_action_func method 00097 void discrete_action_func(const Bag<X>& xb); 00098 // Implementation of the DESS dscrete_output_func method 00099 void discrete_output_func(Bag<X>& yb); 00100 // Implementation of the DESS state_changed method 00101 void state_changed() { state_changed(q); } 00103 ~rk4(); 00104 00105 private: 00106 const double h_max; 00107 double *q, *dq, *t, *k[4], *q_tmp, *es, *en; 00108 int num_state_vars, zero_funcs; 00109 00110 void ode_step(double *qq, double step); 00111 00112 static int sgn(double x) 00113 { 00114 if (x < 0.0) return -1; 00115 else if (x > 0.0) return 1; 00116 else return 0; 00117 } 00118 }; 00119 00120 template <class X> 00121 rk4<X>::rk4(int num_state_vars, double h_max, int zero_funcs): 00122 DESS<X>(), 00123 h_max(h_max), 00124 num_state_vars(num_state_vars), 00125 zero_funcs(zero_funcs) 00126 { 00127 q = new double[num_state_vars]; 00128 dq = new double[num_state_vars]; 00129 t = new double[num_state_vars]; 00130 q_tmp = new double[num_state_vars]; 00131 for (int i = 0; i < 4; i++) 00132 k[i] = new double[num_state_vars]; 00133 en = new double[zero_funcs]; 00134 es = new double[zero_funcs]; 00135 } 00136 00137 template <class X> 00138 rk4<X>::~rk4() 00139 { 00140 delete [] q; 00141 delete [] dq; 00142 delete [] t; 00143 delete [] q_tmp; 00144 for (int i = 0; i < 4; i++) 00145 delete [] k[i]; 00146 delete [] es; 00147 delete [] en; 00148 } 00149 00150 template <class X> 00151 void rk4<X>::evolve_func(double h) 00152 { 00153 ode_step(q,h); 00154 } 00155 00156 template <class X> 00157 double rk4<X>::next_event_func(bool& is_event) 00158 { 00159 // Look for event crossings 00160 double time_event = time_event_func(q); 00161 double sigma = h_max; 00162 state_event_func(q,es); 00163 for (int i = 0; i < zero_funcs; i++) en[i] = es[i]; 00164 while (true) 00165 { 00166 // Look for an event in the next time step 00167 for (int i = 0; i < num_state_vars; i++) 00168 { 00169 q_tmp[i] = q[i]; 00170 } 00171 ode_step(q_tmp,sigma); 00172 state_event_func(q_tmp,en); 00173 // If no event, then return 00174 is_event = false; 00175 for (int i = 0; i < zero_funcs && !is_event; i++) 00176 { 00177 is_event = (sgn(es[i]) != sgn(en[i])); 00178 } 00179 if (!is_event) break; 00180 // Otherwise, halve the interval and try again 00181 sigma /= 2.0; 00182 // If t_event is small, then just return, its been found 00183 if (sigma < 1E-12) 00184 { 00185 is_event = true; 00186 break; 00187 } 00188 } 00189 if (sigma < time_event) 00190 { 00191 return sigma; 00192 } 00193 is_event = true; 00194 return time_event; 00195 } 00196 00197 template <class X> 00198 void rk4<X>::discrete_action_func(const Bag<X>& xb) 00199 { 00200 discrete_action(q,xb); 00201 } 00202 00203 template <class X> 00204 void rk4<X>::discrete_output_func(Bag<X>& yb) 00205 { 00206 discrete_output(q,yb); 00207 } 00208 00209 template <class X> 00210 void rk4<X>::ode_step(double*qq, double step) 00211 { 00212 if (step == 0.0) 00213 { 00214 return; 00215 } 00216 // Compute k1 00217 der_func(qq,dq); 00218 for (int j = 0; j < num_state_vars; j++) 00219 k[0][j] = dq[j]; 00220 // Compute k2 00221 for (int j = 0; j < num_state_vars; j++) 00222 t[j] = qq[j] + 0.5*step*k[0][j]; 00223 der_func(t,dq); 00224 for (int j = 0; j < num_state_vars; j++) 00225 k[1][j] = dq[j]; 00226 // Compute k3 00227 for (int j = 0; j < num_state_vars; j++) 00228 t[j] = qq[j] + 0.5*step*k[1][j]; 00229 der_func(t,dq); 00230 for (int j = 0; j < num_state_vars; j++) 00231 k[2][j] = dq[j]; 00232 // Compute k4 00233 for (int j = 0; j < num_state_vars; j++) 00234 t[j] = qq[j] + step*k[2][j]; 00235 der_func(t,dq); 00236 for (int j = 0; j < num_state_vars; j++) 00237 k[3][j] = dq[j]; 00238 // Compute next state 00239 for (int j = 0; j < num_state_vars; j++) 00240 qq[j] += step*(k[0][j] + 2.0*k[1][j] + 2.0*k[2][j] + k[3][j])/6.0; 00241 } 00242 00243 } // end of namespace 00244 00245 #endif