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