Merge branch 'jmt/units-fix' into maint

[flightgear.git] / src / FDM / JSBSim / models / propulsion / FGForce.cpp

diff --git a/src/FDM/JSBSim/models/propulsion/FGForce.cpp b/src/FDM/JSBSim/models/propulsion/FGForce.cpp
index fec3686d9bd8db7f44dba2515661b69218c01eae..ff7532916325914025583df537d86be4388e5979 100644 (file)
--- a/src/FDM/JSBSim/models/propulsion/FGForce.cpp
+++ b/src/FDM/JSBSim/models/propulsion/FGForce.cpp
@@ -40,12 +40,16 @@ 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 {
 
@@ -55,8 +59,8 @@ 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;
@@ -95,7 +99,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 +116,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();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%