src/FDM/JSBSim/math/FGRungeKutta.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGRungeKutta.cpp
   4  Author:       Thomas Kreitler
   5  Date started: 04/9/2010
   6
   7  ------------- Copyright (C)  -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26
  27
  28 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  29   INCLUDES
  30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  31
  32 #include <cstdio>
  33 #include <iostream>
  34 #include <cmath>
  35
  36 #include "FGRungeKutta.h"
  37
  38 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  39   DEFINITIONS
  40   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  41
  42 using std::cout;
  43 using std::endl;
  44
  45 namespace JSBSim {
  46
  47 static const char *IdSrc = "$Id: FGRungeKutta.cpp,v 1.1 2010/06/02 04:05:13 jberndt Exp $";
  48 static const char *IdHdr = ID_RUNGEKUTTA;
  49
  50 const double FGRungeKutta::RealLimit = 1e30;
  51
  52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 CLASS IMPLEMENTATION
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56 FGRungeKutta::~FGRungeKutta() { };
  57
  58 int FGRungeKutta::init(double x_start, double x_end, int intervals)
  59 {
  60   x0 = x_start;
  61   x1 = x_end;
  62   h  = (x_end - x_start)/intervals;
  63   safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
  64   h05 = h*0.5;
  65   err = 0.0;
  66
  67   if (x0>=x1) {
  68     status &= eFaultyInit;
  69   }
  70   return status;
  71 }
  72
  73 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  74
  75 /*
  76    Make sure that a numerical result is within +/-RealLimit.
  77    This is a hapless try to be portable.
  78    (There will be at least one architecture/compiler combination
  79    where this will fail.)
  80 */
  81
  82 bool FGRungeKutta::sane_val(double x)
  83 {
  84   // assuming +/- inf behave as expected and 'nan' comparisons yield to false
  85   if ( x < RealLimit && x > -RealLimit ) return true;
  86   return false;
  87 }
  88
  89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  90
  91 double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
  92 {
  93   double x = x0;
  94   double y = y_0;
  95   pfo = pf;
  96
  97   iterations = 0;
  98
  99   if (!trace_values) {
 100     while (x<safer_x1) {
 101       y  = approximate(x,y);
 102       if (!sane_val(y)) { status &= eMathError; }
 103       x += h;
 104       iterations++;
 105     }
 106   } else {
 107     while (x<safer_x1) {
 108       cout << x << " " << y << endl;
 109       y = approximate(x,y);
 110       if (!sane_val(y)) { status &= eMathError; }
 111       x += h;
 112       iterations++;
 113     }
 114     cout << x << " " << y << endl;
 115   }
 116
 117   x_end = x; // twimc, store the last x used.
 118   return y;
 119 }
 120
 121
 122
 123 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 124 CLASS IMPLEMENTATION
 125 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 126
 127 FGRK4::~FGRK4() { };
 128
 129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 130
 131 double FGRK4::approximate(double x, double y)
 132 {
 133   double k1,k2,k3,k4;
 134
 135   k1   =  pfo->pFunc(x      , y         );
 136   k2   =  pfo->pFunc(x + h05, y + h05*k1);
 137   k3   =  pfo->pFunc(x + h05, y + h05*k2);
 138   k4   =  pfo->pFunc(x + h  , y + h  *k3);
 139
 140   y   +=  h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
 141
 142   return y;
 143 }
 144
 145
 146 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 147 CLASS IMPLEMENTATION
 148 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 149
 150 // Butcher tableau
 151 const double FGRKFehlberg::A2[] = { 0.0,     1.0/4.0 };
 152 const double FGRKFehlberg::A3[] = { 0.0,     3.0/32.0,       9.0/32.0 };
 153 const double FGRKFehlberg::A4[] = { 0.0,  1932.0/2197.0, -7200.0/2197.0,   7296.0/2197.0 };
 154 const double FGRKFehlberg::A5[] = { 0.0,   439.0/216.0,     -8.0,          3680.0/513.0,    -845.0/4104.0  };
 155 const double FGRKFehlberg::A6[] = { 0.0,    -8.0/27.0,       2.0,         -3544.0/2565.0,   1859.0/4104.0,  -11.0/40.0 };
 156
 157 const double FGRKFehlberg::C[]  = { 0.0, 0.0, 1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0 };
 158
 159 const double FGRKFehlberg::B[]  = { 0.0,     16.0/135.0,   0.0,   6656.0/12825.0,  28561.0/56430.0,   -9.0/50.0,  2.0/55.0 };
 160 const double FGRKFehlberg::Bs[] = { 0.0,     25.0/216.0,   0.0,   1408.0/2565.0,    2197.0/4104.0,    -1.0/5.0,   0.0 };
 161
 162 // use this if truncation is an issue
 163 // const double Ee[] = { 0.0, 1.0/360.0, 0.0, -128.0/4275.0, -2197.0/75240.0, 1.0/50.0, 2.0/55.0 };
 164
 165 FGRKFehlberg::~FGRKFehlberg() { };
 166
 167 double FGRKFehlberg::approximate(double x, double y)
 168 {
 169
 170   double k1,k2,k3,k4,k5,k6, as;
 171
 172   double y4_val;
 173   double y5_val;
 174   double abs_err;
 175   double est_step;
 176   int done = 0;
 177
 178
 179   while (!done) {
 180
 181     err  =  h*h*h*h*h; // h might change
 182
 183     k1   =  pfo->pFunc(x          , y      );
 184
 185     as   =  h*A2[1]*k1;
 186     k2   =  pfo->pFunc(x + C[2]*h , y + as );
 187
 188     as   =  h*(A3[1]*k1 + A3[2]*k2);
 189     k3   =  pfo->pFunc(x + C[3]*h , y + as );
 190
 191     as   =  h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
 192     k4   =  pfo->pFunc(x + C[4]*h , y + as );
 193
 194     as   =  h*(A5[1]*k1 + A5[2]*k2 + A5[3]*k3 + A5[4]*k4);
 195     k5   =  pfo->pFunc(x + C[5]*h , y + as );
 196
 197     as   =  h*(A6[1]*k1 + A6[2]*k2 + A6[3]*k3 + A6[4]*k4 + A6[5]*k5);
 198     k6   =  pfo->pFunc(x + C[6]*h , y + as );
 199
 200     /* B[2]*k2 and Bs[2]*k2 are zero */
 201     y5_val  =  y + h * ( B[1]*k1 +  B[3]*k3 +  B[4]*k4 +  B[5]*k5 + B[6]*k6);
 202     y4_val  =  y + h * (Bs[1]*k1 + Bs[3]*k3 + Bs[4]*k4 + Bs[5]*k5);
 203
 204     abs_err = fabs(y4_val-y5_val);
 205     // same in green
 206     // abs_err = h * (Ee[1] * k1 + Ee[3] * k3 + Ee[4] * k4 + Ee[5] * k5 + Ee[6] * k6);
 207
 208     // estimate step size
 209     if (abs_err > epsilon) {
 210       est_step = sqrt(sqrt(epsilon*h/abs_err));
 211     } else {
 212       est_step=2.0*h; // cheat
 213     }
 214
 215     // check if a smaller step size is proposed
 216
 217     if (shrink_avail>0 && est_step<h) {
 218         h/=2.0;
 219         shrink_avail--;
 220     } else {
 221       done = 1;
 222     }
 223
 224   }
 225
 226   return y4_val;
 227 }
 228
 229
 230 } // namespace JSBSim