csgeom/odesolver.h
Go to the documentation of this file.00001 /* 00002 Copyright (C) 2006 by Marten Svanfeldt 00003 00004 This library is free software; you can redistribute it and/or 00005 modify it under the terms of the GNU Library General Public 00006 License as published by the Free Software Foundation; either 00007 version 2 of the License, or (at your option) any later version. 00008 00009 This library is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00012 Library General Public License for more details. 00013 00014 You should have received a copy of the GNU Library General Public 00015 License along with this library; if not, write to the Free 00016 Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 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 // We need k1-k6 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 // k1 00075 f (t0, y0, k1, size); 00076 00077 // prepare for k2 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 // k2 00085 f (t0 + 0.25f*h, tmp, k2, size); 00086 00087 // prepare for k3 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 // k3 00096 f (t0 + (3.0f/8)*h, tmp, k3, size); 00097 00098 // prepare for k4 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 // k4 00108 f (t0 + (12.0f/13)*h, tmp, k4, size); 00109 00110 // prepare for k5 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 // k5 00121 f (t0 + h, tmp, k5, size); 00122 00123 // prepare for k6 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 // k6 00135 f (t0 + 0.5f*h, tmp, k6, size); 00136 00137 ArgType errMag = 0; 00138 // Finally calculate 4th and 5th order result, error term and final result 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 // We need k1-k6 00181 csVector3 k1, k2, k3, k4, k5, k6; 00182 00183 // k1 00184 k1 = h * f (t0, y0); 00185 00186 // k2 00187 k2 = h * f (t0 + 0.25f*h, y0 + 0.25f*k1); 00188 00189 // k3 00190 k3 = h * f (t0 + (3.0f/8)*h, y0 + (3.0f/32)*k1 00191 + (9.0f/32)*k2); 00192 00193 // k4 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 // k5 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 // k6 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 // Finally calculate 4th and 5th order result, error term and final result 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 // We need k1-k6 00245 ArgType k1, k2, k3, k4, k5, k6; 00246 00247 // k1 00248 k1 = h * f (t0, y0); 00249 00250 // k2 00251 k2 = h * f (t0 + 0.25f*h, y0 + 0.25f*k1); 00252 00253 // k3 00254 k3 = h * f (t0 + (3.0f/8)*h, y0 + (3.0f/32)*k1 00255 + (9.0f/32)*k2); 00256 00257 // k4 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 // k5 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 // k6 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 // Finally calculate 4th and 5th order result, error term and final result 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
Generated for Crystal Space by doxygen 1.4.7