From 76ef08679d05f6393c13b69f98e70f239e2dfe41 Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sun, 28 May 2017 20:12:30 +0200
Subject: [PATCH] Cleaned up the JSBSim interface.

---
 src/FDM/JSBSim/JSBSim.cxx | 64 +++++++++++++++++----------------------
 src/FDM/JSBSim/JSBSim.hxx |  9 +++---
 2 files changed, 33 insertions(+), 40 deletions(-)

diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx
index 5377b933c..9149aeb49 100644
--- a/src/FDM/JSBSim/JSBSim.cxx
+++ b/src/FDM/JSBSim/JSBSim.cxx
@@ -103,10 +103,9 @@ public:
   virtual double GetAGLevel(double t, const FGLocation& l,
                             FGLocation& cont, FGColumnVector3& n,
                             FGColumnVector3& v, FGColumnVector3& w) const {
-    double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) };
-    double contact[3], normal[3], vel[3], angularVel[3], agl = 0;
-    mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal,
-                           vel, angularVel, &agl);
+    double contact[3], normal[3], vel[3], angularVel[3];
+    double agl = mInterface->get_agl_ft(t, l, SG_METER_TO_FEET*2, contact,
+                                        normal, vel, angularVel);
     n = FGColumnVector3( normal[0], normal[1], normal[2] );
     v = FGColumnVector3( vel[0], vel[1], vel[2] );
     w = FGColumnVector3( angularVel[0], angularVel[1], angularVel[2] );
@@ -115,10 +114,9 @@ public:
   }
 
   virtual double GetTerrainGeoCentRadius(double t, const FGLocation& l) const {
-    double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) };
-    double contact[3], normal[3], vel[3], angularVel[3], agl = 0;
-    mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal,
-                           vel, angularVel, &agl);
+    double contact[3], normal[3], vel[3], angularVel[3];
+    mInterface->get_agl_ft(t, l, SG_METER_TO_FEET*2, contact,
+                           normal, vel, angularVel);
     return sqrt(contact[0]*contact[0]+contact[1]*contact[1]+contact[2]*contact[2]);
   }
 
@@ -421,11 +419,11 @@ void FGJSBsim::init()
 
     if ( needTrim ) {
       const FGLocation& cart = fgic->GetPosition();
-      double cart_pos[3], contact[3], d[3], vel[3], agl;
-      update_ground_cache(cart, cart_pos, 0.01);
+      double contact[3], d[3], vel[3];
+      update_ground_cache(cart, 0.01);
 
-      get_agl_ft(fdmex->GetSimTime(), cart_pos, SG_METER_TO_FEET*2, contact,
-                 d, vel, d, &agl);
+      get_agl_ft(fdmex->GetSimTime(), cart, SG_METER_TO_FEET*2, contact, d, vel,
+                 d);
       double terrain_alt = sqrt(contact[0]*contact[0] + contact[1]*contact[1]
                                 + contact[2]*contact[2]) - cart.GetSeaLevelRadius();
 
@@ -519,9 +517,8 @@ void FGJSBsim::update( double dt )
 
     int multiloop = _calc_multiloop(dt);
     FGLocation cart = Auxiliary->GetLocationVRP();
-    double cart_pos[3];
 
-    update_ground_cache(cart, cart_pos, dt);
+    update_ground_cache(cart, dt);
 
     copy_to_JSBsim();
 
@@ -801,10 +798,9 @@ bool FGJSBsim::copy_from_JSBsim()
 
     _set_Altitude_AGL( Propagate->GetDistanceAGL() );
     {
-      double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) };
       double contact[3], d[3], sd, t;
       is_valid_m(&t, d, &sd);
-      get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, d, d, d, &sd);
+      get_agl_ft(t, l, SG_METER_TO_FEET*2, contact, d, d, d);
       double rwrad
         = FGColumnVector3( contact[0], contact[1], contact[2] ).Magnitude();
       _set_Runway_altitude( rwrad - get_Sea_level_radius() );
@@ -1293,8 +1289,7 @@ void FGJSBsim::do_trim(void)
   SG_LOG( SG_FLIGHT, SG_INFO, "  Trim complete" );
 }
 
-bool FGJSBsim::update_ground_cache(const FGLocation& cart, double* cart_pos,
-                                   double dt)
+bool FGJSBsim::update_ground_cache(const FGLocation& cart, double dt)
 {
   // Compute the radius of the aircraft. That is the radius of a ball
   // where all gear units are in. At the moment it is at least 10ft ...
@@ -1311,9 +1306,7 @@ bool FGJSBsim::update_ground_cache(const FGLocation& cart, double* cart_pos,
   // ground in this area.
   double groundCacheRadius = acrad + 2*dt*Propagate->GetUVW().Magnitude();
 
-  cart_pos[0] = cart(1);
-  cart_pos[1] = cart(2);
-  cart_pos[2] = cart(3);
+  double cart_pos[3] {cart(1), cart(2), cart(3)};
   double t0 = fdmex->GetSimTime();
   bool cache_ok = prepare_ground_cache_ft( t0, t0 + dt, cart_pos,
                                            groundCacheRadius );
@@ -1337,22 +1330,22 @@ bool FGJSBsim::update_ground_cache(const FGLocation& cart, double* cart_pos,
   return cache_ok;
 }
 
-bool
-FGJSBsim::get_agl_ft(double t, const double pt[3], double alt_off,
+double
+FGJSBsim::get_agl_ft(double t, const FGColumnVector3& loc, double alt_off,
                      double contact[3], double normal[3], double vel[3],
-                     double angularVel[3], double *agl)
+                     double angularVel[3])
 {
   const simgear::BVHMaterial* material;
   simgear::BVHNode::Id id;
+  double pt[3] {loc(1), loc(2), loc(3)};
 
   // don't check the return value and continue above scenery discontinuity
   // see http://osdir.com/ml/flightgear-sim/2014-04/msg00145.html
   FGInterface::get_agl_ft(t, pt, alt_off, contact, normal, vel,
-                               angularVel, material, id);
+                          angularVel, material, id);
 
   SGGeod geodPt = SGGeod::fromCart(SG_FEET_TO_METER*SGVec3d(pt));
   SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
-  *agl = dot(hlToEc.rotate(SGVec3d(0, 0, 1)), SGVec3d(contact) - SGVec3d(pt));
 
 #ifdef JSBSIM_USE_GROUNDREACTIONS
   bool terrain_active = (terrain->getIntValue("override-level", -1) > 0) ? false : true;
@@ -1382,7 +1375,7 @@ FGJSBsim::get_agl_ft(double t, const double pt[3], double alt_off,
 #else
   terrain->setBoolValue("valid", false);
 #endif
-  return true;
+  return dot(hlToEc.rotate(SGVec3d(0, 0, 1)), SGVec3d(contact) - SGVec3d(pt));
 }
 
 inline static double sqr(double x)
@@ -1448,16 +1441,15 @@ void FGJSBsim::update_external_forces(double t_off)
     hook_tip_body(1) -= hook_length * cos_fi;
     hook_tip_body(3) += hook_length * sin_fi;    
     
-    double contact[3];
-    double ground_normal[3];
-    double ground_vel[3];
-    double ground_angular_vel[3];
-    double root_agl_ft;
-
     if (!got_wire) {
-        bool got = get_agl_ft(t_off, hook_area[1], 0, contact, ground_normal,
-                              ground_vel, ground_angular_vel, &root_agl_ft);
-        if (got && root_agl_ft > 0 && root_agl_ft < hook_length) {
+      double contact[3];
+      double ground_normal[3];
+      double ground_vel[3];
+      double ground_angular_vel[3];
+      double root_agl_ft = get_agl_ft(t_off, hook_root, 0, contact,
+                                      ground_normal, ground_vel,
+                                      ground_angular_vel);
+        if (root_agl_ft > 0 && root_agl_ft < hook_length) {
             FGColumnVector3 ground_normal_body = Tl2b * (Tec2l * FGColumnVector3(ground_normal[0], ground_normal[1], ground_normal[2]));
             FGColumnVector3 contact_body = Tl2b * Location.LocationToLocal(FGColumnVector3(contact[0], contact[1], contact[2]));
             double D = -DotProduct(contact_body, ground_normal_body);
diff --git a/src/FDM/JSBSim/JSBSim.hxx b/src/FDM/JSBSim/JSBSim.hxx
index 54799158e..bf5a11d5b 100644
--- a/src/FDM/JSBSim/JSBSim.hxx
+++ b/src/FDM/JSBSim/JSBSim.hxx
@@ -72,6 +72,7 @@ class FGInitialCondition;
 class FGLocation;
 class FGAccelerations;
 class FGPropertyManager;
+class FGColumnVector3;
 }
 
 // Adding it here will cause a namespace clash in FlightGear -EMH-
@@ -216,9 +217,9 @@ public:
     bool ToggleDataLogging(bool state);
     bool ToggleDataLogging(void);
 
-    bool get_agl_ft(double t, const double pt[3], double alt_off,
-                    double contact[3], double normal[3], double vel[3],
-                    double angularVel[3], double *agl);
+    double get_agl_ft(double t, const JSBSim::FGColumnVector3& loc,
+                      double alt_off, double contact[3], double normal[3],
+                      double vel[3], double angularVel[3]);
 private:
     JSBSim::FGFDMExec *fdmex;
     JSBSim::FGInitialCondition *fgic;
@@ -305,7 +306,7 @@ private:
 
     void do_trim(void);
 
-    bool update_ground_cache(const JSBSim::FGLocation& cart, double* cart_pos, double dt);
+    bool update_ground_cache(const JSBSim::FGLocation& cart, double dt);
     void init_gear(void);
     void update_gear(void);