Andreas Gaeb: fix #222 (JSBSIm reset problems)

[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 1dffff82d0b1698f62ca24fb0446e8134bb9c9fc..0180d18e404b15f73aec103a04550d1822ce50c2 100644 (file)
--- a/src/FDM/JSBSim/models/propulsion/FGForce.cpp
+++ b/src/FDM/JSBSim/models/propulsion/FGForce.cpp
@@ -40,28 +40,31 @@ 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.14 2009/10/24 22:59:30 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;
-  vSense.InitMatrix(1);
 
   Debug(0);
 }
@@ -77,7 +80,7 @@ FGForce::~FGForce()
 
 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.
@@ -96,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:
@@ -110,29 +113,42 @@ 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(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();
   }
 }