00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00023 #ifndef __CS_CSGEOM_ODESOLVER_H__
00024 #define __CS_CSGEOM_ODESOLVER_H__
00025
00026 #include "csgeom/vector3.h"
00027
00028 namespace CS
00029 {
00030 namespace Math
00031 {
00032
00046 class Ode45
00047 {
00048 public:
00049
00061 template<typename FuncType, typename ArgType>
00062 static ArgType Step (FuncType& f, ArgType h, ArgType t0, ArgType* y0,
00063 ArgType* yout, size_t size)
00064 {
00065
00066 CS_ALLOC_STACK_ARRAY(ArgType, k1, size);
00067 CS_ALLOC_STACK_ARRAY(ArgType, k2, size);
00068 CS_ALLOC_STACK_ARRAY(ArgType, k3, size);
00069 CS_ALLOC_STACK_ARRAY(ArgType, k4, size);
00070 CS_ALLOC_STACK_ARRAY(ArgType, k5, size);
00071 CS_ALLOC_STACK_ARRAY(ArgType, k6, size);
00072 CS_ALLOC_STACK_ARRAY(ArgType, tmp, size);
00073
00074
00075 f (t0, y0, k1, size);
00076
00077
00078 for (size_t i = 0; i < size; ++i)
00079 {
00080 k1[i] *= h;
00081 tmp[i] = y0[i] + 0.25*k1[i];
00082 }
00083
00084
00085 f (t0 + 0.25f*h, tmp, k2, size);
00086
00087
00088 for (size_t i = 0; i < size; ++i)
00089 {
00090 k2[i] *= h;
00091 tmp[i] = y0[i] + (3.0/32)*k1[i]
00092 + (9.0/32)*k2[i];
00093 }
00094
00095
00096 f (t0 + (3.0f/8)*h, tmp, k3, size);
00097
00098
00099 for (size_t i = 0; i < size; ++i)
00100 {
00101 k3[i] *= h;
00102 tmp[i] = y0[i] + (1932.0/2197)*k1[i]
00103 - (7200.0/2197)*k2[i]
00104 + (7296.0/2197)*k3[i];
00105 }
00106
00107
00108 f (t0 + (12.0f/13)*h, tmp, k4, size);
00109
00110
00111 for (size_t i = 0; i < size; ++i)
00112 {
00113 k4[i] *= h;
00114 tmp[i] = y0[i] + (439.0/216)*k1[i]
00115 - (8.0)*k2[i]
00116 + (3680.0/513)*k3[i]
00117 - (845.0/4104)*k4[i];
00118 }
00119
00120
00121 f (t0 + h, tmp, k5, size);
00122
00123
00124 for (size_t i = 0; i < size; ++i)
00125 {
00126 k5[i] *= h;
00127 tmp[i] = y0[i] - (8.0/27)*k1[i]
00128 + (2.0)*k2[i]
00129 - (3544.0/2565)*k3[i]
00130 + (1859.0/4104)*k4[i]
00131 - (11.0/40)*k5[i];
00132 }
00133
00134
00135 f (t0 + 0.5f*h, tmp, k6, size);
00136
00137 ArgType errMag = 0;
00138
00139 for (size_t i = 0; i < size; ++i)
00140 {
00141
00142 k6[i] *= h;
00143
00144 ArgType y4 = y0[i] + (25.0/216)*k1[i]
00145 + (1408.0/2565)*k3[i]
00146 + (2197.0/4104)*k4[i]
00147 - (1.0/5)*k5[i];
00148
00149 ArgType y5 = y0[i] + (16.0/315)*k1[i]
00150 + (6656.0/12825)*k3[i]
00151 + (28561.0/56430)*k4[i]
00152 - (9.0f/50)*k5[i]
00153 + (2.0/55)*k6[i];
00154
00155 ArgType yErr = y4 - y5;
00156
00157 yout[i] = y5 + yErr;
00158
00159 errMag += yErr*yErr;
00160 }
00161
00162 return sqrtf (errMag);
00163 }
00164
00165
00176 template<typename FuncType, typename ArgType>
00177 static float Step (FuncType& f, ArgType h, ArgType t0, csVector3 y0,
00178 csVector3& yout)
00179 {
00180
00181 csVector3 k1, k2, k3, k4, k5, k6;
00182
00183
00184 k1 = h * f (t0, y0);
00185
00186
00187 k2 = h * f (t0 + 0.25f*h, y0 + 0.25f*k1);
00188
00189
00190 k3 = h * f (t0 + (3.0f/8)*h, y0 + (3.0f/32)*k1
00191 + (9.0f/32)*k2);
00192
00193
00194 k4 = h * f (t0 + (12.0f/13)*h, y0 + (1932.0f/2197)*k1
00195 - (7200.0f/2197)*k2
00196 + (7296.0f/2197)*k3);
00197
00198
00199 k5 = h * f (t0 + h, y0 + (439.0f/216)*k1
00200 - 8.0f*k2
00201 + (3680.0f/513)*k3
00202 - (845.0f/4104)*k4);
00203
00204
00205 k6 = h * f (t0 + 0.5f*h, y0 - (8.0f/27)*k1
00206 + (2.0f)*k2
00207 - (3544.0f/2565)*k3
00208 + (1859.0f/4104)*k4
00209 - (11.0f/40)*k5);
00210
00211
00212 csVector3 y4 = y0 + (25.0f/216)*k1
00213 + (1408.0f/2565)*k3
00214 + (2197.0f/4104)*k4
00215 - (1.0f/5)*k5;
00216
00217 csVector3 y5 = y0 + (16.0f/315)*k1
00218 + (6656.0f/12825)*k3
00219 + (28561.0f/56430)*k4
00220 - (9.0f/50)*k5
00221 + (2.0f/55)*k6;
00222
00223 csVector3 yErr = y4 - y5;
00224
00225 yout = y5 + yErr;
00226
00227 return yErr.Norm ();
00228 }
00229
00240 template<typename FuncType, typename ArgType>
00241 static ArgType Step (FuncType& f, ArgType h, ArgType t0, ArgType y0,
00242 ArgType& yout)
00243 {
00244
00245 ArgType k1, k2, k3, k4, k5, k6;
00246
00247
00248 k1 = h * f (t0, y0);
00249
00250
00251 k2 = h * f (t0 + 0.25f*h, y0 + 0.25f*k1);
00252
00253
00254 k3 = h * f (t0 + (3.0f/8)*h, y0 + (3.0f/32)*k1
00255 + (9.0f/32)*k2);
00256
00257
00258 k4 = h * f (t0 + (12.0f/13)*h, y0 + (1932.0f/2197)*k1
00259 - (7200.0f/2197)*k2
00260 + (7296.0f/2197)*k3);
00261
00262
00263 k5 = h * f (t0 + h, y0 + (439.0f/216)*k1
00264 - 8.0f*k2
00265 + (3680.0f/513)*k3
00266 - (845.0f/4104)*k4);
00267
00268
00269 k6 = h * f (t0 + 0.5f*h, y0 - (8.0f/27)*k1
00270 + (2.0f)*k2
00271 - (3544.0f/2565)*k3
00272 + (1859.0f/4104)*k4
00273 - (11.0f/40)*k5);
00274
00275
00276 ArgType y4 = y0 + (25.0f/216)*k1
00277 + (1408.0f/2565)*k3
00278 + (2197.0f/4104)*k4
00279 - (1.0f/5)*k5;
00280
00281 ArgType y5 = y0 + (16.0f/315)*k1
00282 + (6656.0f/12825)*k3
00283 + (28561.0f/56430)*k4
00284 - (9.0f/50)*k5
00285 + (2.0f/55)*k6;
00286
00287 ArgType yErr = y4 - y5;
00288
00289 yout = y5 + yErr;
00290
00291 return yErr;
00292 }
00293
00294 };
00295
00296
00297 }
00298 }
00299
00300 #endif