src/FDM/JSBSim/FGForce.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
   2 \r
   3  Source:       FGForce.cpp\r
   4  Author:       Tony Peden\r
   5  Date started: 6/10/00\r
   6 \r
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------\r
   8 \r
   9  This program is free software; you can redistribute it and/or modify it under\r
  10  the terms of the GNU General Public License as published by the Free Software\r
  11  Foundation; either version 2 of the License, or (at your option) any later\r
  12  version.\r
  13 \r
  14  This program is distributed in the hope that it will be useful, but WITHOUT\r
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS\r
  16  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more\r
  17  details.\r
  18 \r
  19  You should have received a copy of the GNU General Public License along with\r
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple\r
  21  Place - Suite 330, Boston, MA  02111-1307, USA.\r
  22 \r
  23  Further information about the GNU General Public License can also be found on\r
  24  the world wide web at http://www.gnu.org.\r
  25 \r
  26 \r
  27  HISTORY\r
  28 --------------------------------------------------------------------------------\r
  29 6/10/00  TP   Created\r
  30 \r
  31 \r
  32 FUNCTIONAL DESCRIPTION\r
  33 --------------------------------------------------------------------------------\r
  34 \r
  35 The purpose of this class is to provide storage for computed forces and\r
  36 encapsulate all the functionality associated with transforming those\r
  37 forces from their native coord system to the body system.  This includes\r
  38 computing the moments due to the difference between the point of application\r
  39 and the cg.\r
  40 \r
  41 */\r
  42 \r
  43 #include "FGFDMExec.h"\r
  44 #include "FGAircraft.h"\r
  45 #include "FGTranslation.h"\r
  46 #include "FGMassBalance.h"\r
  47 #include "FGState.h"\r
  48 #include "FGForce.h"\r
  49 \r
  50 namespace JSBSim {\r
  51 \r
  52 static const char *IdSrc = "$Id$";\r
  53 static const char *IdHdr = ID_FORCE;\r
  54 \r
  55 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  56 \r
  57 FGForce::FGForce(FGFDMExec *FDMExec) :\r
  58                  ttype(tNone),\r
  59                  fdmex(FDMExec)\r
  60 {\r
  61   mT(1,1) = 1; //identity matrix\r
  62   mT(2,2) = 1;\r
  63   mT(3,3) = 1;\r
  64   vSense.InitMatrix(1);\r
  65   Debug(0);\r
  66 }\r
  67 \r
  68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  69 \r
  70 FGForce::~FGForce()\r
  71 {\r
  72   Debug(1);\r
  73 }\r
  74 \r
  75 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  76 \r
  77 FGColumnVector3& FGForce::GetBodyForces(void)\r
  78 {\r
  79   vFb = Transform()*(vFn.multElementWise(vSense));\r
  80 \r
  81   // Find the distance from this vector's acting location to the cg; this\r
  82   // needs to be done like this to convert from structural to body coords.\r
  83   // CG and RP values are in inches\r
  84 \r
  85   vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);\r
  86 \r
  87   vM = vMn + vDXYZ*vFb;\r
  88 \r
  89   return vFb;\r
  90 }\r
  91 \r
  92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
  93 \r
  94 FGMatrix33 FGForce::Transform(void)\r
  95 {\r
  96   switch(ttype) {\r
  97   case tWindBody:\r
  98     return fdmex->GetState()->GetTs2b();\r
  99   case tLocalBody:\r
 100     return fdmex->GetState()->GetTl2b();\r
 101   case tCustom:\r
 102   case tNone:\r
 103     return mT;\r
 104   default:\r
 105     cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;\r
 106     exit(1);\r
 107   }\r
 108 }\r
 109 \r
 110 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 111 \r
 112 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)\r
 113 {\r
 114   if (ttype == tCustom) {\r
 115     double cp,sp,cr,sr,cy,sy;\r
 116 \r
 117     cp=cos(bpitch); sp=sin(bpitch);\r
 118     cr=cos(broll);  sr=sin(broll);\r
 119     cy=cos(byaw);   sy=sin(byaw);\r
 120 \r
 121     mT(1,1)=cp*cy;\r
 122     mT(1,2)=cp*sy;\r
 123     mT(1,3)=-1*sp;\r
 124 \r
 125     mT(2,1)=sr*sp*cy-cr*sy;\r
 126     mT(2,2)=sr*sp*sy+cr*cy;\r
 127     mT(2,3)=sr*cp;\r
 128 \r
 129     mT(3,1)=cr*sp*cy+sr*sy;\r
 130     mT(3,2)=cr*sp*sy-sr*cy;\r
 131     mT(3,3)=cr*cp;\r
 132   }\r
 133 }\r
 134 \r
 135 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
 136 //    The bitmasked value choices are as follows:\r
 137 //    unset: In this case (the default) JSBSim would only print\r
 138 //       out the normally expected messages, essentially echoing\r
 139 //       the config files as they are read. If the environment\r
 140 //       variable is not set, debug_lvl is set to 1 internally\r
 141 //    0: This requests JSBSim not to output any messages\r
 142 //       whatsoever.\r
 143 //    1: This value explicity requests the normal JSBSim\r
 144 //       startup messages\r
 145 //    2: This value asks for a message to be printed out when\r
 146 //       a class is instantiated\r
 147 //    4: When this value is set, a message is displayed when a\r
 148 //       FGModel object executes its Run() method\r
 149 //    8: When this value is set, various runtime state variables\r
 150 //       are printed out periodically\r
 151 //    16: When set various parameters are sanity checked and\r
 152 //       a message is printed out when they go out of bounds\r
 153 \r
 154 void FGForce::Debug(int from)\r
 155 {\r
 156   if (debug_lvl <= 0) return;\r
 157 \r
 158   if (debug_lvl & 1) { // Standard console startup message output\r
 159     if (from == 0) { // Constructor\r
 160 \r
 161     }\r
 162   }\r
 163   if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
 164     if (from == 0) cout << "Instantiated: FGForce" << endl;\r
 165     if (from == 1) cout << "Destroyed:    FGForce" << endl;\r
 166   }\r
 167   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
 168   }\r
 169   if (debug_lvl & 8 ) { // Runtime state variables\r
 170   }\r
 171   if (debug_lvl & 16) { // Sanity checking\r
 172   }\r
 173   if (debug_lvl & 64) {\r
 174     if (from == 0) { // Constructor\r
 175       cout << IdSrc << endl;\r
 176       cout << IdHdr << endl;\r
 177     }\r
 178   }\r
 179 }\r
 180 }\r