From 98b5e7e6fed4e23d1d1191a71401d856724cc0c9 Mon Sep 17 00:00:00 2001
From: ehofman <ehofman>
Date: Mon, 31 Aug 2009 07:06:26 +0000
Subject: [PATCH] Bertrand Coconnier:

Additionally to the original fix (problem with the Tec2b matrix initialization) I have modified the code for the "STRUCTURE" contacts in order not to generate NaNs when the z direction of the body frame is normal to the ground normal. Now there should no longer be any NaN generated by the landing gear code.
---
 src/FDM/JSBSim/models/FGLGear.cpp     | 44 +++++++++++++++++++--------
 src/FDM/JSBSim/models/FGPropagate.cpp | 14 +++++++++
 2 files changed, 45 insertions(+), 13 deletions(-)

diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp
index c7a7534c7..e3cfeadf2 100644
--- a/src/FDM/JSBSim/models/FGLGear.cpp
+++ b/src/FDM/JSBSim/models/FGLGear.cpp
@@ -392,19 +392,37 @@ FGColumnVector3& FGLGear::Force(void)
 
 void FGLGear::ComputeGroundCoordSys(void)
 {
-  // Compute the rolling direction projected on the ground
-  // It consists in finding a vector 'r' such that 'r' lies in the plane (w,z) and r.n = 0 (scalar
-  // product) where:
-  // 'n' is the normal to the ground,
-  // (x,y,z) are the directions defined in the body coord system
-  // and 'w' is 'x' rotated by the steering angle (SteerAngle) in the plane (x,y). 
-  // r = u * w + v * z and r.n = 0 => v/u = -w.n/z.n = a
-  // We also want u**2+v**2=1 and u > 0 (i.e. r orientated in the same 'direction' than w)
-  // after some arithmetic, one finds that :
-  double a = -(vGroundNormal(eX)*cos(SteerAngle)+vGroundNormal(eY)*sin(SteerAngle)) / vGroundNormal(eZ);
-  double u = 1. / sqrt(1. + a*a);
-  double v = a * u;
-  FGColumnVector3 vRollingGroundVec = FGColumnVector3(u * cos(SteerAngle), u * sin(SteerAngle), v);
+  FGColumnVector3 vRollingGroundVec;
+
+  if (eContactType == ctBOGEY) {
+    // Compute the rolling direction projected on the ground
+    // It consists in finding a vector 'r' such that 'r' lies in the plane (w,z) and r.n = 0 (scalar
+    // product) where:
+    // 'n' is the normal to the ground,
+    // (x,y,z) are the directions defined in the body coord system
+    // and 'w' is 'x' rotated by the steering angle (SteerAngle) in the plane (x,y). 
+    // r = u * w + v * z and r.n = 0 => v/u = -w.n/z.n = a
+    // We also want u**2+v**2=1 and u > 0 (i.e. r orientated in the same 'direction' than w)
+    // after some arithmetic, one finds that :
+    double a = -(vGroundNormal(eX)*cos(SteerAngle)+vGroundNormal(eY)*sin(SteerAngle)) / vGroundNormal(eZ);
+    double u = 1. / sqrt(1. + a*a);
+    double v = a * u;
+    vRollingGroundVec = FGColumnVector3(u * cos(SteerAngle), u * sin(SteerAngle), v);
+  }
+  else {
+    // Here the only significant direction is the normal to the ground "vGroundNormal". Since there is
+    // no wheel the 2 other vectors of the orthonormal basis are not meaningful and are only used to
+    // create the transformation matrix Tg2b. So we are building vRollingGroundVec as an arbitrary
+    // vector normal to vGroundNormal
+    if (fabs(vGroundNormal(eX)) > 0.)
+      vRollingGroundVec = FGColumnVector3(-vGroundNormal(eZ)/vGroundNormal(eX), 0., 1.);
+    else if (fabs(vGroundNormal(eY)) > 0.)
+      vRollingGroundVec = FGColumnVector3(0., -vGroundNormal(eZ)/vGroundNormal(eY), 1.);
+    else
+      vRollingGroundVec = FGColumnVector3(1., 0., -vGroundNormal(eX)/vGroundNormal(eZ));
+
+    vRollingGroundVec.Normalize();
+  }
 
   // The sliping direction is the cross product multiplication of the ground normal and rolling
   // directions
diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp
index d665c3062..93d6d4749 100644
--- a/src/FDM/JSBSim/models/FGPropagate.cpp
+++ b/src/FDM/JSBSim/models/FGPropagate.cpp
@@ -189,6 +189,20 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 
   // Recompute the LocalTerrainRadius.
   RecomputeLocalTerrainRadius();
+
+  // These local copies of the transformation matrices are for use for
+  // initial conditions only.
+
+  Tl2b = GetTl2b();           // local to body frame transform
+  Tb2l = Tl2b.Transposed();   // body to local frame transform
+  Tl2ec = GetTl2ec();         // local to ECEF transform
+  Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
+  Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
+  Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
+  Ti2ec = GetTi2ec();         // ECI to ECEF transform
+  Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
+  Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
+  Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-- 
2.39.5