1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Header: FGRungeKutta.cpp
4 Author: Thomas Kreitler
5 Date started: 04/9/2010
7 ------------- Copyright (C) -------------
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
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
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.
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.
28 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
36 #include "FGRungeKutta.h"
38 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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;
50 const double FGRungeKutta::RealLimit = 1e30;
52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56 FGRungeKutta::~FGRungeKutta() { };
58 int FGRungeKutta::init(double x_start, double x_end, int intervals)
62 h = (x_end - x_start)/intervals;
63 safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
68 status &= eFaultyInit;
73 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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.)
82 bool FGRungeKutta::sane_val(double x)
84 // assuming +/- inf behave as expected and 'nan' comparisons yield to false
85 if ( x < RealLimit && x > -RealLimit ) return true;
89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
101 y = approximate(x,y);
102 if (!sane_val(y)) { status &= eMathError; }
108 cout << x << " " << y << endl;
109 y = approximate(x,y);
110 if (!sane_val(y)) { status &= eMathError; }
114 cout << x << " " << y << endl;
117 x_end = x; // twimc, store the last x used.
123 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
125 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131 double FGRK4::approximate(double x, double y)
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);
140 y += h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
146 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
148 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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 };
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 };
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 };
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 };
165 FGRKFehlberg::~FGRKFehlberg() { };
167 double FGRKFehlberg::approximate(double x, double y)
170 double k1,k2,k3,k4,k5,k6, as;
181 err = h*h*h*h*h; // h might change
183 k1 = pfo->pFunc(x , y );
186 k2 = pfo->pFunc(x + C[2]*h , y + as );
188 as = h*(A3[1]*k1 + A3[2]*k2);
189 k3 = pfo->pFunc(x + C[3]*h , y + as );
191 as = h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
192 k4 = pfo->pFunc(x + C[4]*h , y + as );
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 );
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 );
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);
204 abs_err = fabs(y4_val-y5_val);
206 // abs_err = h * (Ee[1] * k1 + Ee[3] * k3 + Ee[4] * k4 + Ee[5] * k5 + Ee[6] * k6);
208 // estimate step size
209 if (abs_err > epsilon) {
210 est_step = sqrt(sqrt(epsilon*h/abs_err));
212 est_step=2.0*h; // cheat
215 // check if a smaller step size is proposed
217 if (shrink_avail>0 && est_step<h) {
230 } // namespace JSBSim