src/FDM/JSBSim/models/propulsion/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 Lesser 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 Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser 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 Lesser 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 <iostream>
  44 #include <cstdlib>
  45
  46 #include "FGForce.h"
  47 #include "FGFDMExec.h"
  48 #include "models/FGPropagate.h"
  49 #include "models/FGMassBalance.h"
  50 #include "models/FGAuxiliary.h"
  51
  52 using namespace std;
  53
  54 namespace JSBSim {
  55
  56 static const char *IdSrc = "$Id: FGForce.cpp,v 1.17 2011/10/31 14:54:41 bcoconni Exp $";
  57 static const char *IdHdr = ID_FORCE;
  58
  59 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  60
  61 FGForce::FGForce(FGFDMExec *FDMExec) :
  62                  fdmex(FDMExec),
  63                  ttype(tNone)
  64 {
  65   vFn.InitMatrix();
  66   vMn.InitMatrix();
  67   vH.InitMatrix();
  68   vOrient.InitMatrix();
  69   vXYZn.InitMatrix();
  70   vActingXYZn.InitMatrix();
  71
  72   vFb.InitMatrix();
  73   vM.InitMatrix();
  74   vDXYZ.InitMatrix();
  75
  76   mT.InitMatrix(1., 0., 0.,
  77                 0., 1., 0.,
  78                 0., 0., 1.);
  79
  80   Debug(0);
  81 }
  82
  83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  84
  85 FGForce::~FGForce()
  86 {
  87   Debug(1);
  88 }
  89
  90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  91
  92 const FGColumnVector3& FGForce::GetBodyForces(void)
  93 {
  94   vFb = Transform()*vFn;
  95
  96   // Find the distance from this vector's acting location to the cg; this
  97   // needs to be done like this to convert from structural to body coords.
  98   // CG and RP values are in inches
  99
 100   vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
 101
 102   vM = vMn + vDXYZ*vFb;
 103
 104   return vFb;
 105 }
 106
 107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 108
 109 const FGMatrix33& FGForce::Transform(void) const
 110 {
 111   switch(ttype) {
 112   case tWindBody:
 113     return fdmex->GetAuxiliary()->GetTw2b();
 114   case tLocalBody:
 115     return fdmex->GetPropagate()->GetTl2b();
 116   case tCustom:
 117   case tNone:
 118     return mT;
 119   default:
 120     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
 121     exit(1);
 122   }
 123 }
 124
 125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 126
 127 void FGForce::UpdateCustomTransformMatrix(void)
 128 {
 129   double cp,sp,cr,sr,cy,sy;
 130   double srsp, crcy, crsy;
 131
 132   cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 133   cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 134   cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 135
 136   srsp = sr*sp;
 137   crcy = cr*cy;
 138   crsy = cr*sy;
 139
 140   mT(1,1) =  cp*cy;
 141   mT(2,1) =  cp*sy;
 142   mT(3,1) = -sp;
 143
 144   mT(1,2) = srsp*cy - crsy;
 145   mT(2,2) = srsp*sy + crcy;
 146   mT(3,2) = sr*cp;
 147
 148   mT(1,3) = crcy*sp + sr*sy;
 149   mT(2,3) = crsy*sp - sr*cy;
 150   mT(3,3) = cr*cp;
 151 }
 152
 153 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 154
 155 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
 156 {
 157   if (ttype == tCustom) {
 158     vOrient(ePitch) = bpitch;
 159     vOrient(eRoll) = broll;
 160     vOrient(eYaw) = byaw;
 161
 162     UpdateCustomTransformMatrix();
 163   }
 164 }
 165
 166 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 167 //    The bitmasked value choices are as follows:
 168 //    unset: In this case (the default) JSBSim would only print
 169 //       out the normally expected messages, essentially echoing
 170 //       the config files as they are read. If the environment
 171 //       variable is not set, debug_lvl is set to 1 internally
 172 //    0: This requests JSBSim not to output any messages
 173 //       whatsoever.
 174 //    1: This value explicity requests the normal JSBSim
 175 //       startup messages
 176 //    2: This value asks for a message to be printed out when
 177 //       a class is instantiated
 178 //    4: When this value is set, a message is displayed when a
 179 //       FGModel object executes its Run() method
 180 //    8: When this value is set, various runtime state variables
 181 //       are printed out periodically
 182 //    16: When set various parameters are sanity checked and
 183 //       a message is printed out when they go out of bounds
 184
 185 void FGForce::Debug(int from)
 186 {
 187   if (debug_lvl <= 0) return;
 188
 189   if (debug_lvl & 1) { // Standard console startup message output
 190     if (from == 0) { // Constructor
 191
 192     }
 193   }
 194   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 195     if (from == 0) cout << "Instantiated: FGForce" << endl;
 196     if (from == 1) cout << "Destroyed:    FGForce" << endl;
 197   }
 198   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 199   }
 200   if (debug_lvl & 8 ) { // Runtime state variables
 201   }
 202   if (debug_lvl & 16) { // Sanity checking
 203   }
 204   if (debug_lvl & 64) {
 205     if (from == 0) { // Constructor
 206       cout << IdSrc << endl;
 207       cout << IdHdr << endl;
 208     }
 209   }
 210 }
 211 }