[Jsbsim-cvslogs] CVS: JSBSim FGGauss2.cpp,NONE,1.1.2.1 FGGauss2.h,NONE,1.1.2.1 FGDefaultTimestep.cpp

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/jsbsim/JSBSim
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv29789

Modified Files:
      Tag: FROHLICH
	FGDefaultTimestep.cpp FGDefaultTimestep.h FGPropagate.cpp 
	Makefile.am 
Added Files:
      Tag: FROHLICH
	FGGauss2.cpp FGGauss2.h 
Log Message:
The third working horse. This one is the energy preserving timestepper.
It works that well that I use it for every 'not on ground' operation for now.

--- NEW FILE ---
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Module:       FGGauss2.cpp
 Author:       Mathias Froehlich
 Date started: 04/24/2004

 ----- Copyright (C) 2004 Mathias Froehlich (Mat...@we...) --------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU 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 General Public License for more
 details.

 You should have received a copy of the GNU 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 General Public License can also be found on
 the world wide web at http://www.gnu.org.

HISTORY
-------------------------------------------------------------------------------
04/24/2004   MF   Created

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGTimestep.h"
#include "FGGauss2.h"
#include "FGNewtonSolve.h"

#include "FGFDMExec.h"
#include "FGAuxiliary.h"
#include "FGVehicleState.h"

namespace JSBSim {

static const char *IdSrc = "$Id: FGGauss2.cpp,v 1.1.2.1 2004/04/27 20:32:04 frohlich Exp $";
static const char *IdHdr = ID_GAUSS2;

// The coefficients of the Runge-Kutta tableau.
// The exact values are just above the numerical values ...

// Runge kutta matrix...
// 1/4
const double FGGauss2::a11 = 0.25;
// 1/4 - sqrt(3)/6
const double FGGauss2::a12 = -0.0386751345948129;
// 1/4 - sqrt(3)/6
const double FGGauss2::a21 = 0.538675134594813;
// 1/4
const double FGGauss2::a22 = 0.25;

// The discrete times
// 1/2 - sqrt(3)/6
const double FGGauss2::c1 = 0.21132486540518711775;
// 1/2 + sqrt(3)/6
const double FGGauss2::c2 = 0.78867513459481288225;

// The weights
const double FGGauss2::b1 = 0.5;
const double FGGauss2::b2 = 0.5;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

FGGauss2::FGGauss2(FGFDMExec* fdmex)
  : FGTimestep(fdmex)
{
  mSteps = mFailed = 0;
  mCollocationPolynomialValid = false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGGauss2::~FGGauss2()
{
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double FGGauss2::ErrorNorm2(const FGVehicleState& ref, const FGVehicleState& diff) const
{
  const double loctol = 1e-4;
  const double ortol = 1e-4;
  const double velatol = 1e-4;
  const double velrtol = 1e-3;
  const double avelatol = 1e-5;
  const double avelrtol = 1e-4;

  double en = 0.0;
  en += FGTimestep::ErrorNorm2( loctol, ref.GetLocation(), diff.GetLocation() );
  en += FGTimestep::ErrorNorm2( ortol, diff.GetOrientation() );
  en += FGTimestep::ErrorNorm2( velatol, velrtol, ref.GetUVW(), diff.GetUVW() );
  en += FGTimestep::ErrorNorm2( avelatol, avelrtol, ref.GetPQR(), diff.GetPQR() );
  return en;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGGauss2::Integrate(Time tEnd, Time& t, FGVehicleState& state)
{
  // Do the actual timesteps.
  // Cycle as long as required to reach tEnd.
  while ( !ReachedTEnd( tEnd, t ) ) {

    // Cut the timestep if required.
    double h = ClipStepsizeToTEnd( tEnd, t, GetStepsize() );

    // Often used values.
    double c1h = c1*h;
    double c2h = c2*h;

    // We assume that the problem is nonstiff.
    // This is a sensible assumption since this symmetric integrator is not
    // stiffly accurate anyway.
    // If it is not stiff, we solve the nonlinear equation
    // with fixpoint iteration.
    // The GNI papers suggest this anyway.

    // An initial guess for the fixpoint iteration.
    // Note that the polynomial coefficients p1 and p2 are set to zero at the
    // first step past a reset.
    FGVehicleState y1 = state;
    FGVehicleState y2 = state;
    if (mCollocationPolynomialValid) {
      y1 += c1h*p1 + (c1h*c1h)*p2;
      y2 += c2h*p1 + (c2h*c2h)*p2;
    }

    // Solve the implicit equation
    bool converged = false;
    int maxit = 10;
    do {
      // Compute new approximations to the state derivatives
      FGVehicleState f1 = ComputeStateDeriv( t+c1h, y1 );
      FGVehicleState f2 = ComputeStateDeriv( t+c2h, y2 );

      // Compute new approximations to the state
      FGVehicleState y1new = state + (h*a11)*f1 + (h*a12)*f2;
      FGVehicleState y2new = state + (h*a21)*f1 + (h*a22)*f2;

      // Check if the increment is small enough ...
      double err = max( ErrorNorm2( y1, y1-y1new ),
                        ErrorNorm2( y2, y2-y2new ) );
      converged = err < 1.0;

      // Use the new approximation
      y1 = y1new;
      y2 = y2new;
    } while (!converged && 0 <= --maxit);

    if (converged) {
      // Compute new approximations to the state derivatives
      FGVehicleState f1 = ComputeStateDeriv( t+c1h, y1 );
      FGVehicleState f2 = ComputeStateDeriv( t+c2h, y2 );

      // Update the solution
      state += (h*b1)*f1 + (h*b2)*f2;

      // Compute the collocation polynomial.
      // Is used for a predictor of the next fixpoint iterate start guess.
      p2 = (0.5/(c1h-c2h))*(f1 - f2);
      p1 = f2 + (2.0*c1h)*p2;
      mCollocationPolynomialValid = true;

    } else {
      // If we cannot solve the nonlinear equation, do an explicit euler step
      ++mFailed;
      mCollocationPolynomialValid = false;

      state += h*ComputeStateDeriv( t, state );
    }

    // Increment the simulation time ...
    t += h;

    // Renormalize the quaternion and the location
    state.Normalize();

    ++mSteps;
  }

  return true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGGauss2::Reset()
{
  // Tell that we cannot use the collocation polynomial for a first
  // guess of the solution to the nonliner system.
  mCollocationPolynomialValid = false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

} // namespace JSBSim

--- NEW FILE ---
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGGauss2.h
 Author:       Mathias Froehlich
 Date started: 04/24/2004

 ----- Copyright (C) 2004 Mathias Froehlich (Mat...@we...) --------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU 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 General Public License for more
 details.

 You should have received a copy of the GNU 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 General Public License can also be found on
 the world wide web at http://www.gnu.org.

HISTORY
-------------------------------------------------------------------------------
04/24/2004   MF   Created

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGGAUSS2_H
#define FGGAUSS2_H

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGTimestep.h"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_GAUSS2 "$Id: FGGauss2.h,v 1.1.2.1 2004/04/27 20:32:04 frohlich Exp $"

namespace JSBSim {

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGFDMExec;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/** This class implements the 4th order 2 stage implicit Gauss
    Collocation method. It is energy preserving if it is run with
    constant stepsize. The method is not well suited for stiff
    problems as we have it on ground, but it works fawlessly well in
    air and in orbit.

    @see E. Hairer, S. P. Norsett and G. Wanner,
    "Solving Ordinary Differential Equations I", 2nd edition,
    Springer-Verlag, 1991
    @see E. Hairer and G. Wanner,
    "Solving Ordinary Differential Equations II", 2nd edition,
    Springer-Verlag, 1996
    @see E. Hairer, Ch. Lubich and G. Wanner,
    "Geometric Numerical Integration. Structure-Preserving Algorithms for
     Ordinary Differential Equations",
    Springer-Verlag, 2002
    @see E. Hairer and M. Hairer,
    "GniCodes - Matlab Programs for Geometric Numerical Integration"
    http://www.unige.ch/math/folks/hairer/preprints/gnicodes.html
    To appear in:
    Frontiers in Numerical Analysis (Durham 2002), Springer, Berlin, 2003
    @see L. F. Shampine, I. Gladwell and S. Thompson,
    "Solving ODEs with MATLAB"

    @author Mathias Froehlich
    @version $Id: FGGauss2.h,v 1.1.2.1 2004/04/27 20:32:04 frohlich Exp $
*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGGauss2 : virtual public FGTimestep {
public:
  FGGauss2(FGFDMExec* fdmex);
  virtual ~FGGauss2();

  /** Do timestepping.

      Perform the actual timesteps ...
  */
  virtual bool Integrate(Time tEnd, Time& t, FGVehicleState& state);

  /** Reset integration.
   */
  virtual bool Reset();

private:
  double ErrorNorm2(const FGVehicleState& ref, const FGVehicleState& diff) const;

  int mFailed;
  int mSteps;

  // Coefficients of the collocation polynomial.
  FGVehicleState p1, p2;
  bool mCollocationPolynomialValid;

  // Runge kutta matrix...
  static const double a11, a12, a21, a22;
  // The descrete times
  static const double c1, c2;
  // The weights
  static const double b1, b2;
};

} // namespace JSBSim

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#endif

Index: FGDefaultTimestep.cpp
===================================================================
RCS file: /cvsroot/jsbsim/JSBSim/Attic/FGDefaultTimestep.cpp,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -r1.1.2.2 -r1.1.2.3
*** FGDefaultTimestep.cpp	26 Apr 2004 10:48:54 -0000	1.1.2.2
--- FGDefaultTimestep.cpp	27 Apr 2004 20:32:04 -0000	1.1.2.3
***************
*** 33,37 ****

  #include "FGTimestep.h"
! #include "FGMcFarland.h"
  #include "FGImplicitEuler.h"
  #include "FGVehicleState.h"
--- 33,37 ----

  #include "FGTimestep.h"
! #include "FGGauss2.h"
  #include "FGImplicitEuler.h"
  #include "FGVehicleState.h"
***************
*** 49,53 ****

  FGDefaultTimestep::FGDefaultTimestep(FGFDMExec* fdmex)
!   : FGTimestep(fdmex), FGMcFarland(fdmex), FGImplicitEuler(fdmex)
  {
    mOnGroundCount = 0;
--- 49,53 ----

  FGDefaultTimestep::FGDefaultTimestep(FGFDMExec* fdmex)
!   : FGTimestep(fdmex), FGGauss2(fdmex), FGImplicitEuler(fdmex)
  {
    mOnGroundCount = 0;
***************
*** 84,88 ****
      if (mOnGroundCount) {
        PutMessage( "FGDefaultTimestep: Switching to \'in air\' timestepping method" );
!       FGMcFarland::Reset();
        mOnGroundCount = 0;
      }
--- 84,88 ----
      if (mOnGroundCount) {
        PutMessage( "FGDefaultTimestep: Switching to \'in air\' timestepping method" );
!       FGGauss2::Reset();
        mOnGroundCount = 0;
      }
***************
*** 90,94 ****

      // Do the real work.
!     return FGMcFarland::Integrate(tEnd, t, state);
    }
  }
--- 90,94 ----

      // Do the real work.
!     return FGGauss2::Integrate(tEnd, t, state);
    }
  }
***************
*** 99,103 ****
  {
    // Just to be shure that everything is reset, tell it both ...
!   bool ret = FGMcFarland::Reset();
    ret = ret && FGImplicitEuler::Reset();
    return ret;
--- 99,103 ----
  {
    // Just to be shure that everything is reset, tell it both ...
!   bool ret = FGGauss2::Reset();
    ret = ret && FGImplicitEuler::Reset();
    return ret;

Index: FGDefaultTimestep.h
===================================================================
RCS file: /cvsroot/jsbsim/JSBSim/Attic/FGDefaultTimestep.h,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -r1.1.2.1 -r1.1.2.2
*** FGDefaultTimestep.h	25 Apr 2004 12:56:01 -0000	1.1.2.1
--- FGDefaultTimestep.h	27 Apr 2004 20:32:04 -0000	1.1.2.2
***************
*** 39,43 ****
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  #include "FGTimestep.h"
! #include "FGMcFarland.h"
  #include "FGImplicitEuler.h"
  #include "FGVehicleState.h"
--- 39,43 ----
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  #include "FGTimestep.h"
! #include "FGGauss2.h"
  #include "FGImplicitEuler.h"
  #include "FGVehicleState.h"
***************
*** 87,91 ****
  CLASS DECLARATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
! class FGDefaultTimestep : public FGMcFarland, public FGImplicitEuler {
  public:
    /// Standard constructor.
--- 87,92 ----
  CLASS DECLARATION
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
! class FGDefaultTimestep
!   : public FGGauss2, public FGImplicitEuler {
  public:
    /// Standard constructor.

Index: FGPropagate.cpp
===================================================================
RCS file: /cvsroot/jsbsim/JSBSim/FGPropagate.cpp,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -r1.1.2.8 -r1.1.2.9
*** FGPropagate.cpp	27 Apr 2004 09:26:21 -0000	1.1.2.8
--- FGPropagate.cpp	27 Apr 2004 20:32:04 -0000	1.1.2.9
***************
*** 48,51 ****
--- 48,52 ----
  #include "FGMcFarland.h"
  #include "FGDopri5.h"
+ #include "FGGauss2.h"
  #include "FGDefaultTimestep.h"
  #include "FGPropertyManager.h"
***************
*** 72,75 ****
--- 73,77 ----
  //   mTimestep = new FGDopri5( FDMExec );
    mTimestep = new FGDefaultTimestep( FDMExec );
+ //   mTimestep = new FGGauss2( FDMExec );

    bind();

Index: Makefile.am
===================================================================
RCS file: /cvsroot/jsbsim/JSBSim/Makefile.am,v
retrieving revision 1.23.2.15
retrieving revision 1.23.2.16
diff -C2 -r1.23.2.15 -r1.23.2.16
*** Makefile.am	25 Apr 2004 12:56:02 -0000	1.23.2.15
--- Makefile.am	27 Apr 2004 20:32:04 -0000	1.23.2.16
***************
*** 64,67 ****
--- 64,69 ----
  	FGForce.cpp \
  	FGForce.h \
+ 	FGGauss2.cpp \
+ 	FGGauss2.h \
  	FGGroundReactions.cpp \
  	FGGroundReactions.h \

[Jsbsim-cvslogs] CVS: JSBSim FGGauss2.cpp,NONE,1.1.2.1 FGGauss2.h,NONE,1.1.2.1 FGDefaultTimestep.cpp

An open source flight dynamics & control software library in C++

[Jsbsim-cvslogs] CVS: JSBSim FGGauss2.cpp,NONE,1.1.2.1 FGGauss2.h,NONE,1.1.2.1 FGDefaultTimestep.cpp,1.1.2.2,1.1.2.3 FGDefaultTimestep.h,1.1.2.1,1.1.2.2 FGPropagate.cpp,1.1.2.8,1.1.2.9 Makefile.am,1.23.2.15,1.23.2.16