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 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 General Public License for more
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.
23 Further information about the GNU 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
43 #include "FGFDMExec.h"
44 #include "FGAircraft.h"
45 #include "FGTranslation.h"
50 static const char *IdSrc = "$Id$";
51 static const char *IdHdr = "ID_FORCE";
53 extern short debug_lvl;
55 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
57 FGForce::FGForce(FGFDMExec *FDMExec) :
69 mT(1,1)=1; //identity matrix
73 if (debug_lvl & 2) cout << "Instantiated: FGForce" << endl;
76 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80 if (debug_lvl & 2) cout << "Destroyed: FGForce" << endl;
83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
85 FGColumnVector FGForce::GetBodyForces(void) {
87 vFb=Transform()*(vFn.multElementWise(vSense));
89 //find the distance from this vector's location to the cg
90 //needs to be done like this to convert from structural to body coords
91 vDXYZ(1) = -(vXYZn(1) - fdmex->GetAircraft()->GetXYZcg()(1))*INCHTOFT;
92 vDXYZ(2) = (vXYZn(2) - fdmex->GetAircraft()->GetXYZcg()(2))*INCHTOFT; //cg and rp values are in inches
93 vDXYZ(3) = -(vXYZn(3) - fdmex->GetAircraft()->GetXYZcg()(3))*INCHTOFT;
100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102 FGMatrix FGForce::Transform(void) {
105 return fdmex->GetState()->GetTs2b(fdmex->GetTranslation()->Getalpha(),fdmex->GetTranslation()->Getbeta());
107 return fdmex->GetState()->GetTl2b();
113 cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
118 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
120 void FGForce::SetAnglesToBody(float broll, float bpitch, float byaw) {
122 if(ttype == tCustom) {
123 float cp,sp,cr,sr,cy,sy;
125 cp=cos(bpitch); sp=sin(bpitch);
126 cr=cos(broll); sr=sin(broll);
127 cy=cos(byaw); sy=sin(byaw);
133 mT(2,1)=sr*sp*cy-cr*sy;
134 mT(2,2)=sr*sp*sy+cr*cy;
137 mT(3,1)=cr*sp*cy+sr*sy;
138 mT(3,2)=cr*sp*sy-sr*cy;
144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
146 void FGForce::Debug(void)
148 //TODO: Add your source code here