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

Generated on Tue Oct 6 20:46:41 2009 for adevs by  doxygen 1.4.7