Branch data Line data Source code
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 [ # # ][ # # ]: 0 : FGRungeKutta::~FGRungeKutta() { };
[ # # ]
57 : :
58 : 0 : int FGRungeKutta::init(double x_start, double x_end, int intervals)
59 : : {
60 : 0 : x0 = x_start;
61 : 0 : x1 = x_end;
62 : 0 : h = (x_end - x_start)/intervals;
63 : 0 : safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
64 : 0 : h05 = h*0.5;
65 : 0 : err = 0.0;
66 : :
67 [ # # ]: 0 : if (x0>=x1) {
68 : 0 : status &= eFaultyInit;
69 : : }
70 : 0 : 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 : 0 : bool FGRungeKutta::sane_val(double x)
83 : : {
84 : : // assuming +/- inf behave as expected and 'nan' comparisons yield to false
85 [ # # ][ # # ]: 0 : if ( x < RealLimit && x > -RealLimit ) return true;
[ # # ][ # # ]
[ # # ][ # # ]
86 : 0 : return false;
87 : : }
88 : :
89 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 : :
91 : 0 : double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
92 : : {
93 : 0 : double x = x0;
94 : 0 : double y = y_0;
95 : 0 : pfo = pf;
96 : :
97 : 0 : iterations = 0;
98 : :
99 [ # # ]: 0 : if (!trace_values) {
100 [ # # ]: 0 : while (x<safer_x1) {
101 : 0 : y = approximate(x,y);
102 [ # # ]: 0 : if (!sane_val(y)) { status &= eMathError; }
103 : 0 : x += h;
104 : 0 : iterations++;
105 : : }
106 : : } else {
107 [ # # ]: 0 : while (x<safer_x1) {
108 : 0 : cout << x << " " << y << endl;
109 : 0 : y = approximate(x,y);
110 [ # # ]: 0 : if (!sane_val(y)) { status &= eMathError; }
111 : 0 : x += h;
112 : 0 : iterations++;
113 : : }
114 : 0 : cout << x << " " << y << endl;
115 : : }
116 : :
117 : 0 : x_end = x; // twimc, store the last x used.
118 : 0 : return y;
119 : : }
120 : :
121 : :
122 : :
123 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
124 : : CLASS IMPLEMENTATION
125 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
126 : :
127 [ # # ][ # # ]: 0 : FGRK4::~FGRK4() { };
[ # # ]
128 : :
129 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130 : :
131 : 0 : double FGRK4::approximate(double x, double y)
132 : : {
133 : : double k1,k2,k3,k4;
134 : :
135 : 0 : k1 = pfo->pFunc(x , y );
136 : 0 : k2 = pfo->pFunc(x + h05, y + h05*k1);
137 : 0 : k3 = pfo->pFunc(x + h05, y + h05*k2);
138 : 0 : k4 = pfo->pFunc(x + h , y + h *k3);
139 : :
140 : 0 : y += h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
141 : :
142 : 0 : 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 [ # # ][ # # ]: 0 : FGRKFehlberg::~FGRKFehlberg() { };
[ # # ]
166 : :
167 : 0 : 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 : 0 : int done = 0;
177 : :
178 : :
179 [ # # ]: 0 : while (!done) {
180 : :
181 : 0 : err = h*h*h*h*h; // h might change
182 : :
183 : 0 : k1 = pfo->pFunc(x , y );
184 : :
185 : 0 : as = h*A2[1]*k1;
186 : 0 : k2 = pfo->pFunc(x + C[2]*h , y + as );
187 : :
188 : 0 : as = h*(A3[1]*k1 + A3[2]*k2);
189 : 0 : k3 = pfo->pFunc(x + C[3]*h , y + as );
190 : :
191 : 0 : as = h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
192 : 0 : k4 = pfo->pFunc(x + C[4]*h , y + as );
193 : :
194 : 0 : as = h*(A5[1]*k1 + A5[2]*k2 + A5[3]*k3 + A5[4]*k4);
195 : 0 : k5 = pfo->pFunc(x + C[5]*h , y + as );
196 : :
197 : 0 : as = h*(A6[1]*k1 + A6[2]*k2 + A6[3]*k3 + A6[4]*k4 + A6[5]*k5);
198 : 0 : k6 = pfo->pFunc(x + C[6]*h , y + as );
199 : :
200 : : /* B[2]*k2 and Bs[2]*k2 are zero */
201 : 0 : y5_val = y + h * ( B[1]*k1 + B[3]*k3 + B[4]*k4 + B[5]*k5 + B[6]*k6);
202 : 0 : y4_val = y + h * (Bs[1]*k1 + Bs[3]*k3 + Bs[4]*k4 + Bs[5]*k5);
203 : :
204 : 0 : 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 [ # # ]: 0 : if (abs_err > epsilon) {
210 : 0 : est_step = sqrt(sqrt(epsilon*h/abs_err));
211 : : } else {
212 : 0 : est_step=2.0*h; // cheat
213 : : }
214 : :
215 : : // check if a smaller step size is proposed
216 : :
217 [ # # ][ # # ]: 0 : if (shrink_avail>0 && est_step<h) {
218 : 0 : h/=2.0;
219 : 0 : shrink_avail--;
220 : : } else {
221 : 0 : done = 1;
222 : : }
223 : :
224 : : }
225 : :
226 : 0 : return y4_val;
227 : : }
228 : :
229 : :
230 [ + + ][ + - ]: 12 : } // namespace JSBSim
|
