00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #pragma once
00014
00015 #ifndef __ODESOLVER__
00016 #define __ODESOLVER__
00017
00018
00019 #include "NonLinearGroupElement.h"
00020 #include "POLYLIE.h"
00021 #include "libalgebra.h"
00022
00023
00025
00032 template <typename my_alg_type>
00033 class OdeSolver
00034 {
00035 typedef typename my_alg_type::SCA SCA;
00036
00037 static const unsigned myDIM = my_alg_type::myDIM;
00038
00039 public:
00041 OdeSolver(const SCA epsilon=SCA(0.001)) : atol(epsilon), rtol(epsilon), S(SCA(0.9)), maxscale(SCA(10.0)), minscale(SCA(0.2))
00042 {
00043 };
00044
00046 ~OdeSolver(void)
00047 {
00048 };
00049
00051 AbstractSolutionPoint<my_alg_type> solve(AbstractSolutionPoint<my_alg_type> & InitialPoint, POLYLIE<my_alg_type> & theEquationIn)
00052 {
00053 SCA x;
00054 AbstractSolutionPoint<my_alg_type> yOLD;
00055 AbstractSolutionPoint<my_alg_type> yNEW;
00056 AbstractSolutionPoint<my_alg_type> yNEWstar;
00057 AbstractSolutionPoint<my_alg_type> k1;
00058 AbstractSolutionPoint<my_alg_type> k2;
00059 AbstractSolutionPoint<my_alg_type> k3;
00060 AbstractSolutionPoint<my_alg_type> k4;
00061 AbstractSolutionPoint<my_alg_type> k5;
00062 AbstractSolutionPoint<my_alg_type> k6;
00063 SCA err;
00064 SCA sk;
00065 SCA scale;
00066 SCA h;
00067 bool reject;
00068 int previousrejections;
00069
00070 x = SCA(0);
00071 yOLD = InitialPoint;
00072
00073 h = SCA(0.1);
00074 previousrejections = 0;
00075 reject = true;
00076
00077 while(x < SCA(1))
00078 {
00079 while(reject)
00080 {
00081 k1 = h*theEquationIn.evaluate(yOLD);
00082 k2 = h*theEquationIn.evaluate(yOLD+SCA(2)/SCA(10)*k1);
00083 k3 = h*theEquationIn.evaluate(yOLD+SCA(3)/SCA(40)*k1+SCA(9)/SCA(40)*k2);
00084 k4 = h*theEquationIn.evaluate(yOLD+SCA(44)/SCA(45)*k1-SCA(56)/SCA(15)*k2+SCA(32)/SCA(9)*k3);
00085 k5 = h*theEquationIn.evaluate(yOLD+SCA(19372)/SCA(6561)*k1-SCA(25360)/SCA(2187)*k2+SCA(64448)/SCA(6561)*k3-SCA(212)/SCA(729)*k4);
00086 k6 = h*theEquationIn.evaluate(yOLD+SCA(9017)/SCA(3168)*k1-SCA(355)/SCA(33)*k2+SCA(46732)/SCA(5247)*k3+SCA(49)/SCA(176)*k4-SCA(5103)/SCA(18656)*k5);
00087
00088 yNEW = yOLD + SCA(35)/SCA(384)*k1 + SCA(500)/SCA(1113)*k3 + SCA(125)/SCA(192)*k4 - SCA(2187)/SCA(6784)*k5 + SCA(11)/SCA(84)*k6;
00089 yNEWstar = yOLD + SCA(5179)/SCA(57600)*k1 + SCA(7571)/SCA(16695)*k3 + SCA(393)/SCA(640)*k4 - SCA(92097)/SCA(339200)*k5 + SCA(187)/SCA(2100)*k6;
00090
00091 err = SCA(0);
00092
00093 for(unsigned int i=1; i<=myDIM; i++)
00094 {
00095 sk = atol + rtol*maximum(absolutevalue(yNEW[LET(i)]), absolutevalue(yNEWstar[LET(i)]));
00096 err += ((yNEW[LET(i)]-yNEWstar[LET(i)])*(yNEW[LET(i)]-yNEWstar[LET(i)]))/(sk*sk);
00097 }
00098 err = sqrt(err/myDIM);
00099
00100 if(err <= SCA(1))
00101 {
00102 reject = false;
00103 }
00104 else
00105 {
00106 scale = maximum(S*pow(err, -0.2), minscale);
00107 h *= scale;
00108 previousrejections += 1;
00109 }
00110 }
00111
00112 x += h;
00113 yOLD = yNEW;
00114
00115
00116 if(err == SCA(0))
00117 {
00118 scale = maxscale;
00119 }
00120 else
00121 {
00122 scale = S*pow(err, -0.2);
00123
00124 if(scale < minscale)
00125 {
00126 scale = minscale;
00127 }
00128
00129 if(scale > maxscale)
00130 {
00131 scale = maxscale;
00132 }
00133 }
00134
00135 if(previousrejections > 0)
00136 {
00137 if(scale > SCA(1))
00138 {
00139 scale = SCA(1);
00140 }
00141
00142 h *= scale;
00143 }
00144 else
00145 {
00146 h *= scale;
00147 }
00148
00149 previousrejections = 0;
00150
00151 if(x+h <= SCA(1))
00152 {
00153 reject = true;
00154 }
00155 else
00156 {
00157 h = SCA(1) - x;
00158
00159 if(h > SCA(0))
00160 {
00161 reject = true;
00162 }
00163 }
00164 }
00165
00166 return yNEW;
00167 };
00168
00169 private:
00171 const SCA atol;
00172
00174 const SCA rtol;
00175
00177 const SCA S;
00178
00179 const SCA maxscale;
00180
00181 const SCA minscale;
00182
00183 SCA absolutevalue(SCA a)
00184 {
00185 if(a>=0)
00186 {
00187 return a;
00188 }
00189 else
00190 {
00191 return -a;
00192 }
00193 };
00194
00195 SCA maximum(SCA a, SCA b)
00196 {
00197 if(a>=b)
00198 {
00199 return a;
00200 }
00201 else
00202 {
00203 return b;
00204 }
00205 };
00206 };
00207
00208 #endif // __ODESOLVER__