1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
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
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
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.
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.
28 --------------------------------------------------------------------------------
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
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
47 #include "FGFDMExec.h"
48 #include "models/FGPropagate.h"
49 #include "models/FGMassBalance.h"
50 #include "models/FGAuxiliary.h"
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;
59 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
61 FGForce::FGForce(FGFDMExec *FDMExec) :
70 vActingXYZn.InitMatrix();
76 mT.InitMatrix(1., 0., 0.,
83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
92 const FGColumnVector3& FGForce::GetBodyForces(void)
94 vFb = Transform()*vFn;
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
100 vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
102 vM = vMn + vDXYZ*vFb;
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 const FGMatrix33& FGForce::Transform(void) const
113 return fdmex->GetAuxiliary()->GetTw2b();
115 return fdmex->GetPropagate()->GetTl2b();
120 cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
127 void FGForce::UpdateCustomTransformMatrix(void)
129 double cp,sp,cr,sr,cy,sy;
130 double srsp, crcy, crsy;
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));
144 mT(1,2) = srsp*cy - crsy;
145 mT(2,2) = srsp*sy + crcy;
148 mT(1,3) = crcy*sp + sr*sy;
149 mT(2,3) = crsy*sp - sr*cy;
153 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
155 void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
157 if (ttype == tCustom) {
158 vOrient(ePitch) = bpitch;
159 vOrient(eRoll) = broll;
160 vOrient(eYaw) = byaw;
162 UpdateCustomTransformMatrix();
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
174 // 1: This value explicity requests the normal JSBSim
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
185 void FGForce::Debug(int from)
187 if (debug_lvl <= 0) return;
189 if (debug_lvl & 1) { // Standard console startup message output
190 if (from == 0) { // Constructor
194 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
195 if (from == 0) cout << "Instantiated: FGForce" << endl;
196 if (from == 1) cout << "Destroyed: FGForce" << endl;
198 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
200 if (debug_lvl & 8 ) { // Runtime state variables
202 if (debug_lvl & 16) { // Sanity checking
204 if (debug_lvl & 64) {
205 if (from == 0) { // Constructor
206 cout << IdSrc << endl;
207 cout << IdHdr << endl;