]> git.mxchange.org Git - flightgear.git/blobdiff - src/FDM/JSBSim/models/propulsion/FGForce.cpp
Merge branch 'next' into durk-atc
[flightgear.git] / src / FDM / JSBSim / models / propulsion / FGForce.cpp
index fec3686d9bd8db7f44dba2515661b69218c01eae..0bebb1772ab3579e56d902faef47c10e89b2d6ef 100644 (file)
@@ -40,27 +40,42 @@ and the cg.
 
 */
 
-#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 {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGForce.cpp,v 1.15 2011/02/17 00:20:52 jberndt Exp $";
 static const char *IdHdr = ID_FORCE;
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 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;
+  vFn.InitMatrix();
+  vMn.InitMatrix();
+  vH.InitMatrix();
+  vOrient.InitMatrix();
+  vXYZn.InitMatrix();
+  vActingXYZn.InitMatrix();
+
+  vFb.InitMatrix();
+  vM.InitMatrix();
+  vDXYZ.InitMatrix();
+
+  mT.InitMatrix(1., 0., 0.,
+                0., 1., 0.,
+                0., 0., 1.);
 
   Debug(0);
 }
@@ -95,7 +110,7 @@ FGMatrix33 FGForce::Transform(void)
 {
   switch(ttype) {
   case tWindBody:
-    return fdmex->GetState()->GetTs2b();
+    return fdmex->GetAerodynamics()->GetTw2b();
   case tLocalBody:
     return fdmex->GetPropagate()->GetTl2b();
   case tCustom:
@@ -112,23 +127,27 @@ FGMatrix33 FGForce::Transform(void)
 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(1,2) =  cp*sy;
-  mT(1,3) = -sp;
+  mT(2,1) =  cp*sy;
+  mT(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(1,2) = srsp*cy - crsy;
+  mT(2,2) = srsp*sy + crcy;
+  mT(3,2) = sr*cp;
 
-  mT(3,1) = cr*sp*cy + sr*sy;
-  mT(3,2) = cr*sp*sy - sr*cy;
+  mT(1,3) = crcy*sp + sr*sy;
+  mT(2,3) = crsy*sp - sr*cy;
   mT(3,3) = cr*cp;
-  mT = mT.Inverse();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%