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