From 679bda6980d8f8e47791c66a0c9bb0d6c9e42a83 Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Mon, 24 Aug 2020 21:09:54 +0200
Subject: [PATCH] Better precision for Tl2ec and Tec2l computations.

---
 src/models/FGInertial.cpp         | 15 ++++++---
 src/models/FGInertial.h           |  4 +--
 tests/unit_tests/FGInertialTest.h | 51 ++++++++++++++++++++++++++-----
 3 files changed, 55 insertions(+), 15 deletions(-)

diff --git a/src/models/FGInertial.cpp b/src/models/FGInertial.cpp
index afb07681f5..3bcab01488 100644
--- a/src/models/FGInertial.cpp
+++ b/src/models/FGInertial.cpp
@@ -138,10 +138,9 @@ bool FGInertial::Run(bool Holding)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-FGMatrix33 FGInertial::GetTl2ec(FGLocation& location) const
+FGMatrix33 FGInertial::GetTl2ec(const FGLocation& location) const
 {
-  FGColumnVector3 North, Down, East{-location.GetSinLongitude(),
-                                    location.GetCosLongitude(), 0.};
+  FGColumnVector3 North, Down, East{-location(eY), location(eX), 0.};
 
   switch (gravType) {
   case gtStandard:
@@ -151,9 +150,15 @@ FGMatrix33 FGInertial::GetTl2ec(FGLocation& location) const
     }
     break;
   case gtWGS84:
-    Down = GetGravityJ2(location);
-  }
+    {
+      FGLocation sea_level = location;
+      sea_level.SetPositionGeodetic(location.GetLongitude(),
+                                    location.GetGeodLatitudeRad(), 0.0);
+      Down = GetGravityJ2(location);
+      Down -= vOmegaPlanet*(vOmegaPlanet*sea_level);}
+    }
   Down.Normalize();
+  East.Normalize();
   North = East*Down;
 
   return FGMatrix33{North(eX), East(eX), Down(eX),
diff --git a/src/models/FGInertial.h b/src/models/FGInertial.h
index 55839ce193..b9d5519e77 100644
--- a/src/models/FGInertial.h
+++ b/src/models/FGInertial.h
@@ -161,7 +161,7 @@ class FGInertial : public FGModel {
       @return a rotation matrix of the transform from the earth centered frame
               to the local horizontal frame.
   */
-  FGMatrix33 GetTl2ec(FGLocation& location) const;
+  FGMatrix33 GetTl2ec(const FGLocation& location) const;
 
   /** Transform matrix from the earth centered to local horizontal frame.
       The local frame is the NED (North-East-Down) frame. Since the Down
@@ -175,7 +175,7 @@ class FGInertial : public FGModel {
       @return a rotation matrix of the transform from the earth centered frame
               to the local horizontal frame.
   */
-  FGMatrix33 GetTec2l(FGLocation& location) const
+  FGMatrix33 GetTec2l(const FGLocation& location) const
   { return GetTl2ec(location).Transposed(); }
 
   struct Inputs {
diff --git a/tests/unit_tests/FGInertialTest.h b/tests/unit_tests/FGInertialTest.h
index 7ea2f570e9..68dbd0b811 100644
--- a/tests/unit_tests/FGInertialTest.h
+++ b/tests/unit_tests/FGInertialTest.h
@@ -1,4 +1,3 @@
-#include <limits>
 #include <cxxtest/TestSuite.h>
 
 #include <FGFDMExec.h>
@@ -7,7 +6,7 @@
 
 using namespace JSBSim;
 
-const double epsilon = 100. * std::numeric_limits<double>::epsilon();
+const double epsilon = 1e-5;
 constexpr double degtorad = M_PI / 180.;
 
 class FGInertialTest : public CxxTest::TestSuite
@@ -18,16 +17,52 @@ class FGInertialTest : public CxxTest::TestSuite
     FGInertial* planet = fdmex.GetInertial();
     fdmex.SetPropertyValue("simulation/gravity-model", 0);
     FGLocation loc;
-    FGMatrix33 Tl2ec, Tec2l;
+    FGMatrix33 Tec2l;
     double radius = planet->GetSemimajor();
 
     for(double lon=-180.; lon <= 180.; lon += 30.) {
+      double longitude = lon * degtorad;
+      double cosLon = cos(longitude);
+      double sinLon = sin(longitude);
+
       for(double lat=-90.; lat <= 90.; lat += 30.) {
-        loc.SetPosition(lon*degtorad, lat*degtorad, radius);
-        Tl2ec = planet->GetTl2ec(loc);
-        Tec2l = planet->GetTec2l(loc);
-        TS_ASSERT_MATRIX_EQUALS(loc.GetTl2ec(), Tl2ec);
-        TS_ASSERT_MATRIX_EQUALS(loc.GetTec2l(), Tec2l);
+        double latitude = lat * degtorad;
+        loc.SetPosition(longitude, latitude, radius);
+        double cosLat = cos(latitude);
+        double sinLat = sin(latitude);
+        Tec2l = { -cosLon*sinLat, -sinLon*sinLat,  cosLat,
+                      -sinLon   ,     cosLon    ,    0.0 ,
+                  -cosLon*cosLat, -sinLon*cosLat, -sinLat  };
+        TS_ASSERT_MATRIX_EQUALS(planet->GetTec2l(loc), Tec2l);
+        TS_ASSERT_MATRIX_EQUALS(planet->GetTl2ec(loc), Tec2l.Transposed());
+      }
+    }
+  }
+
+  void testTransformationMatricesWGS84Earth() {
+    FGFDMExec fdmex;
+    FGInertial* planet = fdmex.GetInertial();
+    fdmex.SetPropertyValue("simulation/gravity-model", 1);
+    FGLocation loc;
+    FGMatrix33 Tec2l;
+
+    loc.SetEllipse(planet->GetSemimajor(), planet->GetSemiminor());
+
+    for(double lat=-90.; lat <= 90.; lat += 30.) {
+      double latitude = lat * degtorad;
+      double cosLat = cos(latitude);
+      double sinLat = sin(latitude);
+
+      for(double lon=-180.; lon <= 180.; lon += 30.) {
+        double longitude = lon * degtorad;
+        loc.SetPositionGeodetic(longitude, latitude, 0.0);
+        double cosLon = cos(longitude);
+        double sinLon = sin(longitude);
+        Tec2l = { -cosLon*sinLat, -sinLon*sinLat,  cosLat,
+                      -sinLon   ,     cosLon    ,    0.0 ,
+                  -cosLon*cosLat, -sinLon*cosLat, -sinLat  };
+        TS_ASSERT_MATRIX_EQUALS(planet->GetTec2l(loc), Tec2l);
+        TS_ASSERT_MATRIX_EQUALS(planet->GetTl2ec(loc), Tec2l.Transposed());
       }
     }
   }