src/FDM/JSBSim/FGForce.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Source:       FGForce.cpp
   4  Author:       Tony Peden
   5  Date started: 6/10/00
   6
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26
  27  HISTORY
  28 --------------------------------------------------------------------------------
  29 6/10/00  TP   Created
  30
  31
  32 FUNCTIONAL DESCRIPTION
  33 --------------------------------------------------------------------------------
  34
  35 The purpose of this class is to provide storage for computed forces and
  36 encapsulate all the functionality associated with transforming those
  37 forces from their native coord system to the body system.  This includes
  38 computing the moments due to the difference between the point of application
  39 and the cg.
  40
  41 */
  42
  43 #include "FGFDMExec.h"
  44 #include "FGAircraft.h"
  45 #include "FGTranslation.h"
  46 #include "FGMatrix33.h"
  47 #include "FGColumnVector3.h"
  48 #include "FGColumnVector4.h"
  49 #include "FGDefs.h"
  50 #include "FGForce.h"
  51
  52 static const char *IdSrc = "$Id$";
  53 static const char *IdHdr = ID_FORCE;
  54
  55 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56
  57 FGForce::FGForce(FGFDMExec *FDMExec) :
  58     vFn(3),
  59     vMn(3),
  60     fdmex(FDMExec),
  61     vFb(3),
  62     vM(3),
  63     vXYZn(3),
  64     vDXYZ(3),
  65     mT(3,3),
  66     vH(3),
  67     vSense(3),
  68     ttype(tNone)
  69 {
  70   mT(1,1) = 1; //identity matrix
  71   mT(2,2) = 1;
  72   mT(3,3) = 1;
  73   vSense.InitMatrix(1);
  74   if (debug_lvl & 2) cout << "Instantiated: FGForce" << endl;
  75 }
  76
  77 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78
  79 FGForce::~FGForce()
  80 {
  81   if (debug_lvl & 2) cout << "Destroyed:    FGForce" << endl;
  82 }
  83
  84 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  85
  86 FGColumnVector3& FGForce::GetBodyForces(void) {
  87
  88   vFb = Transform()*(vFn.multElementWise(vSense));
  89
  90   //find the distance from this vector's location to the cg
  91   //needs to be done like this to convert from structural to body coords
  92   vDXYZ(1) = -(vXYZn(1) - fdmex->GetMassBalance()->GetXYZcg(1))*INCHTOFT;
  93   vDXYZ(2) =  (vXYZn(2) - fdmex->GetMassBalance()->GetXYZcg(2))*INCHTOFT;  //cg and rp values are in inches
  94   vDXYZ(3) = -(vXYZn(3) - fdmex->GetMassBalance()->GetXYZcg(3))*INCHTOFT;
  95
  96   // include rotational effects. vH will be set in descendent class such as
  97   // FGPropeller, and in most other cases will be zero.
  98   vM = vMn + vDXYZ*vFb + fdmex->GetRotation()->GetPQR()*vH;
  99
 100   return vFb;
 101 }
 102
 103 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 104
 105 FGMatrix33 FGForce::Transform(void) {
 106   switch(ttype) {
 107   case tWindBody:
 108     return fdmex->GetState()->GetTs2b(fdmex->GetTranslation()->Getalpha(),fdmex->GetTranslation()->Getbeta());
 109   case tLocalBody:
 110     return fdmex->GetState()->GetTl2b();
 111   case tCustom:
 112
 113   case tNone:
 114     return mT;
 115   default:
 116     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
 117     exit(1);
 118   }
 119 }
 120
 121 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 122
 123 void FGForce::SetAnglesToBody(float broll, float bpitch, float byaw) {
 124
 125   if(ttype == tCustom) {
 126     float cp,sp,cr,sr,cy,sy;
 127
 128     cp=cos(bpitch); sp=sin(bpitch);
 129     cr=cos(broll);  sr=sin(broll);
 130     cy=cos(byaw);   sy=sin(byaw);
 131
 132     mT(1,1)=cp*cy;
 133     mT(1,2)=cp*sy;
 134     mT(1,3)=-1*sp;
 135
 136     mT(2,1)=sr*sp*cy-cr*sy;
 137     mT(2,2)=sr*sp*sy+cr*cy;
 138     mT(2,3)=sr*cp;
 139
 140     mT(3,1)=cr*sp*cy+sr*sy;
 141     mT(3,2)=cr*sp*sy-sr*cy;
 142     mT(3,3)=cr*cp;
 143   }
 144
 145 }
 146
 147 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 148
 149 void FGForce::Debug(void)
 150 {
 151     //TODO: Add your source code here
 152 }
 153