From 7053b45c8c686bacb29f45f3286a01ca81caaab4 Mon Sep 17 00:00:00 2001
From: Erik Hofman <erik@ehofman.com>
Date: Fri, 16 Jul 2010 08:00:21 +0200
Subject: [PATCH] Sync with JSBSim CVS

---
 src/FDM/JSBSim/math/FGRungeKutta.cpp | 230 +++++++++++++++++++++++++++
 src/FDM/JSBSim/math/FGRungeKutta.h   | 189 ++++++++++++++++++++++
 2 files changed, 419 insertions(+)
 create mode 100644 src/FDM/JSBSim/math/FGRungeKutta.cpp
 create mode 100644 src/FDM/JSBSim/math/FGRungeKutta.h

diff --git a/src/FDM/JSBSim/math/FGRungeKutta.cpp b/src/FDM/JSBSim/math/FGRungeKutta.cpp
new file mode 100644
index 000000000..bc3dabc07
--- /dev/null
+++ b/src/FDM/JSBSim/math/FGRungeKutta.cpp
@@ -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
diff --git a/src/FDM/JSBSim/math/FGRungeKutta.h b/src/FDM/JSBSim/math/FGRungeKutta.h
new file mode 100644
index 000000000..692957725
--- /dev/null
+++ b/src/FDM/JSBSim/math/FGRungeKutta.h
@@ -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
-- 
2.39.5