Sync. w. JSBSim cvs

[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 f54ac9996a21d646a2a08046c6bdbf795de478f7..9f39088d7788d5702fadb42a32d71e1c1f2487b8 100644 (file)
--- a/src/FDM/JSBSim/models/propulsion/FGForce.cpp
+++ b/src/FDM/JSBSim/models/propulsion/FGForce.cpp
@@ -7,20 +7,20 @@
  ------------- 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.
 
 
@@ -40,12 +40,12 @@ and the cg.
 
 */
 
+#include "FGForce.h"
 #include <FGFDMExec.h>
 #include <models/FGAircraft.h>
 #include <models/FGPropagate.h>
 #include <models/FGMassBalance.h>
-#include <FGState.h>
-#include "FGForce.h"
+#include <models/FGAerodynamics.h>
 
 namespace JSBSim {
 
@@ -61,7 +61,7 @@ FGForce::FGForce(FGFDMExec *FDMExec) :
   mT(1,1) = 1; //identity matrix
   mT(2,2) = 1;
   mT(3,3) = 1;
-  vSense.InitMatrix(1);
+
   Debug(0);
 }
 
@@ -76,7 +76,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.
@@ -95,7 +95,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:
@@ -109,29 +109,38 @@ FGMatrix33 FGForce::Transform(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGForce::UpdateCustomTransformMatrix(void)
+{
+  double cp,sp,cr,sr,cy,sy;
+
+  cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
+  cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
+  cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
+
+  mT(1,1) =  cp*cy;
+  mT(1,2) =  cp*sy;
+  mT(1,3) = -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;
+  mT = mT.Inverse();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 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();
   }
 }