From 24a148880ce9ec47f09421971ae36cd7a26f73ef Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sun, 28 May 2017 20:14:33 +0200
Subject: [PATCH] Fixes to the trim on ground feature.

* Skip the gears when located over water. They will be skipped by the ground reactions anyway making the trim irrelevant.
* Ignore the ground bumpiness during the trim. The trim algorithm seems not to
be robust enough to handle that. This does not make much difference to the
converged solution anyway since the bumpiness is generally low.
* Fixed the WOW status for contacts over water (was 'false' is now 'true').
* All the gear/contact data are now properly reset when WOW==false. The reset
code is now common to all the case where WOW is false.
---
 src/FDM/JSBSim/initialization/FGTrim.cpp |  5 +++-
 src/FDM/JSBSim/models/FGLGear.cpp        | 36 +++++++++---------------
 src/FDM/JSBSim/models/FGSurface.cpp      |  3 ++
 3 files changed, 21 insertions(+), 23 deletions(-)

diff --git a/src/FDM/JSBSim/initialization/FGTrim.cpp b/src/FDM/JSBSim/initialization/FGTrim.cpp
index 4b56b9ff7..22072d83c 100644
--- a/src/FDM/JSBSim/initialization/FGTrim.cpp
+++ b/src/FDM/JSBSim/initialization/FGTrim.cpp
@@ -402,8 +402,11 @@ void FGTrim::trimOnGround(void)
     FGColumnVector3 normal, vDummy;
     FGLocation lDummy;
     double height = gearLoc.GetContactPoint(lDummy, normal, vDummy, vDummy);
-    c.normal = Tec2b * normal;
 
+    if (gear->IsBogey() && !GroundReactions->GetSolid())
+      continue;
+
+    c.normal = Tec2b * normal;
     contacts.push_back(c);
 
     if (height < hmin) {
diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp
index f8e6e2d81..6727fb7e5 100644
--- a/src/FDM/JSBSim/models/FGLGear.cpp
+++ b/src/FDM/JSBSim/models/FGLGear.cpp
@@ -298,15 +298,19 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
 
     // Does this surface contact point interact with another surface?
     if (surface) {
-      height -= (*surface).GetBumpHeight();
+      if (!fdmex->GetTrimStatus())
+        height -= (*surface).GetBumpHeight();
       staticFFactor = (*surface).GetStaticFFactor();
       rollingFFactor = (*surface).GetRollingFFactor();
       maximumForce = (*surface).GetMaximumForce();
       isSolid =  (*surface).GetSolid();
     }
 
+    FGColumnVector3 vWhlDisplVec;
+    double LGearProj = 1.0;
+
     if (height < 0.0) {
-      WOW = isSolid;
+      WOW = true;
       vGroundNormal = in.Tec2b * normal;
 
       // The height returned by GetGroundCallback() is the AGL and is expressed
@@ -314,8 +318,7 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
       // this height in actual compression of the strut (BOGEY) or in the normal
       // direction to the ground (STRUCTURE)
       double normalZ = (in.Tec2l*normal)(eZ);
-      double LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
-      FGColumnVector3 vWhlDisplVec;
+      LGearProj = -(mTGear.Transposed() * vGroundNormal)(eZ);
 
       // The following equations use the vector to the tire contact patch
       // including the strut compression.
@@ -326,8 +329,7 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
           vWhlDisplVec = mTGear * FGColumnVector3(0., 0., -compressLength);
         } else {
           // Gears don't (or hardly) compress in liquids
-          compressLength = 0.0;
-          vWhlDisplVec = 0.0 * vGroundNormal;
+          WOW = false;
         }
         break;
       case ctSTRUCTURE:
@@ -335,19 +337,16 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
         vWhlDisplVec = compressLength * vGroundNormal;
         break;
       }
+    }
+    else
+      WOW = false;
 
+    if (WOW) {
       FGColumnVector3 vWhlContactVec = vWhlBodyVec + vWhlDisplVec;
       vActingXYZn = vXYZn + Tb2s * vWhlDisplVec;
       FGColumnVector3 vBodyWhlVel = in.PQR * vWhlContactVec;
       vBodyWhlVel += in.UVW - in.Tec2b * terrainVel;
-
-      if (isSolid) {
-        vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
-      } else {
-        // wheels don't spin up in liquids: let wheel spin down slowly
-        vWhlVelVec(eX) -= 13.0 * in.TotalDeltaT;
-        if (vWhlVelVec(eX) < 0.0) vWhlVelVec(eX) = 0.0;
-      }
+      vWhlVelVec = mTGear.Transposed() * vBodyWhlVel;
 
       InitializeReporting();
       ComputeSteeringAngle();
@@ -375,14 +374,12 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
       // Prepare the Jacobians and the Lagrange multipliers for later friction
       // forces calculations.
       ComputeJacobian(vWhlContactVec);
-
     } else { // Gear is NOT compressed
-
-      WOW = false;
       compressLength = 0.0;
       compressSpeed = 0.0;
       WheelSlip = 0.0;
       StrutForce = 0.0;
+      vWhlDisplVec.InitMatrix();
 
       LMultiplier[ftRoll].value = 0.0;
       LMultiplier[ftSide].value = 0.0;
@@ -397,11 +394,6 @@ const FGColumnVector3& FGLGear::GetBodyForces(FGSurface *surface)
 
       ResetReporting();
     }
-
-  } else if (gearPos < 0.01) { // Gear UP
-
-    WOW = false;
-    vWhlVelVec.InitMatrix();
   }
 
   if (!fdmex->GetTrimStatus()) {
diff --git a/src/FDM/JSBSim/models/FGSurface.cpp b/src/FDM/JSBSim/models/FGSurface.cpp
index f91b57ba3..bf47892ee 100644
--- a/src/FDM/JSBSim/models/FGSurface.cpp
+++ b/src/FDM/JSBSim/models/FGSurface.cpp
@@ -79,6 +79,9 @@ void FGSurface::resetValues(void)
   maximumForce = DBL_MAX;
   bumpiness = 0.0;
   isSolid = true;
+  pos[0] = 0.0;
+  pos[1] = 0.0;
+  pos[2] = 0.0;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%