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
00033 template <class X> class rk4: public DESS<X>
00034 {
00035 public:
00042 rk4(int num_state_vars, double h_max, int zero_crossing_funcs = 1);
00046 void init(int i, double q0) { q[i] = q0; }
00050 const double* getStateVars() const { return q; }
00054 int getNumStateVars() const { return num_state_vars; }
00059 virtual void der_func(const double* q, double* dq) = 0;
00066 virtual void state_event_func(const double* q, double* z) = 0;
00071 virtual double time_event_func(const double* q) = 0;
00079 virtual void discrete_action(double* q, const Bag<X>& xb) = 0;
00084 virtual void discrete_output(const double* q, Bag<X>& yb) = 0;
00090 virtual void state_changed(const double* q){};
00091
00092 void evolve_func(double h);
00093
00094 double next_event_func(bool& is_event);
00095
00096 void discrete_action_func(const Bag<X>& xb);
00097
00098 void discrete_output_func(Bag<X>& yb);
00099
00100 void state_changed() { state_changed(q); }
00102 ~rk4();
00103
00104 private:
00105 const double h_max;
00106 double *q, *dq, *t, *k[4], *q_tmp, *es, *en;
00107 int num_state_vars, zero_funcs;
00108
00109 void ode_step(double *qq, double step);
00110
00111 static int sgn(double x)
00112 {
00113 if (x < 0.0) return -1;
00114 else if (x > 0.0) return 1;
00115 else return 0;
00116 }
00117 };
00118
00119 template <class X>
00120 rk4<X>::rk4(int num_state_vars, double h_max, int zero_funcs):
00121 DESS<X>(),
00122 h_max(h_max),
00123 num_state_vars(num_state_vars),
00124 zero_funcs(zero_funcs)
00125 {
00126 q = new double[num_state_vars];
00127 dq = new double[num_state_vars];
00128 t = new double[num_state_vars];
00129 q_tmp = new double[num_state_vars];
00130 for (int i = 0; i < 4; i++)
00131 k[i] = new double[num_state_vars];
00132 en = new double[zero_funcs];
00133 es = new double[zero_funcs];
00134 }
00135
00136 template <class X>
00137 rk4<X>::~rk4()
00138 {
00139 delete [] q;
00140 delete [] dq;
00141 delete [] t;
00142 delete [] q_tmp;
00143 for (int i = 0; i < 4; i++)
00144 delete [] k[i];
00145 delete [] es;
00146 delete [] en;
00147 }
00148
00149 template <class X>
00150 void rk4<X>::evolve_func(double h)
00151 {
00152 ode_step(q,h);
00153 }
00154
00155 template <class X>
00156 double rk4<X>::next_event_func(bool& is_event)
00157 {
00158
00159 double time_event = time_event_func(q);
00160 double sigma = h_max;
00161 state_event_func(q,es);
00162 for (int i = 0; i < zero_funcs; i++) en[i] = es[i];
00163 while (true)
00164 {
00165
00166 for (int i = 0; i < num_state_vars; i++)
00167 {
00168 q_tmp[i] = q[i];
00169 }
00170 ode_step(q_tmp,sigma);
00171 state_event_func(q_tmp,en);
00172
00173 is_event = false;
00174 for (int i = 0; i < zero_funcs && !is_event; i++)
00175 {
00176 is_event = (sgn(es[i]) != sgn(en[i]));
00177 }
00178 if (!is_event) break;
00179
00180 sigma /= 2.0;
00181
00182 if (sigma < 1E-12)
00183 {
00184 is_event = true;
00185 break;
00186 }
00187 }
00188 if (sigma < time_event)
00189 {
00190 return sigma;
00191 }
00192 is_event = true;
00193 return time_event;
00194 }
00195
00196 template <class X>
00197 void rk4<X>::discrete_action_func(const Bag<X>& xb)
00198 {
00199 discrete_action(q,xb);
00200 }
00201
00202 template <class X>
00203 void rk4<X>::discrete_output_func(Bag<X>& yb)
00204 {
00205 discrete_output(q,yb);
00206 }
00207
00208 template <class X>
00209 void rk4<X>::ode_step(double*qq, double step)
00210 {
00211 if (step == 0.0)
00212 {
00213 return;
00214 }
00215
00216 der_func(qq,dq);
00217 for (int j = 0; j < num_state_vars; j++)
00218 k[0][j] = dq[j];
00219
00220 for (int j = 0; j < num_state_vars; j++)
00221 t[j] = qq[j] + 0.5*step*k[0][j];
00222 der_func(t,dq);
00223 for (int j = 0; j < num_state_vars; j++)
00224 k[1][j] = dq[j];
00225
00226 for (int j = 0; j < num_state_vars; j++)
00227 t[j] = qq[j] + 0.5*step*k[1][j];
00228 der_func(t,dq);
00229 for (int j = 0; j < num_state_vars; j++)
00230 k[2][j] = dq[j];
00231
00232 for (int j = 0; j < num_state_vars; j++)
00233 t[j] = qq[j] + step*k[2][j];
00234 der_func(t,dq);
00235 for (int j = 0; j < num_state_vars; j++)
00236 k[3][j] = dq[j];
00237
00238 for (int j = 0; j < num_state_vars; j++)
00239 qq[j] += step*(k[0][j] + 2.0*k[1][j] + 2.0*k[2][j] + k[3][j])/6.0;
00240 }
00241
00242 }
00243
00244 #endif