CrystalSpace

Public API Reference

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 1.2.1 by doxygen 1.5.3