00001 #ifndef VVELIB_ALGORITHMS_SOLVER_H
00002 #define VVELIB_ALGORITHMS_SOLVER_H
00003
00009 #include <config.h>
00010
00011 #ifdef WIN32
00012 # include <float.h>
00013 # define isnan _isnan
00014 # define finite _finite
00015 #else
00016 # include <cmath>
00017 # ifndef GXX_USE_GNUXX0X
00018 using std::isnan;
00019 # endif
00020 #endif
00021
00022 #include <iostream>
00023 #include <string>
00024 #include <util/vector.h>
00025 #include <util/matrix.h>
00026 #include <algorithms/parallel.h>
00027
00151 namespace solver
00152 {
00153 #define VERTEX graph::Vertex<VertexContent>
00154
00155 using std::endl;
00156 using std::cout;
00157 using std::cerr;
00158
00159 using parallel::memberFunction;
00160 using parallel::freeFunction;
00161 using parallel::threadEval;
00162 using parallel::threadJoin;
00163
00164 #ifdef __GNUC__
00165 double max(double a, double b) { return (a>b)?a:b; }
00166 double min(double a, double b) { return (a<b)?a:b; }
00167 #endif
00168
00169 struct default_id_t {};
00170
00171 template <size_t nb_vars, typename identifier>
00172 struct tag_holder {};
00173
00174 template <size_t nb_vars>
00175 struct _VertexInternals
00176 {
00177 typedef util::Vector<nb_vars, double> Vec;
00178 typedef util::Matrix<nb_vars,nb_vars,double> Mat;
00179
00180 Vec pDerivs;
00181 Vec ppDerivs;
00182 Vec pVars;
00183 Vec mVars;
00184 Vec fVars;
00185 Vec k1, k2, k3, k4;
00186
00187
00188 Mat AA;
00189 Mat AT;
00190 Vec b;
00191
00192
00193 Vec x;
00194 Vec x0;
00195 Vec t0;
00196 Vec r1;
00197 Vec r2;
00198 Vec p1;
00199 Vec p2;
00200 Vec AAp1;
00201 Vec ATp2;
00202 };
00203
00204 template <size_t nb_vars>
00205 struct _EdgeInternals
00206 {
00207 typedef util::Matrix<nb_vars,nb_vars,double> Mat;
00208
00209 Mat AA;
00210 Mat AT;
00211 };
00212
00218 enum SolvingMethod
00219 {
00221 Euler,
00223 AdaptiveEuler,
00225 Fixedpoint,
00227 Midpoint,
00229 RungeKutta,
00231 AdaptiveRungeKutta,
00233 AdaptiveCrankNicholson
00234 };
00235
00236 inline QTextStream& operator<<(QTextStream& s, const SolvingMethod& m)
00237 {
00238 switch(m)
00239 {
00240 case Euler:
00241 s << "Euler";
00242 break;
00243 case AdaptiveEuler:
00244 s << "AdaptiveEuler";
00245 break;
00246 case Fixedpoint:
00247 s << "Fixedpoint";
00248 break;
00249 case Midpoint:
00250 s << "Midpoint";
00251 break;
00252 case RungeKutta:
00253 s << "RungeKutta";
00254 break;
00255 case AdaptiveRungeKutta:
00256 s << "AdaptiveRungeKutta";
00257 break;
00258 case AdaptiveCrankNicholson:
00259 s << "AdaptiveCrankNicholson";
00260 break;
00261 }
00262 return s;
00263 }
00264
00265 inline QTextStream& operator>>(QTextStream& s, SolvingMethod& m)
00266 {
00267 QString name;
00268 s >> name;
00269 if( name == "Euler" )
00270 m = Euler;
00271 else if( name == "AdaptiveEuler" )
00272 m = AdaptiveEuler;
00273 else if( name == "Fixedpoint" )
00274 m = Fixedpoint;
00275 else if( name == "Midpoint" )
00276 m = Midpoint;
00277 else if( name == "RungeKutta" )
00278 m = RungeKutta;
00279 else if( name == "AdaptiveRungeKutta" )
00280 m = AdaptiveRungeKutta;
00281 else if( name == "AdaptiveCrankNicholson" )
00282 m = AdaptiveCrankNicholson;
00283 else
00284 m = Euler;
00285 return s;
00286 }
00287
00288
00289 inline std::ostream& operator<<(std::ostream& s, const SolvingMethod& m)
00290 {
00291 switch(m)
00292 {
00293 case Euler:
00294 s << "Euler";
00295 break;
00296 case AdaptiveEuler:
00297 s << "AdaptiveEuler";
00298 break;
00299 case Fixedpoint:
00300 s << "Fixedpoint";
00301 break;
00302 case Midpoint:
00303 s << "Midpoint";
00304 break;
00305 case RungeKutta:
00306 s << "RungeKutta";
00307 break;
00308 case AdaptiveRungeKutta:
00309 s << "AdaptiveRungeKutta";
00310 break;
00311 case AdaptiveCrankNicholson:
00312 s << "AdaptiveCrankNicholson";
00313 break;
00314 }
00315 return s;
00316 }
00317
00318 inline std::istream& operator>>(std::istream& s, SolvingMethod& m)
00319 {
00320 std::string name;
00321 s >> name;
00322 if( name == "Euler" )
00323 m = Euler;
00324 else if( name == "AdaptiveEuler" )
00325 m = AdaptiveEuler;
00326 else if( name == "Fixedpoint" )
00327 m = Fixedpoint;
00328 else if( name == "Midpoint" )
00329 m = Midpoint;
00330 else if( name == "RungeKutta" )
00331 m = RungeKutta;
00332 else if( name == "AdaptiveRungeKutta" )
00333 m = AdaptiveRungeKutta;
00334 else if( name == "AdaptiveCrankNicholson" )
00335 m = AdaptiveCrankNicholson;
00336 else
00337 m = Euler;
00338 return s;
00339 }
00340
00346 enum ToleranceType
00347 {
00349 MAX_COMPONENT,
00351 MEAN_COMPONENT
00352 };
00353
00354 inline std::ostream& operator<<(std::ostream& s, const ToleranceType& m)
00355 {
00356 switch(m)
00357 {
00358 case MAX_COMPONENT:
00359 s << "MaxComponent";
00360 break;
00361 case MEAN_COMPONENT:
00362 s << "MeanComponent";
00363 break;
00364 }
00365 return s;
00366 }
00367
00368 inline std::istream& operator>>(std::istream& s, ToleranceType& m)
00369 {
00370 std::string name;
00371 s >> name;
00372 if( name == "MaxComponent" )
00373 m = MAX_COMPONENT;
00374 else if( name == "MeanComponent" )
00375 m = MEAN_COMPONENT;
00376 else
00377 m = MAX_COMPONENT;
00378 return s;
00379 }
00380
00395 template <size_t nb_vars, typename identifier = default_id_t>
00396 struct Solver
00397 {
00399 typedef util::Vector<nb_vars, double> Vec;
00401 typedef util::Matrix<nb_vars,nb_vars,double> Mat;
00403 typedef _VertexInternals<nb_vars> VertexInternals;
00405 typedef _EdgeInternals<nb_vars> EdgeInternals;
00406
00408 typedef tag_holder<nb_vars,identifier> tag_t;
00409
00411
00412
00414 SolvingMethod method;
00416 SolvingMethod current_method;
00418 double dt;
00420 int step;
00421
00423 int PrintStats;
00424
00426
00427
00429 double EulerDt;
00431 double FixedPointDt;
00433 double MidPointDt;
00435 double RungeKuttaDt;
00437 double InitialDt;
00439 double MinDt;
00441 double MaxDt;
00443
00445
00446
00448 ToleranceType AEulerTolType;
00450 double AEulerIncDt;
00452 double AEulerResDt;
00454 double AEulerResTol;
00456 double AEulerLowTol;
00458 double AEulerHighTol;
00460
00462
00463
00465 double FixedPointMaxSteps;
00467 double FixedPointTol;
00469 ToleranceType FixedPointTolType;
00471
00473
00474
00476 double ARungeIncDt;
00478 double ARungeResDt;
00480 ToleranceType ARungeTolType;
00482 double ARungeResTol;
00484 double ARungeLowTol;
00486 double ARungeHighTol;
00488
00490
00491
00493 double CRIncDt;
00495 double CRResDt;
00497 double CRAvgCPU;
00499 double CRMinCPU;
00501
00503
00504
00506 double NewtTol;
00508 ToleranceType NewtTolType;
00510 int NewtMaxSteps;
00512
00514
00515
00517 double ConjGradTol;
00519 ToleranceType ConjGradTolType;
00521 double ConjGradMaxSteps;
00523 bool ConstNbPartials;
00525
00527
00528
00530 double Dx;
00532 bool PrintMatrix;
00534
00536
00537
00539 double peff;
00541 double pdt;
00543
00544
00546 bool initialized;
00547
00549 Solver()
00550 : initialized(false)
00551 {}
00552
00554 Solver( util::Parms& parms, const QString& section, bool selectAlgorithm = true )
00555 : initialized(false)
00556 {
00557 readParms(parms, section, selectAlgorithm);
00558 initialize();
00559 }
00560
00568 void readParms( util::Parms& parms, const QString& section, bool selectAlgorithm = true )
00569 {
00570 if(selectAlgorithm)
00571 {
00572 parms(section, "Solver", method);
00573 cout << "Solver selected: " << method << endl;
00574 }
00575
00576
00577 parms(section, "PrintStats", PrintStats);
00578
00579
00580 parms(section, "EulerDt", EulerDt);
00581 parms(section, "FixedPointDt", FixedPointDt);
00582 parms(section, "MidPointDt", MidPointDt);
00583 parms(section, "RungeKuttaDt", RungeKuttaDt);
00584 parms(section, "InitialDt", InitialDt);
00585 parms(section, "MaxDt", MaxDt);
00586
00587
00588 parms(section, "AEulerTolType", AEulerTolType);
00589 parms(section, "AEulerIncDt", AEulerIncDt);
00590 parms(section, "AEulerResDt", AEulerResDt);
00591 parms(section, "AEulerResTol", AEulerResTol);
00592 parms(section, "AEulerLowTol", AEulerLowTol);
00593 parms(section, "AEulerHighTol", AEulerHighTol);
00594
00595
00596 parms(section, "FixedPointMaxSteps", FixedPointMaxSteps);
00597 parms(section, "FixedPointTol", FixedPointTol);
00598 parms(section, "FixedPointTolType", FixedPointTolType);
00599
00600
00601 parms(section, "ARungeIncDt", ARungeIncDt);
00602 parms(section, "ARungeResDt", ARungeResDt);
00603 parms(section, "ARungeTolType", ARungeTolType);
00604 parms(section, "ARungeResTol", ARungeResTol);
00605 parms(section, "ARungeLowTol", ARungeLowTol);
00606 parms(section, "ARungeHighTol", ARungeHighTol);
00607
00608
00609 parms(section, "CRIncDt", CRIncDt);
00610 parms(section, "CRResDt", CRResDt);
00611 parms(section, "CRAvgCPU", CRAvgCPU);
00612 parms(section, "CRMinCPU", CRMinCPU);
00613
00614
00615 parms(section, "NewtTol", NewtTol);
00616 parms(section, "NewtTolType", NewtTolType);
00617 parms(section, "NewtMaxSteps", NewtMaxSteps);
00618
00619
00620 parms(section, "ConjGradTol", ConjGradTol);
00621 parms(section, "ConjGradTolType", ConjGradTolType);
00622 parms(section, "ConjGradMaxSteps", ConjGradMaxSteps);
00623 parms(section, "ConstNbPartials", ConstNbPartials);
00624
00625
00626 parms(section, "Dx", Dx);
00627 parms(section, "PrintMatrix", PrintMatrix);
00628
00629 initialize(false);
00630 }
00631
00637 void initialize(bool update_dt = true)
00638 {
00639 if(update_dt or !initialized)
00640 {
00641 peff = 0;
00642 pdt = 0;
00643 step = 0;
00644 dt = 0;
00645 }
00646
00647 current_method = method;
00648 switch(method)
00649 {
00650 case Euler:
00651 dt = EulerDt;
00652 break;
00653 case AdaptiveEuler:
00654 if(update_dt)
00655 dt = EulerDt;
00656 break;
00657 case Fixedpoint:
00658 dt = FixedPointDt;
00659 break;
00660 case Midpoint:
00661 dt = MidPointDt;
00662 break;
00663 case RungeKutta:
00664 dt = RungeKuttaDt;
00665 break;
00666 case AdaptiveRungeKutta:
00667 case AdaptiveCrankNicholson:
00668 if(update_dt or !initialized)
00669 dt = InitialDt;
00670 break;
00671 }
00672 initialized = true;
00673 }
00674
00678 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00679 void solveEuler(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00680 {
00681 if(!initialized)
00682 {
00683 cerr << "Error, trying to use uninitialized solver" << endl;
00684 return;
00685 }
00686 typedef graph::Vertex<VertexContent> vertex;
00687 typedef graph::VVGraph<VertexContent,EdgeContent,compact> vvgraph;
00688 forall(const vertex& v, S)
00689 {
00690 model.updateDerivatives(v, tag_t());
00691 }
00692 forall(const vertex& v, S)
00693 {
00694 Vec& vars = model.values(v, tag_t());
00695 const Vec& derivs = model.derivatives(v, tag_t());
00696 vars += dt * derivs;
00697 }
00698 step++;
00699 }
00700
00705 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00706 void solveAdaptiveEuler(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00707 {
00708 if(!initialized)
00709 {
00710 cerr << "Error, trying to use uninitialized solver" << endl;
00711 return;
00712 }
00713 typedef graph::Vertex<VertexContent> vertex;
00714 double err = 0;
00715 forall(const vertex& v, S)
00716 {
00717 model.updateDerivatives(v, tag_t());
00718 }
00719 forall(const vertex& v, S)
00720 {
00721 VertexInternals& interns = model.vertexInternals(v, tag_t());
00722 Vec& vars = model.values(v, tag_t());
00723 const Vec& derivs = model.derivatives(v, tag_t());
00724 double derr = norm(derivs - interns.pDerivs);
00725 switch(AEulerTolType)
00726 {
00727 case MEAN_COMPONENT:
00728 err += derr;
00729 break;
00730 case MAX_COMPONENT:
00731 default:
00732 if(err < derr)
00733 err = derr;
00734 }
00735 vars += dt * derivs;
00736 interns.ppDerivs = interns.pDerivs;
00737 interns.pDerivs = derivs;
00738 }
00739 if(AEulerTolType == MEAN_COMPONENT)
00740 err /= S.size();
00741
00742 if(step)
00743 {
00744 if(err > AEulerResTol && dt > MinDt)
00745 {
00746 forall( vertex v, S )
00747 {
00748 VertexInternals& interns = model.vertexInternals(v, tag_t());
00749 Vec& vars = model.values(v, tag_t());
00750 const Vec& derivs = model.derivatives(v, tag_t());
00751 vars -= dt * (1.0 - AEulerResDt) * derivs;
00752 interns.pDerivs = interns.ppDerivs;
00753 }
00754 dt *= AEulerResDt;
00755 if(PrintStats >= 1)
00756 cout << "Euler restart Err:" << err << " Timestep:" << dt << endl;
00757 }
00758 else if(err > AEulerHighTol)
00759 dt -= dt * AEulerIncDt;
00760 else if(err < AEulerLowTol)
00761 dt += dt * AEulerIncDt;
00762
00763
00764 dt = min(MaxDt, max(MinDt, dt));
00765 }
00766 step++;
00767 }
00768
00772 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00773 void solveFixedpoint(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00774 {
00775 if(!initialized)
00776 {
00777 cerr << "Error, trying to use uninitialized solver" << endl;
00778 return;
00779 }
00780 typedef graph::Vertex<VertexContent> vertex;
00781
00782 forall(const vertex& v, S)
00783 {
00784 model.updateDerivatives(v, tag_t());
00785 }
00786 forall(const vertex& v, S)
00787 {
00788 VertexInternals& interns = model.vertexInternals(v, tag_t());
00789
00790 interns.pDerivs = model.derivatives(v, tag_t());
00791 Vec& vars = model.values(v, tag_t());
00792 interns.pVars = vars;
00793
00794 vars += dt * interns.pDerivs;
00795 }
00796
00797
00798 double fxerr = 0;
00799 int fxsteps = 0;
00800 while(true)
00801 {
00802 fxsteps++;
00803 fxerr = 0;
00804 forall(const vertex& v, S)
00805 {
00806 model.updateDerivatives(v, tag_t());
00807 }
00808
00809 forall(const vertex& v, S)
00810 {
00811 VertexInternals& interns = model.vertexInternals(v, tag_t());
00812 Vec derivs = model.derivatives(v, tag_t());
00813 Vec& vars = model.values(v, tag_t());
00814 vars = interns.pVars + dt * (derivs + interns.pDerivs)/2.0;
00815 double err = norm(derivs - interns.pDerivs);
00816 switch(FixedPointTolType)
00817 {
00818 case MEAN_COMPONENT:
00819 fxerr += err;
00820 break;
00821 case MAX_COMPONENT:
00822 default:
00823 if(err > fxerr)
00824 fxerr = err;
00825 }
00826 interns.pDerivs = derivs;
00827 }
00828 if(FixedPointTolType == MEAN_COMPONENT)
00829 fxerr /= S.size();
00830
00831 if(fxerr < FixedPointTol || fxsteps >= FixedPointMaxSteps)
00832 break;
00833 }
00834 if(PrintStats >= 2)
00835 cout << "Fixed point steps:" << fxsteps << ", Err:" << fxerr << endl;
00836 step++;
00837 }
00838
00842 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00843 void solveMidpoint(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00844 {
00845 if(!initialized)
00846 {
00847 cerr << "Error, trying to use uninitialized solver" << endl;
00848 return;
00849 }
00850 typedef graph::Vertex<VertexContent> vertex;
00851
00852 forall( vertex v, S)
00853 {
00854 VertexInternals& interns = model.vertexInternals(v, tag_t());
00855 interns.pVars = model.values(v, tag_t());
00856 model.updateDerivatives(v, tag_t());
00857 interns.k1 = model.derivatives(v, tag_t());
00858 }
00859
00860
00861 forall(const vertex& v, S)
00862 {
00863 VertexInternals& interns = model.vertexInternals(v, tag_t());
00864 Vec &vars = model.values(v, tag_t());
00865 vars += dt * 0.5 * interns.k1;
00866 }
00867
00868 forall(const vertex& v, S)
00869 {
00870 VertexInternals& interns = model.vertexInternals(v, tag_t());
00871 model.updateDerivatives(v, tag_t());
00872 interns.k2 = model.derivatives(v, tag_t());
00873 }
00874
00875
00876 forall(const vertex& v, S)
00877 {
00878 VertexInternals& interns = model.vertexInternals(v, tag_t());
00879 Vec &vars = model.values(v, tag_t());
00880 vars = interns.pVars + dt * interns.k2;
00881 }
00882 step++;
00883 }
00884
00888 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00889 void solveRungeKutta(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00890 {
00891 if(!initialized)
00892 {
00893 cerr << "Error, trying to use uninitialized solver" << endl;
00894 return;
00895 }
00896 typedef graph::Vertex<VertexContent> vertex;
00897
00898 forall(const vertex& v, S)
00899 {
00900 VertexInternals& interns = model.vertexInternals(v, tag_t());
00901 interns.pVars = model.values(v, tag_t());
00902 model.updateDerivatives(v, tag_t());
00903 interns.k1 = model.derivatives(v, tag_t());
00904 }
00905
00906
00907 forall(const vertex& v, S)
00908 {
00909 VertexInternals& interns = model.vertexInternals(v, tag_t());
00910 Vec &vars = model.values(v, tag_t());
00911 vars = interns.pVars + dt * .5 * interns.k1;
00912 }
00913 forall(const vertex& v, S)
00914 {
00915 VertexInternals& interns = model.vertexInternals(v, tag_t());
00916 model.updateDerivatives(v, tag_t());
00917 interns.k2 = model.derivatives(v, tag_t());
00918 }
00919
00920
00921 forall(const vertex& v, S)
00922 {
00923 VertexInternals& interns = model.vertexInternals(v, tag_t());
00924 Vec &vars = model.values(v, tag_t());
00925 vars = interns.pVars + dt * .5 * interns.k2;
00926 }
00927 forall(const vertex& v, S)
00928 {
00929 VertexInternals& interns = model.vertexInternals(v, tag_t());
00930 model.updateDerivatives(v, tag_t());
00931 interns.k3 = model.derivatives(v, tag_t());
00932 }
00933
00934
00935 forall(const vertex& v, S)
00936 {
00937 VertexInternals& interns = model.vertexInternals(v, tag_t());
00938 Vec &vars = model.values(v, tag_t());
00939 vars = interns.pVars + dt * interns.k3;
00940 }
00941 forall(const vertex& v, S)
00942 {
00943 VertexInternals& interns = model.vertexInternals(v, tag_t());
00944 model.updateDerivatives(v, tag_t());
00945 interns.k4 = model.derivatives(v, tag_t());
00946 }
00947
00948
00949 forall(const vertex& v, S)
00950 {
00951 VertexInternals& interns = model.vertexInternals(v, tag_t());
00952 Vec &vars = model.values(v, tag_t());
00953 vars = interns.pVars;
00954 vars += dt * ((interns.k1+interns.k4)/6.0 + (interns.k2 + interns.k3)/3.0);
00955 }
00956 step++;
00957 }
00958
00963 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
00964 void solveAdaptiveRungeKutta(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
00965 {
00966 if(!initialized)
00967 {
00968 cerr << "Error, trying to use uninitialized solver" << endl;
00969 return;
00970 }
00971 typedef graph::Vertex<VertexContent> vertex;
00972 double err = 0.0;
00973 for(int rstep = 1; rstep <= 3; rstep++)
00974 {
00975 double ldt = dt;
00976 if(rstep != 1)
00977 ldt *= .5;
00978
00979
00980 if(rstep != 2)
00981 {
00982 forall(const vertex& v, S)
00983 {
00984 VertexInternals& interns = model.vertexInternals(v, tag_t());
00985 if(rstep == 1)
00986 interns.pVars = model.values(v, tag_t());
00987 model.updateDerivatives(v, tag_t());
00988 interns.k1 = model.derivatives(v, tag_t());
00989 }
00990 }
00991
00992
00993 forall(const vertex& v, S)
00994 {
00995 VertexInternals& interns = model.vertexInternals(v, tag_t());
00996 Vec &vars = model.values(v, tag_t());
00997 if(rstep != 3)
00998 vars = interns.pVars;
00999 else
01000 vars = interns.mVars;
01001 vars += ldt * .5 * interns.k1;
01002 }
01003 forall(const vertex& v, S)
01004 {
01005 model.updateDerivatives(v, tag_t());
01006 VertexInternals& interns = model.vertexInternals(v, tag_t());
01007 interns.k2 = model.derivatives(v, tag_t());
01008 }
01009
01010
01011 forall(const vertex& v, S)
01012 {
01013 VertexInternals& interns = model.vertexInternals(v, tag_t());
01014 Vec &vars = model.values(v, tag_t());
01015 if(rstep != 3)
01016 vars = interns.pVars;
01017 else
01018 vars = interns.mVars;
01019 vars += ldt * .5 * interns.k2;
01020 }
01021 forall(const vertex& v, S)
01022 {
01023 model.updateDerivatives(v, tag_t());
01024 VertexInternals& interns = model.vertexInternals(v, tag_t());
01025 interns.k3 = model.derivatives(v, tag_t());
01026 }
01027
01028
01029 forall(const vertex& v, S)
01030 {
01031 VertexInternals& interns = model.vertexInternals(v, tag_t());
01032 Vec &vars = model.values(v, tag_t());
01033 if(rstep != 3)
01034 vars = interns.pVars;
01035 else
01036 vars = interns.mVars;
01037 vars += ldt * interns.k3;
01038 }
01039 forall(const vertex& v, S)
01040 {
01041 model.updateDerivatives(v, tag_t());
01042 VertexInternals& interns = model.vertexInternals(v, tag_t());
01043 interns.k4 = model.derivatives(v, tag_t());
01044 }
01045
01046
01047 forall(const vertex& v, S)
01048 {
01049 VertexInternals& interns = model.vertexInternals(v, tag_t());
01050 Vec &vars = model.values(v, tag_t());
01051 if(rstep != 3)
01052 vars = interns.pVars;
01053 else
01054 vars = interns.mVars;
01055
01056 vars += ldt * ((interns.k1 + interns.k4)/6.0 + (interns.k2 + interns.k3)/3.0);
01057 if(rstep == 1)
01058 {
01059 interns.fVars = vars;
01060 }
01061 else if(rstep == 2)
01062 {
01063 interns.mVars = vars;
01064 }
01065 else
01066 {
01067 double derr = norm(vars - interns.fVars)/norm(interns.fVars);
01068 switch(ARungeTolType)
01069 {
01070 case MEAN_COMPONENT:
01071 err += derr;
01072 break;
01073 case MAX_COMPONENT:
01074 default:
01075 if(derr > err)
01076 err = derr;
01077 }
01078 }
01079 }
01080 }
01081 if(ARungeTolType == MEAN_COMPONENT)
01082 err /= S.size();
01083
01084
01085 if(step != 0)
01086 {
01087 if(err > ARungeResTol && dt > MinDt)
01088 {
01089 forall(const vertex& v, S)
01090 {
01091 VertexInternals &interns = model.vertexInternals(v, tag_t());
01092 Vec &vars = model.values(v, tag_t());
01093 vars = interns.pVars;
01094 }
01095 dt *= ARungeResDt;
01096 if(PrintStats >= 1)
01097 cout << "Runge-Kutta restart Err:" << err << " Timestep:" << dt << endl;
01098 }
01099 else if(err > ARungeHighTol)
01100 dt -= dt * ARungeIncDt;
01101 else if(err < ARungeLowTol)
01102 dt += dt * ARungeIncDt;
01103
01104
01105 dt = min(MaxDt, max(MinDt, dt));
01106 }
01107 step++;
01108 }
01109
01114 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
01115 void FindPartials(const VERTEX& v, graph::VVGraph<VertexContent,EdgeContent,compact>& S, double dt,
01116 int newtstep, Model& model)
01117 {
01118 if(!initialized)
01119 {
01120 cerr << "Error, trying to use uninitialized solver" << endl;
01121 return;
01122 }
01123 typedef graph::Vertex<VertexContent> vertex;
01124
01125 VertexInternals& interns = model.vertexInternals(v, tag_t());
01126 model.updateDerivatives(v, tag_t());
01127 Vec ovars = model.values(v, tag_t());
01128 Vec odels = model.derivatives(v, tag_t());
01129
01130
01131 for(size_t i = 0; i < nb_vars; i++)
01132 {
01133 Vec& vars = model.values(v, tag_t());
01134 vars = ovars;
01135 vars[i] += Dx;
01136 model.updateDerivatives(v, tag_t());
01137 Vec dels = model.derivatives(v, tag_t());
01138 Vec partials = (dels - odels)/Dx * dt/2.0;
01139 partials[i] -= 1.0;
01140 interns.AT[i] = partials;
01141 }
01142 interns.AA = transpose(interns.AT);
01143 model.values(v, tag_t()) = ovars;
01144
01145
01146 if(newtstep == 0 || !ConstNbPartials)
01147 {
01148
01149 forall(const vertex& n, S.neighbors(v))
01150 {
01151
01152 Vec ovars = model.values(n, tag_t());
01153 EdgeInternals &einterns = model.edgeInternals(v,n,S, tag_t());
01154
01155
01156 for(size_t i = 0; i < nb_vars; i++)
01157 {
01158 Vec &vars = model.values(n, tag_t());
01159 vars = ovars;
01160 vars[i] += Dx;
01161 model.updateDerivatives(v, tag_t());
01162 Vec dels = model.derivatives(v, tag_t());
01163 Vec partials = (dels - odels)/Dx * dt/2.0;
01164 einterns.AT[i] = partials;
01165 }
01166 model.edgeInternals(n,v,S,tag_t()).AA = transpose(einterns.AT);
01167 model.values(n, tag_t()) = ovars;
01168 }
01169 }
01170 model.derivatives(v, tag_t()) = odels;
01171 }
01172
01173
01178 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
01179 void solveAdaptiveCrankNicholson(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
01180 {
01181 if(!initialized)
01182 {
01183 cerr << "Error, trying to use uninitialized solver" << endl;
01184 return;
01185 }
01186 typedef graph::Vertex<VertexContent> vertex;
01187 int conjGradMaxSteps = int(ConjGradMaxSteps*S.size());
01188 int newthalf = (int)(NewtMaxSteps/2.0);
01189 int cghalf = (int)(conjGradMaxSteps/2.0);
01190
01191 int cgsteps = 0;
01192 int newtsteps = 0;
01193 long cpu = 0;
01194
01195 double rr = 0;
01196 newtsteps = 0;
01197
01198
01199 forall(const vertex& v, S)
01200 {
01201 model.vertexInternals(v, tag_t()).x0 = model.values(v, tag_t());
01202 }
01203
01204
01205 bool restart = false;
01206 while(true)
01207 {
01208 if(restart)
01209 {
01210 cgsteps = newtsteps = 0;
01211 cpu = 1;
01212 dt *= CRResDt;
01213 restart = false;
01214
01215 forall(const vertex& v, S)
01216 {
01217 model.values(v, tag_t()) = model.vertexInternals(v, tag_t()).x0;
01218 }
01219 }
01220 if(newtsteps == 0)
01221 {
01222
01223 forall(const vertex& v, S)
01224 {
01225 model.updateDerivatives(v, tag_t());
01226 VertexInternals &interns = model.vertexInternals(v, tag_t());
01227 interns.t0 = model.derivatives(v, tag_t()) * dt/2.0;
01228 interns.t0 += interns.x0;
01229 }
01230 }
01231
01232
01233 forall(const vertex& v, S)
01234 {
01235 FindPartials(v, S, dt, newtsteps, model);
01236 VertexInternals &interns = model.vertexInternals(v, tag_t());
01237 interns.b = (model.values(v, tag_t())
01238 - model.derivatives(v, tag_t())*dt/2.0
01239 - interns.t0);
01240 }
01241
01242 newtsteps++;
01243
01244
01245 long newtcpu = 1;
01246 int newtover = newtsteps - newthalf;
01247 if(newtover > 1)
01248 {
01249 newtcpu += newtover;
01250 if(PrintStats >= 3)
01251 cout << "Newton step:" << newtsteps << " penalized by:" << newtover << endl;
01252 }
01253 if(!ConstNbPartials)
01254 newtcpu *= nb_vars;
01255 cpu += newtcpu;
01256
01257
01258 if(PrintMatrix)
01259 {
01260 cout << "Bi-conjugate Gradient Matrix" << endl;
01261
01262 }
01263
01264
01265 rr = 0;
01266 double orr = 0;
01267 forall(const vertex& v, S)
01268 {
01269 VertexInternals &interns = model.vertexInternals(v, tag_t());
01270 interns.x = 0.0;
01271 interns.r1 = interns.r2 = -interns.b;
01272 interns.p1 = -interns.r1;
01273 interns.p2 = -interns.r2;
01274 rr += interns.r2 * interns.r1;
01275 }
01276
01277
01278 if(PrintStats >= 3)
01279 cout << "Bi-conjugate gradient initial residual:" << fabs(rr)/S.size() << " step size:" << dt << endl;
01280
01281
01282 cgsteps = 0;
01283 double cgerr = 0;
01284 while(true)
01285 {
01286 cgsteps++;
01287
01288
01289 int cgcpu = 1;
01290 int cgover = cgsteps - cghalf;
01291 if(cgover > 1)
01292 {
01293 cgcpu += cgover;
01294 if(PrintStats >= 3)
01295 cout << "Bi-conjugate gradient step:" << cgsteps << " penalized by:" << cgover << endl;
01296 }
01297 cpu += cgcpu;
01298
01299
01300 double p2AAp1 = 0;
01301 forall(const vertex& v, S)
01302 {
01303 VertexInternals &v_interns = model.vertexInternals(v, tag_t());
01304 v_interns.AAp1 = v_interns.AA * v_interns.p1;
01305 v_interns.ATp2 = v_interns.AT * v_interns.p2;
01306 forall(const vertex& n, S.neighbors(v))
01307 {
01308 VertexInternals &n_interns = model.vertexInternals(n, tag_t());
01309 EdgeInternals &e_interns = model.edgeInternals(v, n, S, tag_t());
01310 v_interns.AAp1 += e_interns.AA * n_interns.p1;
01311 v_interns.ATp2 += e_interns.AT * n_interns.p2;
01312 }
01313 p2AAp1 += v_interns.p2 * v_interns.AAp1;
01314 }
01315
01316
01317 double alpha = rr / p2AAp1;
01318
01319
01320 orr = rr;
01321 rr = 0;
01322 cgerr = 0;
01323 forall(const vertex& v, S)
01324 {
01325 VertexInternals &interns = model.vertexInternals(v, tag_t());
01326
01327 interns.x += alpha * interns.p1;
01328
01329
01330 interns.r1 += alpha * interns.AAp1;
01331 interns.r2 += alpha * interns.ATp2;
01332 double res = interns.r2 * interns.r1;
01333 rr += res;
01334 res = fabs(res);
01335 switch(ConjGradTolType)
01336 {
01337 case MEAN_COMPONENT:
01338 cgerr += res;
01339 break;
01340 case MAX_COMPONENT:
01341 default:
01342 if(res > cgerr)
01343 cgerr = res;
01344 }
01345 }
01346 if(ConjGradTolType == MEAN_COMPONENT)
01347 cgerr /= S.size();
01348
01349
01350 double beta = rr/orr;
01351 forall(const vertex& v, S)
01352 {
01353 VertexInternals &interns = model.vertexInternals(v, tag_t());
01354 interns.p1 = beta * interns.p1 - interns.r1;
01355 interns.p2 = beta * interns.p2 - interns.r2;
01356 }
01357
01358
01359 if(PrintStats >= 3)
01360 cout << "Bi-conj grad step:" << cgsteps << " Residual:" << cgerr << endl;
01361
01362
01363 if(cgerr < ConjGradTol)
01364 break;
01365
01366
01367 if(cgsteps >= conjGradMaxSteps || isnan(cgerr) || !finite(cgerr))
01368 {
01369 if(PrintStats >= 1)
01370 {
01371 if(cgsteps >= conjGradMaxSteps)
01372 cout << "Restarting step:" << step << ", Conj-Grad too many iterations at stepsize:" << dt << endl;
01373 else
01374 cout << "Restarting step:" << step << ", Conj-Grad residual out of bounds" << endl;
01375 }
01376 restart = true;
01377 break;
01378 }
01379 }
01380
01381 double newterr = 0.0;
01382 if(cgsteps > 0 && !restart)
01383 {
01384
01385 forall(const vertex& v, S)
01386 {
01387 VertexInternals &interns = model.vertexInternals(v, tag_t());
01388 Vec &vars = model.values(v, tag_t());
01389 vars += interns.x;
01390 double corr = norm(interns.x);
01391 switch(NewtTolType)
01392 {
01393 case MEAN_COMPONENT:
01394 newterr += corr;
01395 break;
01396 case MAX_COMPONENT:
01397 default:
01398 if(corr > newterr)
01399 newterr = corr;
01400 }
01401 }
01402 if(NewtTolType == MEAN_COMPONENT)
01403 newterr /= S.size();
01404
01405 if(PrintStats >= 2)
01406 {
01407 cout << "Newton step:" << newtsteps << " Corr Size:" << newterr << " ";
01408 cout << "Bi-Conj steps:" << cgsteps << " Residual:" << cgerr << endl;
01409 }
01410 if(newterr < NewtTol && !restart)
01411 break;
01412 }
01413
01414 if(newtsteps >= NewtMaxSteps || isnan(newterr) || !finite(newterr))
01415 {
01416 if(PrintStats >= 1)
01417 {
01418 if(newtsteps >= NewtMaxSteps)
01419 cout << "Restarting:" << step << ", Newton iterations exceeded at stepsize:" << dt << endl;
01420 else
01421 cout << "Restarting:" << step << ", Newton correction out of bounds" << endl;
01422 }
01423 restart = true;
01424 }
01425 }
01426
01427
01428 double eff = dt/double(cpu);
01429 double inc = dt * CRIncDt;
01430 if(cpu > CRMinCPU)
01431 {
01432 if(peff > eff)
01433 {
01434 if(dt >= pdt)
01435 inc *= -1;
01436 }
01437 else
01438 {
01439 if(dt < pdt)
01440 inc *= -1;
01441 }
01442 }
01443
01444 peff = (peff * (1.0 - CRAvgCPU) + CRAvgCPU * eff);
01445 pdt = dt;
01446
01447 if(PrintStats >= 2)
01448 cout << "Stepsize:" << dt << ", current increment:" << inc << endl;
01449
01450 dt += inc;
01451
01452 if(dt > MaxDt)
01453 dt = MaxDt;
01454 }
01455
01459 template <typename VertexContent, typename EdgeContent, bool compact, typename Model>
01460 void operator()(graph::VVGraph<VertexContent,EdgeContent,compact>& S, Model& model)
01461 {
01462 switch(current_method)
01463 {
01464 case Euler:
01465 solveEuler(S, model);
01466 break;
01467 case AdaptiveEuler:
01468 solveAdaptiveEuler(S, model);
01469 break;
01470 case Fixedpoint:
01471 solveFixedpoint(S, model);
01472 break;
01473 case Midpoint:
01474 solveMidpoint(S, model);
01475 break;
01476 case RungeKutta:
01477 solveRungeKutta(S, model);
01478 break;
01479 case AdaptiveRungeKutta:
01480 solveAdaptiveRungeKutta(S, model);
01481 break;
01482 case AdaptiveCrankNicholson:
01483 solveAdaptiveCrankNicholson(S, model);
01484 break;
01485 }
01486 }
01487 };
01488
01489 #undef VERTEX
01490 };
01491
01492 #endif // VVELIB_ALGORITHMS_SOLVER_H
01493