FGAccelerations.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGAccelerations.h
 Author:       Jon S. Berndt
 Date started: 07/12/11

 ------------- Copyright (C) 2011  Jon S. Berndt (jon@jsbsim.org) -------------

 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
--------------------------------------------------------------------------------
07/12/11   JSB   Created

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

#ifndef FGACCELERATIONS_H
#define FGACCELERATIONS_H

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

#include <vector>

#include "models/FGModel.h"
#include "math/FGColumnVector3.h"
#include "math/LagrangeMultiplier.h"
#include "math/FGMatrix33.h"

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

#define ID_ACCELERATIONS "$Id: FGAccelerations.h,v 1.20 2016/05/22 10:28:23 bcoconni Exp $"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

namespace JSBSim {

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

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

class FGAccelerations : public FGModel {
public:
  FGAccelerations(FGFDMExec* Executive);

  ~FGAccelerations();

  enum eGravType {
    gtStandard,
    gtWGS84
  };

  bool InitModel(void);

  bool Run(bool Holding);

  const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }

  const FGColumnVector3& GetUVWidot(void) const { return vUVWidot; }

  const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}

  const FGColumnVector3& GetPQRidot(void) const {return vPQRidot;}

  double GetUVWdot(int idx) const { return vUVWdot(idx); }

  const FGColumnVector3& GetBodyAccel(void) const { return vBodyAccel; }

  const FGColumnVector3& GetGravAccel(void) const {return vGravAccel; }

  double GetGravAccelMagnitude(void) const { return vGravAccel.Magnitude(); }

  double GetBodyAccel(int idx) const { return vBodyAccel(idx); }

  double GetPQRdot(int axis) const {return vPQRdot(axis);}

  double GetMoments(int idx) const { return in.Moment(idx) + vFrictionMoments(idx); }
  FGColumnVector3 GetMoments(void) const { return in.Moment + vFrictionMoments; }

  double GetForces(int idx) const { return in.Force(idx) + vFrictionForces(idx); }
  FGColumnVector3 GetForces(void) const { return in.Force + vFrictionForces; }

  double GetGroundMoments(int idx) const { return in.GroundMoment(idx) + vFrictionMoments(idx); }
  FGColumnVector3 GetGroundMoments(void) const { return in.GroundMoment + vFrictionMoments; }

  double GetGroundForces(int idx) const { return in.GroundForce(idx) + vFrictionForces(idx); }
  FGColumnVector3 GetGroundForces(void) const { return in.GroundForce + vFrictionForces; }

  double GetWeight(int idx) const { return in.Mass * (in.Ti2b * vGravAccel)(idx); }
  FGColumnVector3 GetWeight(void) const { return in.Mass * in.Ti2b * vGravAccel; }

  void InitializeDerivatives(void);

  void SetHoldDown(bool hd);

  struct Inputs {
    FGMatrix33 J;
    FGMatrix33 Jinv;
    FGMatrix33 Ti2b;
    FGMatrix33 Tb2i;
    FGMatrix33 Tec2b;
    FGMatrix33 Tec2i;
    FGColumnVector3 Moment;
    FGColumnVector3 GroundMoment;
    FGColumnVector3 Force;
    FGColumnVector3 GroundForce;
    FGColumnVector3 J2Grav;
    FGColumnVector3 vPQRi;
    FGColumnVector3 vPQR;
    FGColumnVector3 vUVW;
    FGColumnVector3 vInertialPosition;
    FGColumnVector3 vOmegaPlanet;
    FGColumnVector3 TerrainVelocity;
    FGColumnVector3 TerrainAngularVel;
    double DeltaT;
    double Mass;
    double GAccel;
    std::vector<LagrangeMultiplier*> *MultipliersList;
  } in;

private:

  FGColumnVector3 vPQRdot, vPQRidot;
  FGColumnVector3 vUVWdot, vUVWidot;
  FGColumnVector3 vBodyAccel;
  FGColumnVector3 vGravAccel;
  FGColumnVector3 vFrictionForces;
  FGColumnVector3 vFrictionMoments;

  int gravType;
  bool gravTorque;

  void CalculatePQRdot(void);
  void CalculateUVWdot(void);

  void ResolveFrictionForces(double dt);

  void bind(void);
  void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif