--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ 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
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ 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
projects / flightgear.git / commitdiff