JSBSim Flight Dynamics Model 1.2.2 (22 Mar 2025)
An Open Source Flight Dynamics and Control Software Library in C++
Loading...
Searching...
No Matches
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 "FGJSBBase.h"
37#include "FGRungeKutta.h"
38
39/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 DEFINITIONS
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
42
43using std::cout;
44using std::endl;
45
46namespace JSBSim {
47
48const double FGRungeKutta::RealLimit = 1e30;
49
50/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
51CLASS IMPLEMENTATION
52%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
53
54FGRungeKutta::~FGRungeKutta() { };
55
56int FGRungeKutta::init(double x_start, double x_end, int intervals)
57{
58 x0 = x_start;
59 x1 = x_end;
60 h = (x_end - x_start)/intervals;
61 safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
62 h05 = h*0.5;
63 err = 0.0;
64
65 if (x0>=x1) {
66 status &= eFaultyInit;
67 }
68 return status;
69}
70
71//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72
73/*
74 Make sure that a numerical result is within +/-RealLimit.
75 This is a hapless try to be portable.
76 (There will be at least one architecture/compiler combination
77 where this will fail.)
78*/
79
80bool FGRungeKutta::sane_val(double x)
81{
82 // assuming +/- inf behave as expected and 'nan' comparisons yield to false
83 if ( x < RealLimit && x > -RealLimit ) return true;
84 return false;
85}
86
87//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
88
89double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
90{
91 double x = x0;
92 double y = y_0;
93 pfo = pf;
94
95 iterations = 0;
96
97 if (!trace_values) {
98 while (x<safer_x1) {
99 y = approximate(x,y);
100 if (!sane_val(y)) { status &= eMathError; }
101 x += h;
102 iterations++;
103 }
104 } else {
105 while (x<safer_x1) {
106 cout << x << " " << y << endl;
107 y = approximate(x,y);
108 if (!sane_val(y)) { status &= eMathError; }
109 x += h;
110 iterations++;
111 }
112 cout << x << " " << y << endl;
113 }
114
115 x_end = x; // twimc, store the last x used.
116 return y;
117}
118
119
120
121/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122CLASS IMPLEMENTATION
123%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
124
125FGRK4::~FGRK4() { };
126
127//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128
129double FGRK4::approximate(double x, double y)
130{
131 double k1,k2,k3,k4;
132
133 k1 = pfo->pFunc(x , y );
134 k2 = pfo->pFunc(x + h05, y + h05*k1);
135 k3 = pfo->pFunc(x + h05, y + h05*k2);
136 k4 = pfo->pFunc(x + h , y + h *k3);
137
138 y += h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
139
140 return y;
141}
142
143
144/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145CLASS IMPLEMENTATION
146%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
147
148// Butcher tableau
149const double FGRKFehlberg::A2[] = { 0.0, 1.0/4.0 };
150const double FGRKFehlberg::A3[] = { 0.0, 3.0/32.0, 9.0/32.0 };
151const double FGRKFehlberg::A4[] = { 0.0, 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0 };
152const double FGRKFehlberg::A5[] = { 0.0, 439.0/216.0, -8.0, 3680.0/513.0, -845.0/4104.0 };
153const double FGRKFehlberg::A6[] = { 0.0, -8.0/27.0, 2.0, -3544.0/2565.0, 1859.0/4104.0, -11.0/40.0 };
154
155const 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 };
156
157const 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 };
158const 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 };
159
160// use this if truncation is an issue
161// 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 };
162
163FGRKFehlberg::~FGRKFehlberg() { };
164
165double FGRKFehlberg::approximate(double x, double y)
166{
167
168 double k1,k2,k3,k4,k5,k6, as;
169
170 double y4_val;
171 double y5_val;
172 double abs_err;
173 double est_step;
174 int done = 0;
175
176
177 while (!done) {
178
179 err = h*h*h*h*h; // h might change
180
181 k1 = pfo->pFunc(x , y );
182
183 as = h*A2[1]*k1;
184 k2 = pfo->pFunc(x + C[2]*h , y + as );
185
186 as = h*(A3[1]*k1 + A3[2]*k2);
187 k3 = pfo->pFunc(x + C[3]*h , y + as );
188
189 as = h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
190 k4 = pfo->pFunc(x + C[4]*h , y + as );
191
192 as = h*(A5[1]*k1 + A5[2]*k2 + A5[3]*k3 + A5[4]*k4);
193 k5 = pfo->pFunc(x + C[5]*h , y + as );
194
195 as = h*(A6[1]*k1 + A6[2]*k2 + A6[3]*k3 + A6[4]*k4 + A6[5]*k5);
196 k6 = pfo->pFunc(x + C[6]*h , y + as );
197
198 /* B[2]*k2 and Bs[2]*k2 are zero */
199 y5_val = y + h * ( B[1]*k1 + B[3]*k3 + B[4]*k4 + B[5]*k5 + B[6]*k6);
200 y4_val = y + h * (Bs[1]*k1 + Bs[3]*k3 + Bs[4]*k4 + Bs[5]*k5);
201
202 abs_err = fabs(y4_val-y5_val);
203 // same in green
204 // abs_err = h * (Ee[1] * k1 + Ee[3] * k3 + Ee[4] * k4 + Ee[5] * k5 + Ee[6] * k6);
205
206 // estimate step size
207 if (abs_err > epsilon) {
208 est_step = sqrt(sqrt(epsilon*h/abs_err));
209 } else {
210 est_step=2.0*h; // cheat
211 }
212
213 // check if a smaller step size is proposed
214
215 if (shrink_avail>0 && est_step<h) {
216 h/=2.0;
217 shrink_avail--;
218 } else {
219 done = 1;
220 }
221
222 }
223
224 return y4_val;
225}
226
227} // namespace JSBSim