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 "FGForce.h"
  50
  51 static const char *IdSrc = "$Id$";
  52 static const char *IdHdr = ID_FORCE;
  53
  54 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55
  56 FGForce::FGForce(FGFDMExec *FDMExec) :
  57                  ttype(tNone),
  58                  fdmex(FDMExec)
  59 {
  60   mT(1,1) = 1; //identity matrix
  61   mT(2,2) = 1;
  62   mT(3,3) = 1;
  63   vSense.InitMatrix(1);
  64   Debug(0);
  65 }
  66
  67 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  68
  69 FGForce::~FGForce()
  70 {
  71   Debug(1);
  72 }
  73
  74 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  75
  76 FGColumnVector3& FGForce::GetBodyForces(void)
  77 {
  78   vFb = Transform()*(vFn.multElementWise(vSense));
  79
  80   // Find the distance from this vector's acting location to the cg; this
  81   // needs to be done like this to convert from structural to body coords.
  82   // CG and RP values are in inches
  83
  84   vDXYZ(eX) = -(vActingXYZn(eX) - fdmex->GetMassBalance()->GetXYZcg(eX))*inchtoft;
  85   vDXYZ(eY) =  (vActingXYZn(eY) - fdmex->GetMassBalance()->GetXYZcg(eY))*inchtoft;
  86   vDXYZ(eZ) = -(vActingXYZn(eZ) - fdmex->GetMassBalance()->GetXYZcg(eZ))*inchtoft;
  87
  88   vM = vMn + vDXYZ*vFb;
  89
  90   return vFb;
  91 }
  92
  93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  94
  95 FGMatrix33 FGForce::Transform(void)
  96 {
  97   switch(ttype) {
  98   case tWindBody:
  99     return fdmex->GetState()->GetTs2b();
 100   case tLocalBody:
 101     return fdmex->GetState()->GetTl2b();
 102   case tCustom:
 103
 104   case tNone:
 105     return mT;
 106   default:
 107     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
 108     exit(1);
 109   }
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113
 114 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
 115 {
 116   if (ttype == tCustom) {
 117     double cp,sp,cr,sr,cy,sy;
 118
 119     cp=cos(bpitch); sp=sin(bpitch);
 120     cr=cos(broll);  sr=sin(broll);
 121     cy=cos(byaw);   sy=sin(byaw);
 122
 123     mT(1,1)=cp*cy;
 124     mT(1,2)=cp*sy;
 125     mT(1,3)=-1*sp;
 126
 127     mT(2,1)=sr*sp*cy-cr*sy;
 128     mT(2,2)=sr*sp*sy+cr*cy;
 129     mT(2,3)=sr*cp;
 130
 131     mT(3,1)=cr*sp*cy+sr*sy;
 132     mT(3,2)=cr*sp*sy-sr*cy;
 133     mT(3,3)=cr*cp;
 134   }
 135 }
 136
 137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 138 //    The bitmasked value choices are as follows:
 139 //    unset: In this case (the default) JSBSim would only print
 140 //       out the normally expected messages, essentially echoing
 141 //       the config files as they are read. If the environment
 142 //       variable is not set, debug_lvl is set to 1 internally
 143 //    0: This requests JSBSim not to output any messages
 144 //       whatsoever.
 145 //    1: This value explicity requests the normal JSBSim
 146 //       startup messages
 147 //    2: This value asks for a message to be printed out when
 148 //       a class is instantiated
 149 //    4: When this value is set, a message is displayed when a
 150 //       FGModel object executes its Run() method
 151 //    8: When this value is set, various runtime state variables
 152 //       are printed out periodically
 153 //    16: When set various parameters are sanity checked and
 154 //       a message is printed out when they go out of bounds
 155
 156 void FGForce::Debug(int from)
 157 {
 158   if (debug_lvl <= 0) return;
 159
 160   if (debug_lvl & 1) { // Standard console startup message output
 161     if (from == 0) { // Constructor
 162
 163     }
 164   }
 165   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 166     if (from == 0) cout << "Instantiated: FGForce" << endl;
 167     if (from == 1) cout << "Destroyed:    FGForce" << endl;
 168   }
 169   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 170   }
 171   if (debug_lvl & 8 ) { // Runtime state variables
 172   }
 173   if (debug_lvl & 16) { // Sanity checking
 174   }
 175   if (debug_lvl & 64) {
 176     if (from == 0) { // Constructor
 177       cout << IdSrc << endl;
 178       cout << IdHdr << endl;
 179     }
 180   }
 181 }
 182