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