fly/flightgear
1
0
Fork 0
Code Activity

Sync with JSBSim CVS

Browse source
This commit is contained in:
Erik Hofman 2010-07-16 08:00:21 +02:00
parent 93c6d15e19
commit 7053b45c8c
2 changed files with 419 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

230
src/FDM/JSBSim/math/FGRungeKutta.cpp Normal file
View file

@ -0,0 +1,230 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGRungeKutta.cpp
Author: Thomas Kreitler
Date started: 04/9/2010
------------- Copyright (C) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <cstdio>
#include <iostream>
#include <cmath>
#include "FGRungeKutta.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
using std::cout;
using std::endl;
namespace JSBSim {
static const char *IdSrc = "$Id: FGRungeKutta.cpp,v 1.1 2010/06/02 04:05:13 jberndt Exp $";
static const char *IdHdr = ID_RUNGEKUTTA;
const double FGRungeKutta::RealLimit = 1e30;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGRungeKutta::~FGRungeKutta() { };
int FGRungeKutta::init(double x_start, double x_end, int intervals)
{
x0 = x_start;
x1 = x_end;
h = (x_end - x_start)/intervals;
safer_x1 = x1 - h*1e-6; // avoid 'intervals*h < x1'
h05 = h*0.5;
err = 0.0;
if (x0>=x1) {
status &= eFaultyInit;
}
return status;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
Make sure that a numerical result is within +/-RealLimit.
This is a hapless try to be portable.
(There will be at least one architecture/compiler combination
where this will fail.)
*/
bool FGRungeKutta::sane_val(double x)
{
// assuming +/- inf behave as expected and 'nan' comparisons yield to false
if ( x < RealLimit && x > -RealLimit ) return true;
return false;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGRungeKutta::evolve(double y_0, FGRungeKuttaProblem *pf)
{
double x = x0;
double y = y_0;
pfo = pf;
iterations = 0;
if (!trace_values) {
while (x<safer_x1) {
y = approximate(x,y);
if (!sane_val(y)) { status &= eMathError; }
x += h;
iterations++;
}
} else {
while (x<safer_x1) {
cout << x << " " << y << endl;
y = approximate(x,y);
if (!sane_val(y)) { status &= eMathError; }
x += h;
iterations++;
}
cout << x << " " << y << endl;
}
x_end = x; // twimc, store the last x used.
return y;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGRK4::~FGRK4() { };
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double FGRK4::approximate(double x, double y)
{
double k1,k2,k3,k4;
k1 = pfo->pFunc(x , y );
k2 = pfo->pFunc(x + h05, y + h05*k1);
k3 = pfo->pFunc(x + h05, y + h05*k2);
k4 = pfo->pFunc(x + h , y + h *k3);
y += h/6.0 * ( k1 + 2.0*k2 + 2.0*k3 + k4 );
return y;
}
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// Butcher tableau
const double FGRKFehlberg::A2[] = { 0.0, 1.0/4.0 };
const double FGRKFehlberg::A3[] = { 0.0, 3.0/32.0, 9.0/32.0 };
const double FGRKFehlberg::A4[] = { 0.0, 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0 };
const double FGRKFehlberg::A5[] = { 0.0, 439.0/216.0, -8.0, 3680.0/513.0, -845.0/4104.0 };
const double FGRKFehlberg::A6[] = { 0.0, -8.0/27.0, 2.0, -3544.0/2565.0, 1859.0/4104.0, -11.0/40.0 };
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 };
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 };
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 };
// use this if truncation is an issue
// 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 };
FGRKFehlberg::~FGRKFehlberg() { };
double FGRKFehlberg::approximate(double x, double y)
{
double k1,k2,k3,k4,k5,k6, as;
double y4_val;
double y5_val;
double abs_err;
double est_step;
int done = 0;
while (!done) {
err = h*h*h*h*h; // h might change
k1 = pfo->pFunc(x , y );
as = h*A2[1]*k1;
k2 = pfo->pFunc(x + C[2]*h , y + as );
as = h*(A3[1]*k1 + A3[2]*k2);
k3 = pfo->pFunc(x + C[3]*h , y + as );
as = h*(A4[1]*k1 + A4[2]*k2 + A4[3]*k3);
k4 = pfo->pFunc(x + C[4]*h , y + as );
as = h*(A5[1]*k1 + A5[2]*k2 + A5[3]*k3 + A5[4]*k4);
k5 = pfo->pFunc(x + C[5]*h , y + as );
as = h*(A6[1]*k1 + A6[2]*k2 + A6[3]*k3 + A6[4]*k4 + A6[5]*k5);
k6 = pfo->pFunc(x + C[6]*h , y + as );
/* B[2]*k2 and Bs[2]*k2 are zero */
y5_val = y + h * ( B[1]*k1 + B[3]*k3 + B[4]*k4 + B[5]*k5 + B[6]*k6);
y4_val = y + h * (Bs[1]*k1 + Bs[3]*k3 + Bs[4]*k4 + Bs[5]*k5);
abs_err = fabs(y4_val-y5_val);
// same in green
// abs_err = h * (Ee[1] * k1 + Ee[3] * k3 + Ee[4] * k4 + Ee[5] * k5 + Ee[6] * k6);
// estimate step size
if (abs_err > epsilon) {
est_step = sqrt(sqrt(epsilon*h/abs_err));
} else {
est_step=2.0*h; // cheat
}
// check if a smaller step size is proposed
if (shrink_avail>0 && est_step<h) {
h/=2.0;
shrink_avail--;
} else {
done = 1;
}
}
return y4_val;
}
} // namespace JSBSim

189
src/FDM/JSBSim/math/FGRungeKutta.h Normal file
View file

@ -0,0 +1,189 @@
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGRungeKutta.h
Author: Thomas Kreitler
Date started: 04/9/2010
------------- Copyright (C) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#ifndef FGRUNGEKUTTA_H
#define FGRUNGEKUTTA_H
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
// #include "FGJSBBase.h" // later
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_RUNGEKUTTA "$Id: FGRungeKutta.h,v 1.1 2010/06/02 04:05:13 jberndt Exp $"
namespace JSBSim {
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Minimalistic implementation of some Runge-Kutta methods. Runge-Kutta methods
are a standard for solving ordinary differential equation (ODE) initial
value problems. The code follows closely the description given on
Wikipedia, see http://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods.
For more powerfull routines see GNU Scientific Library (GSL)
or GNU Plotutils 'ode'.
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRungeKuttaProblem
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Abstract base for the function to solve.
*/
class FGRungeKuttaProblem {
public:
virtual double pFunc(double x, double y) = 0;
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRungeKutta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Abstract base.
*/
class FGRungeKutta {
public:
enum eStates { eNoError=0, eMathError=1, eFaultyInit=2, eEvolve=4, eUnknown=8} ;
int init(double x_start, double x_end, int intervals = 4);
double evolve(double y_0, FGRungeKuttaProblem *pf);
double getXEnd() { return x_end; }
double getError() { return err; }
int getStatus() { return status; }
int getIterations() { return iterations; }
void clearStatus() { status = eNoError; }
void setTrace(bool t) { trace_values = t; }
protected:
// avoid accidents
FGRungeKutta(): status(eNoError), trace_values(false), iterations(0) {};
virtual ~FGRungeKutta();
FGRungeKuttaProblem *pfo;
double h;
double h05; // h*0.5, halfwidth
double err;
private:
virtual double approximate(double x, double y) = 0;
bool sane_val(double x);
static const double RealLimit;
double x0, x1;
double safer_x1;
double x_end;
int status;
bool trace_values;
int iterations;
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRK4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Classical RK4.
*/
class FGRK4 : public FGRungeKutta {
virtual ~FGRK4();
private:
double approximate(double x, double y);
};
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DECLARATION: FGRKFehlberg
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/**
Runge-Kutta-Fehlberg method.
This is a semi adaptive implementation of rkf - the interval only
shrinks. As a result interval calculations remain trivial, but
sometimes too many calculations are performed.
Rationale: this code is not meant to be a universal pain-reliever
for ode's. Rather it provides some safety if the number of
intervals is set too low, or the problem function behaves a bit
nasty in rare conditions.
*/
class FGRKFehlberg : public FGRungeKutta {
public:
FGRKFehlberg() : shrink_avail(4), epsilon(1e-12) { };
virtual ~FGRKFehlberg();
double getEpsilon() { return epsilon; }
int getShrinkAvail() { return shrink_avail; }
void setEpsilon(double e) { epsilon = e; }
void setShrinkAvail(int s) { shrink_avail = s; }
private:
double approximate(double x, double y);
int shrink_avail;
double epsilon;
static const double A2[], A3[], A4[], A5[], A6[];
static const double B[], Bs[], C[];
};
} // namespace JSBSim
#endif