CLHEP VERSION 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 <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