/home/nutarojj/adevs/trunk/include/adevs_rk4.h

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

Generated on Mon Sep 20 14:35:39 2010 for adevs by  doxygen 1.4.7