00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00093 void evolve_func(double h);
00094
00095 double next_event_func(bool& is_event);
00096
00097 void discrete_action_func(const Bag<X>& xb);
00098
00099 void discrete_output_func(Bag<X>& yb);
00100
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
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
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
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
00181 sigma /= 2.0;
00182
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
00217 der_func(qq,dq);
00218 for (int j = 0; j < num_state_vars; j++)
00219 k[0][j] = dq[j];
00220
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
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
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
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 }
00244
00245 #endif