------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
This program is free software; you can redistribute it and/or modify it under
- the terms of the GNU General Public License as published by the Free Software
+ the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
- FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
- You should have received a copy of the GNU General Public License along with
+ You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
- Further information about the GNU General Public License can also be found on
+ Further information about the GNU Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
*/
-#include <FGFDMExec.h>
-#include <models/FGAircraft.h>
-#include <models/FGPropagate.h>
-#include <models/FGMassBalance.h>
-#include <FGState.h>
#include "FGForce.h"
+#include "FGFDMExec.h"
+#include "models/FGAircraft.h"
+#include "models/FGPropagate.h"
+#include "models/FGMassBalance.h"
+#include "models/FGAerodynamics.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
namespace JSBSim {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGForce::FGForce(FGFDMExec *FDMExec) :
- ttype(tNone),
- fdmex(FDMExec)
+ fdmex(FDMExec),
+ ttype(tNone)
{
mT(1,1) = 1; //identity matrix
mT(2,2) = 1;
mT(3,3) = 1;
- vSense.InitMatrix(1);
+
Debug(0);
}
FGColumnVector3& FGForce::GetBodyForces(void)
{
- vFb = Transform()*(vFn.multElementWise(vSense));
+ vFb = Transform()*vFn;
// Find the distance from this vector's acting location to the cg; this
// needs to be done like this to convert from structural to body coords.
{
switch(ttype) {
case tWindBody:
- return fdmex->GetState()->GetTs2b();
+ return fdmex->GetAerodynamics()->GetTw2b();
case tLocalBody:
return fdmex->GetPropagate()->GetTl2b();
case tCustom:
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGForce::UpdateCustomTransformMatrix(void)
+{
+ double cp,sp,cr,sr,cy,sy;
+ double srsp, crcy, crsy;
+
+ cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
+ cr=cos(vOrient(eRoll)); sr=sin(vOrient(eRoll));
+ cy=cos(vOrient(eYaw)); sy=sin(vOrient(eYaw));
+
+ srsp = sr*sp;
+ crcy = cr*cy;
+ crsy = cr*sy;
+
+ mT(1,1) = cp*cy;
+ mT(2,1) = cp*sy;
+ mT(3,1) = -sp;
+
+ mT(1,2) = srsp*cy - crsy;
+ mT(2,2) = srsp*sy + crcy;
+ mT(3,2) = sr*cp;
+
+ mT(1,3) = crcy*sp + sr*sy;
+ mT(2,3) = crsy*sp - sr*cy;
+ mT(3,3) = cr*cp;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
{
if (ttype == tCustom) {
- double cp,sp,cr,sr,cy,sy;
vOrient(ePitch) = bpitch;
vOrient(eRoll) = broll;
vOrient(eYaw) = byaw;
- cp=cos(bpitch); sp=sin(bpitch);
- cr=cos(broll); sr=sin(broll);
- cy=cos(byaw); sy=sin(byaw);
-
- mT(1,1)=cp*cy;
- mT(1,2)=cp*sy;
- mT(1,3)=-1*sp;
-
- mT(2,1)=sr*sp*cy-cr*sy;
- mT(2,2)=sr*sp*sy+cr*cy;
- mT(2,3)=sr*cp;
-
- mT(3,1)=cr*sp*cy+sr*sy;
- mT(3,2)=cr*sp*sy-sr*cy;
- mT(3,3)=cr*cp;
+ UpdateCustomTransformMatrix();
}
}