FGMassBalance.h

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

 Header:       FGMassBalance.h
 Author:       Jon S. Berndt
 Date started: 09/12/2000

 ------------- Copyright (C) 2000  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
--------------------------------------------------------------------------------
09/12/2000  JSB  Created

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

#ifndef FGMASSBALANCE_H
#define FGMASSBALANCE_H

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

#include <vector>
#include <string>
#include "FGModel.h"
#include "math/FGColumnVector3.h"
#include "math/FGMatrix33.h"

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

#define ID_MASSBALANCE "$Id: FGMassBalance.h,v 1.38 2016/12/21 08:08:32 ehofman Exp $"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONSS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

namespace JSBSim {

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

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

class FGMassBalance : public FGModel
{

public:
  FGMassBalance(FGFDMExec*);
  ~FGMassBalance();

  bool Load(Element* el);
  bool InitModel(void);
  bool Run(bool Holding);

  double GetMass(void) const {return Mass;}
  double GetWeight(void) const {return Weight;}
  double GetEmptyWeight(void) const {return EmptyWeight;}
  const FGColumnVector3& GetXYZcg(void) const {return vXYZcg;}
  double GetXYZcg(int axis) const  {return vXYZcg(axis);}
  const FGColumnVector3& GetDeltaXYZcg(void) const {return vDeltaXYZcg;}
  double GetDeltaXYZcg(int axis) const  {return vDeltaXYZcg(axis);}

  FGMatrix33 GetPointmassInertia(double mass_sl, const FGColumnVector3& r) const
  {
    FGColumnVector3 v = StructuralToBody( r );
    FGColumnVector3 sv = mass_sl*v;
    double xx = sv(1)*v(1);
    double yy = sv(2)*v(2);
    double zz = sv(3)*v(3);
    double xy = -sv(1)*v(2);
    double xz = -sv(1)*v(3);
    double yz = -sv(2)*v(3);
    return FGMatrix33( yy+zz, xy, xz,
                       xy, xx+zz, yz,
                       xz, yz, xx+yy );
  }

  FGColumnVector3 StructuralToBody(const FGColumnVector3& r) const;

  void SetEmptyWeight(double EW) { EmptyWeight = EW;}
  void SetBaseCG(const FGColumnVector3& CG) {vbaseXYZcg = vXYZcg = CG;}

  void AddPointMass(Element* el);
  double GetTotalPointMassWeight(void) const;

  const FGColumnVector3& GetPointMassMoment(void);
  const FGMatrix33& GetJ(void) const {return mJ;}
  const FGMatrix33& GetJinv(void) const {return mJinv;}
  void SetAircraftBaseInertias(const FGMatrix33& BaseJ) {baseJ = BaseJ;}
  void GetMassPropertiesReport(int i);
  
  struct Inputs {
    double GasMass;
    double TanksWeight;
    FGColumnVector3 GasMoment;
    FGMatrix33 GasInertia;
    FGColumnVector3 TanksMoment;
    FGMatrix33 TankInertia;
  } in;

private:
  double Weight;
  double EmptyWeight;
  double Mass;
  FGMatrix33 mJ;
  FGMatrix33 mJinv;
  FGMatrix33 pmJ;
  FGMatrix33 baseJ;
  FGColumnVector3 vXYZcg;
  FGColumnVector3 vLastXYZcg;
  FGColumnVector3 vDeltaXYZcg;
  FGColumnVector3 vDeltaXYZcgBody;
  FGColumnVector3 vXYZtank;
  FGColumnVector3 vbaseXYZcg;
  FGColumnVector3 vPMxyz;
  FGColumnVector3 PointMassCG;
  const FGMatrix33& CalculatePMInertias(void);
  double GetIxx(void) const { return mJ(1,1); }
  double GetIyy(void) const { return mJ(2,2); }
  double GetIzz(void) const { return mJ(3,3); }
  double GetIxy(void) const { return -mJ(1,2); }
  double GetIxz(void) const { return -mJ(1,3); }
  double GetIyz(void) const { return -mJ(2,3); }

  struct PointMass {
    PointMass(double w, FGColumnVector3& vXYZ) {
      Weight = w;
      Location = vXYZ;
      mPMInertia.InitMatrix();
      Radius = 0.0;
      Length = 0.0;
    }

    void CalculateShapeInertia(void) {
      switch(eShapeType) {
        case esTube:
          mPMInertia(1,1) = (Weight/(slugtolb))*Radius*Radius; // mr^2
          mPMInertia(2,2) = (Weight/(slugtolb*12))*(6*Radius*Radius + Length*Length);
          mPMInertia(3,3) = mPMInertia(2,2);
          break;
        case esCylinder:
          mPMInertia(1,1) = (Weight/(slugtolb*2))*Radius*Radius; // 0.5*mr^2
          mPMInertia(2,2) = (Weight/(slugtolb*12))*(3*Radius*Radius + Length*Length);
          mPMInertia(3,3) = mPMInertia(2,2);
          break;
        case esSphere:
          mPMInertia(1,1) = (Weight/(slugtolb*3))*Radius*Radius*2; // (2mr^2)/3
          mPMInertia(2,2) = mPMInertia(1,1);
          mPMInertia(3,3) = mPMInertia(1,1);
          break;
        case esBall:
          mPMInertia(1,1) = (Weight/(slugtolb*5))*Radius*Radius*2; // (2mr^2)/5
          mPMInertia(2,2) = mPMInertia(1,1);
          mPMInertia(3,3) = mPMInertia(1,1);
          break;
        default:
          break;
      }
    }

    enum esShape {esUnspecified, esTube, esCylinder, esSphere, esBall} eShapeType;
    FGColumnVector3 Location;
    double Weight; 
    double Radius; 
    double Length; 
    std::string Name;
    FGMatrix33 mPMInertia;

    double GetPointMassLocation(int axis) const {return Location(axis);}
    double GetPointMassWeight(void) const {return Weight;}
    esShape GetShapeType(void) {return eShapeType;}
    const FGColumnVector3& GetLocation(void) {return Location;}
    const FGMatrix33& GetPointMassInertia(void) {return mPMInertia;}
    const std::string& GetName(void) {return Name;}

    void SetPointMassLocation(int axis, double value) {Location(axis) = value;}
    void SetPointMassWeight(double wt) {
      Weight = wt;
      CalculateShapeInertia();
    }
    void SetPointMassShapeType(esShape st) {eShapeType = st;}
    void SetRadius(double r) {Radius = r;}
    void SetLength(double l) {Length = l;}
    void SetName(const std::string& name) {Name = name;}
    void SetPointMassMoI(const FGMatrix33& MoI) { mPMInertia = MoI; }
    double GetPointMassMoI(int r, int c) {return mPMInertia(r,c);}

    void bind(FGPropertyManager* PropertyManager, unsigned int num);
  };

  std::vector <struct PointMass*> PointMasses;

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