CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

RKIntegrator.cc
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 // $Id: 
00003 #include "CLHEP/GenericFunctions/RKIntegrator.hh"
00004 #include "CLHEP/GenericFunctions/Variable.hh"
00005 #include <climits>
00006 #include <cmath>      // for pow()
00007 #include <stdexcept>
00008 namespace Genfun {
00009 FUNCTION_OBJECT_IMP(RKIntegrator::RKFunction)
00010 
00011 RKIntegrator::RKFunction::RKFunction(RKData *data, unsigned int index)
00012   :_data(data),
00013    _index(index)
00014 {
00015   _data->ref();
00016 }
00017 
00018 RKIntegrator::RKFunction::~RKFunction() 
00019 {
00020   _data->unref();
00021 }
00022 
00023 RKIntegrator::RKFunction::RKFunction(const RKIntegrator::RKFunction & right)
00024   :_data(right._data),
00025    _index(right._index)
00026 {
00027   _data->ref();
00028 }
00029 
00030 
00031 double RKIntegrator::RKFunction::operator() (double t) const {
00032   if (t<0) return 0;
00033   if (!_data->_locked) _data->lock();
00034 
00035 
00036   // Do this first, thereafter, just read the cache
00037   _data->recache();
00038   
00039 
00040   // If the cache is empty, make an entry for t=0;
00041   size_t nvar = _data->_startingValParameter.size();
00042   if (_data->_fx.empty()) {
00043     RKData::Data d(nvar);
00044     d.time=0;
00045     Argument arg(nvar);
00046     for (size_t f=0;f<nvar;f++) {
00047       d.variable[f]=_data->_startingValParameterCache[f];
00048       arg[f]=d.variable[f];
00049     }
00050     _data->_fx.insert(d);
00051   }
00052 
00053 
00054   RKData::Data dt(nvar);
00055   dt.time=t;
00056   std::set<RKData::Data>::iterator s=_data->_fx.find(dt);
00057   if (s!=_data->_fx.end()) {
00058     // Then, there is nothing to do.  Don't touch the
00059     // list.  Just get the variable:
00060     return (*s).variable[_index];
00061   }
00062   else {
00063     s=_data->_fx.lower_bound(dt);
00064     // Back up:
00065     if (!(s!=_data->_fx.begin())) throw std::runtime_error("Runtime error in RKIntegrator");
00066     s--;
00067 
00068     //std::vector<double> errors;
00069     rkstep(*s, dt);
00070     _data->_fx.insert(s,dt);
00071 
00072     return dt.variable[_index];
00073   }
00074 }
00075 
00076 
00077 RKIntegrator::RKData::RKData():_locked(false) {
00078 }
00079 
00080 RKIntegrator::RKData::~RKData() {
00081   for (size_t i=0;i<_startingValParameter.size();i++) delete _startingValParameter[i];
00082   for (size_t i=0;i<_controlParameter.size();i++)     delete _controlParameter[i];
00083   for (size_t i=0;i<_diffEqn.size();  i++)            delete _diffEqn[i];
00084 }
00085 
00086 RKIntegrator::RKIntegrator() :
00087   _data(new RKData())
00088 {
00089   _data->ref();
00090 }
00091 
00092 RKIntegrator::~RKIntegrator() {
00093   _data->unref();
00094   for (size_t i=0;i<_fcn.size();i++) delete _fcn[i];
00095 }
00096 
00097 Parameter * RKIntegrator::addDiffEquation(const AbsFunction * diffEquation,
00098                                           const std::string &variableName,
00099                                           double defStartingValue,
00100                                           double defValueMin,
00101                                           double defValueMax) {
00102   Parameter *par = new Parameter(variableName, defStartingValue, defValueMin, defValueMax);
00103   _data->_startingValParameter.push_back(par);
00104   _data->_diffEqn.push_back(diffEquation->clone());
00105   _data->_startingValParameterCache.push_back(defStartingValue);
00106   _fcn.push_back(new RKFunction(_data,_fcn.size()));
00107   return par;
00108 }
00109 
00110 
00111 
00112 
00113 
00114 Parameter * RKIntegrator::createControlParameter (const std::string & variableName,
00115                                                   double defStartingValue,
00116                                                   double startingValueMin,
00117                                                   double startingValueMax) {
00118 
00119   Parameter *par = new Parameter(variableName, defStartingValue, startingValueMin, startingValueMax);
00120   _data->_controlParameter.push_back(par);
00121   _data->_controlParameterCache.push_back(defStartingValue);
00122   return par;
00123 
00124 }
00125 
00126 
00127 
00128 const RKIntegrator::RKFunction * RKIntegrator::getFunction(unsigned int i) const {
00129   return _fcn[i];
00130 }
00131 
00132 
00133 
00134 void RKIntegrator::RKData::lock() {
00135   if (!_locked) {
00136     unsigned int size = _diffEqn.size();
00137     for (size_t i=0;i<size;i++) {
00138       if (!(_diffEqn[i]->dimensionality()==size)) throw std::runtime_error("Runtime error in RKIntegrator");
00139     }
00140     _locked=true;
00141   }
00142 }
00143 
00144 
00145 
00146 void RKIntegrator::RKData::recache() {
00147 
00148   bool stale=false;
00149   if (!stale) {
00150     for (size_t p=0;p<_startingValParameter.size();p++) {
00151       if (_startingValParameter[p]->getValue()!=_startingValParameterCache[p]) {
00152         _startingValParameterCache[p]=_startingValParameter[p]->getValue();
00153         stale=true;
00154         break;
00155       }
00156     }
00157   }
00158 
00159   if (!stale) {
00160     for (size_t p=0;p<_controlParameter.size();p++) {
00161       if (_controlParameter[p]->getValue()!=_controlParameterCache[p]) {
00162         _controlParameterCache[p]=_controlParameter[p]->getValue();
00163         stale=true;
00164         break;
00165       }
00166     }
00167   }
00168   
00169   if (stale) {
00170     _fx.erase(_fx.begin(),_fx.end());
00171   }
00172   
00173 }
00174 
00175 
00176 
00177 void RKIntegrator::RKFunction::rkstep(const RKIntegrator::RKData::Data & s, RKIntegrator::RKData::Data & d)  const {
00178   //
00179   // Adaptive stepsize control
00180   //
00181   const int nvar = s.variable.size();
00182   const double eps    = 1.0E-6;
00183   const double SAFETY = 0.9;
00184   const double PSHRNK = -0.25;
00185   const double PGROW  = -0.20;
00186   const double ERRCON = -1.89E-4;
00187   const double TINY   = 1.0E-30;
00188   double hnext;
00189 
00190   
00191   RKData::Data Tmp0(nvar),Tmp1(nvar);
00192   Tmp0=s;
00193   Tmp1=d;
00194   bool done=false;
00195   while (1) { // "Same time as"...
00196 
00197     //--------------------------------------//
00198     // Take one step, from Tmp0 to Tmp1:    //
00199     //--------------------------------------//
00200 
00201     double h = Tmp1.time - Tmp0.time;
00202     while (1) {
00203       std::vector<double> errors;
00204       
00205       rkck(Tmp0, Tmp1, errors);
00206       for (size_t e=0;e<errors.size();e++) {
00207         errors[e] = fabs(errors[e]) / (fabs(Tmp0.variable[e]) + fabs(Tmp0.firstDerivative[e]*h) + TINY);
00208       }
00209       double emax = (*std::max_element(errors.begin(),errors.end()))/eps;
00210       if (emax > 1) {
00211         h = std::max(SAFETY*h*pow(emax,PSHRNK),0.1*h);
00212         if  (!(((float) Tmp0.time+h - (float) Tmp0.time) > 0) ) {
00213           std::cerr << "Warning, RK Integrator step underflow" << std::endl;
00214         }
00215         Tmp1.time = Tmp0.time+h;
00216         continue;
00217       }
00218       else {
00219         
00220         if (emax > ERRCON) {
00221           hnext = SAFETY*h*pow(emax,PGROW);
00222         }
00223         else {
00224           hnext = 5.0*h;
00225         }
00226         if (Tmp1==d) {
00227           done=true;
00228           break;
00229         }
00230         else {
00231           Tmp0=Tmp1;
00232           Tmp1.time = std::min(Tmp0.time + hnext, d.time);
00233           break;
00234         }
00235       }
00236     }
00237 
00238     //--------------------------------------//
00239     // End of Step.                         //
00240     //--------------------------------------//
00241 
00242     if (done) break;
00243   }
00244   d=Tmp1;
00245 }
00246 
00247 void RKIntegrator::RKFunction::rkck(const RKIntegrator::RKData::Data & s, RKIntegrator::RKData::Data & d, std::vector<double> & errors)  const {
00248   
00249 #ifdef NONAUTONOMOUS_EQUATIONS
00250   static const double
00251     a2=0.2,
00252     a3=0.3,
00253     a4=0.6,
00254     a5=1.0, 
00255     a6=0.875;
00256 #endif
00257   
00258   static const double
00259     b21=0.2,
00260     b31=3.0/40.0,
00261     b32=9.0/40.0,
00262     b41=0.3,
00263     b42=-0.9,
00264     b43=1.2,
00265     b51=-11.0/54.0,
00266     b52=2.5,
00267     b53=-70.0/27.0,
00268     b54=35.0/27.0,
00269     b61=1631.0/55296.0,
00270     b62=175.0/512.0,
00271     b63=575.0/13824.0,
00272     b64=44275.0/110592.0,
00273     b65=253.0/4096.0;
00274 
00275   static const double
00276     c1=37.0/378.0,
00277     c3=250.0/621.0,
00278     c4=125.0/594.0,
00279     c6=512.0/1771.0;
00280   
00281   static const double
00282     dc1=c1-2825.0/27648.0,
00283     dc3=c3-18575.0/48384.0,
00284     dc4=c4-13525.0/55296.0,
00285     dc5=-277.0/14336.0,
00286     dc6=c6 - 0.25;
00287 
00288   // First step:
00289   double h = d.time - s.time;
00290   if (h<=0) throw std::runtime_error ("Runtime error in RKIntegrator (zero or negative stepsize)");
00291   unsigned int nv = s.variable.size();
00292   Argument arg(nv), arg0(nv), d1(nv),d2(nv), d3(nv), d4(nv), d5(nv), d6(nv);
00293   
00294 
00295   for (size_t v=0;v<nv;v++) { arg0[v]=s.variable[v];}
00296   
00297   if (!s.dcalc) {
00298     for (size_t v=0;v<nv;v++) {d1[v]=(*_data->_diffEqn[v])(arg0);}
00299     for (size_t v=0;v<nv;v++) {s.firstDerivative[v]=d1[v];}
00300     s.dcalc=true;
00301   }
00302   else {
00303     for (size_t v=0;v<nv;v++) { d1[v] = s.firstDerivative[v];}
00304   }    
00305     
00306 
00307   for (size_t v=0;v<nv;v++) { arg[v] = arg0[v] + b21*h*d1[v];} 
00308   
00309   for (size_t v=0;v<nv;v++) { d2[v] = (*_data->_diffEqn[v])(arg);}
00310   for (size_t v=0;v<nv;v++) { arg[v] = arg0[v] + h*(b31*d1[v]+b32*d2[v]);} 
00311   
00312   
00313   for (size_t v=0;v<nv;v++) { d3[v] = (*_data->_diffEqn[v])(arg);}
00314   for (size_t v=0;v<nv;v++) { arg[v] = arg0[v] + h*(b41*d1[v]+b42*d2[v]+b43*d3[v]);}
00315   
00316   for (size_t v=0;v<nv;v++) { d4[v] = (*_data->_diffEqn[v])(arg);}
00317   for (size_t v=0;v<nv;v++) { arg[v] = arg0[v] + h*(b51*d1[v]+b52*d2[v]+b53*d3[v] + b54*d4[v]);}
00318   
00319   for (size_t v=0;v<nv;v++) { d5[v] = (*_data->_diffEqn[v])(arg);}
00320   for (size_t v=0;v<nv;v++) { arg[v] = arg0[v] + h*(b61*d1[v]+b62*d2[v]+b63*d3[v] + b64*d4[v] + b65*d5[v]);}
00321   
00322   for (size_t v=0;v<nv;v++) { d6[v] = (*_data->_diffEqn[v])(arg);}
00323   
00324   for (size_t v=0;v<nv;v++) { d.variable[v] = arg0[v] + h*(c1*d1[v]+c3*d3[v]+c4*d4[v]+c6*d6[v]);}
00325   errors.erase(errors.begin(),errors.end());
00326   
00327   for (size_t v=0;v<nv;v++) { errors.push_back(h*(dc1*d1[v]+dc3*d3[v]+dc4*d4[v]+dc5*d5[v]+dc6*d6[v]));}
00328 }
00329 
00330 
00331 
00332 } // namespace Genfun