Main Page | Class Hierarchy | Class List | File List | Class Members

/home/nutarojj/Desktop/adevs/include/adevs_rk45.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_rk45_h_
00021 #define _adevs_rk45_h_
00022 #include "adevs_dess.h"
00023 #include <cmath>
00024 #include <algorithm>
00025 
00026 namespace adevs
00027 {
00028 
00038 template <class X> class rk45: public DESS<X>
00039 {
00040         public:
00048                 rk45(int num_state_vars, double h_max, double err_tol,
00049                                 int zero_crossing_funcs, double event_tol = 1E-12);
00053                 void init(int i, double q0) { q[i] = q0; }
00057                 const double* getStateVars() const { return q; }
00061                 int getNumStateVars() const { return num_state_vars; }
00066                 virtual void der_func(const double* q, double* dq) = 0;
00073                 virtual void state_event_func(const double* q, double* z) = 0;
00078                 virtual double time_event_func(const double* q) = 0;
00086                 virtual void discrete_action(double* q, const Bag<X>& xb) = 0;
00091                 virtual void discrete_output(const double* q, Bag<X>& yb) = 0;
00100                 virtual void state_changed(const double* q){}
00101                 // Implementation of the DESS evolve_func method
00102                 void evolve_func(double h);
00103                 // Implementation of the DESS next_event_func method
00104                 double next_event_func(bool& is_event);
00105                 // Implementation of the DESS discrete_action_func method
00106                 void discrete_action_func(const Bag<X>& xb);
00107                 // Implementation of the DESS dscrete_output_func method
00108                 void discrete_output_func(Bag<X>& yb);
00109                 // Implementation of the DESS state changed method
00110                 void state_changed() { state_changed(q); }
00112                 ~rk45();
00113 
00114         private:
00115                 // Maximum step size, trunc. err. tolerance, and
00116                 // state event detection time tolerance
00117                 const double h_max, err_tol, event_tol;
00118                 // Scratch variables to implement the integration 
00119                 // and event detection scheme
00120                 double *q, *dq, *t, *k[6], *q_tmp, *es, *en;
00121                 // Current time step selection
00122                 double h_cur;
00123                 // Number of state variables and level crossing functions
00124                 int num_state_vars, zero_funcs;
00125                 /*
00126                  * This makes an appropriate sized step and stores the
00127                  * result in qq. The method returns truncation error
00128                  * estimate.
00129                  */
00130                 double ode_step(double *qq, double step);
00131 
00132                 static int sgn(double x)
00133                 {
00134                         if (x < 0.0) return -1;
00135                         else if (x > 0.0) return 1;
00136                         else return 0;
00137                 }
00138 };
00139 
00140 template <class X>
00141 rk45<X>::rk45(int num_state_vars, double h_max, double err_tol, int zero_funcs, double event_tol):
00142 DESS<X>(),
00143 h_max(h_max),
00144 err_tol(err_tol),
00145 event_tol(event_tol),
00146 num_state_vars(num_state_vars),
00147 zero_funcs(zero_funcs)
00148 {
00149         h_cur = h_max;
00150         q = new double[num_state_vars];
00151         dq = new double[num_state_vars];
00152         t = new double[num_state_vars];
00153         q_tmp = new double[num_state_vars];
00154         for (int i = 0; i < 6; i++)
00155                 k[i] = new double[num_state_vars];
00156         en = new double[zero_funcs];
00157         es = new double[zero_funcs];
00158 }
00159 
00160 template <class X>
00161 rk45<X>::~rk45()
00162 {
00163         delete [] q;
00164         delete [] dq;
00165         delete [] t;
00166         delete [] q_tmp;
00167         for (int i = 0; i < 6; i++)
00168                 delete [] k[i];
00169         delete [] es;
00170         delete [] en;
00171 }
00172 
00173 template <class X>
00174 void rk45<X>::evolve_func(double h)
00175 {
00176         ode_step(q,h);
00177 }
00178 
00179 template <class X>
00180 double rk45<X>::next_event_func(bool& is_event)
00181 {
00182         // Get the next time event
00183         double time_event = time_event_func(q);
00184         // Look for the next integrator step size
00185         h_cur = std::min(h_max,h_cur*1.1); // Try something a little bigger
00186         for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00187         while (ode_step(q_tmp,h_cur) > err_tol) // Shrink until the tolerance is met
00188         {
00189                 h_cur *= 0.9;
00190                 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00191         }
00192         // Look for the next state event
00193         double sigma = h_cur;
00194         state_event_func(q,es);
00195         for (int i = 0; i < zero_funcs; i++) en[i] = es[i];
00196         while (true)
00197         {
00198                 // Look for an event in the next time step
00199                 for (int i = 0; i < num_state_vars; i++) q_tmp[i] = q[i];
00200                 // This will update the h_cur variable to satisfy
00201                 // the error tolerance. Therefore, the step size
00202                 // selection for error will also be found during
00203                 // the search for state events
00204                 ode_step(q_tmp,sigma);
00205                 state_event_func(q_tmp,en);
00206                 // If no event, then return
00207                 is_event = false;
00208                 for (int i = 0; i < zero_funcs && !is_event; i++)
00209                 {
00210                         is_event = (sgn(es[i]) != sgn(en[i]));
00211                 }
00212                 if (!is_event) break;
00213                 // Otherwise, halve the interval and try again
00214                 sigma /= 2.0;
00215                 // If t_event is small, then just return, its been found
00216                 if (sigma < event_tol)
00217                 {
00218                         is_event = true;
00219                         break;
00220                 }
00221         }
00222         // Return the time until the next event
00223         if (sigma < time_event) 
00224         {
00225                 return sigma;
00226         }
00227         is_event = true;
00228         return time_event;
00229 }
00230 
00231 template <class X>      
00232 void rk45<X>::discrete_action_func(const Bag<X>& xb)
00233 {
00234         discrete_action(q,xb);
00235 }
00236 
00237 template <class X>      
00238 void rk45<X>::discrete_output_func(Bag<X>& yb)
00239 {
00240         discrete_output(q,yb);
00241 }
00242 
00243 template <class X>      
00244 double rk45<X>::ode_step(double*qq, double step)
00245 {
00246         if (step == 0.0)
00247         {
00248                 return 0.0;
00249         }
00250         // Compute k1
00251         der_func(qq,dq); 
00252         for (int j = 0; j < num_state_vars; j++)
00253                 k[0][j] = step*dq[j];
00254         // Compute k2
00255         for (int j = 0; j < num_state_vars; j++)
00256                 t[j] = qq[j] + 0.5*k[0][j];
00257         der_func(t,dq);
00258         for (int j = 0; j < num_state_vars; j++)
00259                 k[1][j] = step*dq[j];
00260         // Compute k3
00261         for (int j = 0; j < num_state_vars; j++)
00262                 t[j] = qq[j] + 0.25*(k[0][j]+k[1][j]);
00263         der_func(t,dq);
00264         for (int j = 0; j < num_state_vars; j++)
00265                 k[2][j] = step*dq[j];
00266         // Compute k4
00267         for (int j = 0; j < num_state_vars; j++)
00268                 t[j] = qq[j] - k[1][j] + 2.0*k[2][j];
00269         der_func(t,dq);
00270         for (int j = 0; j < num_state_vars; j++)
00271                 k[3][j] = step*dq[j];
00272         // Compute k5
00273         for (int j = 0; j < num_state_vars; j++)
00274                 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];
00275         der_func(t,dq);
00276         for (int j = 0; j < num_state_vars; j++)
00277                 k[4][j] = step*dq[j];
00278         // Compute k6
00279         for (int j = 0; j < num_state_vars; j++)
00280                 t[j] = qq[j] + (28.0/625.0)*k[0][j] - 0.2*k[1][j] + (546.0/625.0)*k[2][j]
00281                         + (54.0/625.0)*k[3][j] - (378.0/625.0)*k[4][j];
00282         der_func(t,dq);
00283         for (int j = 0 ; j < num_state_vars; j++)
00284                 k[5][j] = step*dq[j];
00285         // Compute next state and maximum approx. error
00286         double err = 0.0;
00287         for (int j = 0; j < num_state_vars; j++)
00288         {
00289                 qq[j] += (1.0/24.0)*k[0][j] + (5.0/48.0)*k[3][j] + 
00290                         (27.0/56.0)*k[4][j] + (125.0/336.0)*k[5][j];
00291                 err = std::max(err,
00292                                 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
00293                                         -125.0*k[5][j]/336.0));
00294         }
00295         return err;
00296 }
00297 
00298 } // end of namespace
00299 
00300 #endif

Generated on Mon Jun 1 09:53:42 2009 for adevs by  doxygen 1.3.9.1