Port over remaining Point3D usage to the more type and unit safe SG* classes.

[flightgear.git] / src / FDM / flight.cxx

diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx
index e749b76fd637a1967911077e2fee21cd90848597..0e4ff912a3948a99ffce66c9a298993d110eae60 100644 (file)
--- a/src/FDM/flight.cxx
+++ b/src/FDM/flight.cxx
@@ -163,6 +163,8 @@ FGInterface::common_init ()
 
     set_inited( true );
 
+    ground_cache.set_cache_time_offset(globals->get_sim_time_sec());
+
 //     stamp();
 //     set_remainder( 0 );
 
@@ -643,18 +645,21 @@ void FGInterface::_busdump(void) {
 }
 
 bool
-FGInterface::prepare_ground_cache_m(double ref_time, const double pt[3],
-                                    double rad)
+FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime,
+                                    const double pt[3], double rad)
 {
-  return ground_cache.prepare_ground_cache(ref_time, SGVec3d(pt), rad);
+  return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
+                                           SGVec3d(pt), rad);
 }
 
-bool FGInterface::prepare_ground_cache_ft(double ref_time, const double pt[3],
-                                          double rad)
+bool
+FGInterface::prepare_ground_cache_ft(double startSimTime, double endSimTime,
+                                     const double pt[3], double rad)
 {
   // Convert units and do the real work.
   SGVec3d pt_ft = SG_FEET_TO_METER*SGVec3d(pt);
-  return ground_cache.prepare_ground_cache(ref_time, pt_ft, rad*SG_FEET_TO_METER);
+  return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
+                                           pt_ft, rad*SG_FEET_TO_METER);
 }
 
 bool
@@ -731,9 +736,8 @@ FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
   SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
   SGVec3d _contact, _normal, _linearVel, _angularVel;
   material = 0;
-  if (!ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
-                            _angularVel, id, material))
-      return false;
+  bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+                                  _angularVel, id, material);
   // correct the linear velocity, since the line intersector delivers
   // values for the start point and the get_agl function should
   // traditionally deliver for the contact point
@@ -743,7 +747,7 @@ FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
   assign(normal, _normal);
   assign(linearVel, _linearVel);
   assign(angularVel, _angularVel);
-  return true;
+  return ret;
 }
 
 bool
@@ -757,9 +761,8 @@ FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
   pt_m *= SG_FEET_TO_METER;
   SGVec3d _contact, _normal, _linearVel, _angularVel;
   material = 0;
-  if (!ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
-                            _angularVel, id, material))
-      return false;
+  bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+                                  _angularVel, id, material);
   // correct the linear velocity, since the line intersector delivers
   // values for the start point and the get_agl function should
   // traditionally deliver for the contact point
@@ -770,7 +773,7 @@ FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
   assign( normal, _normal );
   assign( linearVel, SG_METER_TO_FEET*_linearVel );
   assign( angularVel, _angularVel );
-  return true;
+  return ret;
 }
 
 bool
@@ -824,32 +827,37 @@ FGInterface::get_groundlevel_m(const SGGeod& geod)
 
   // FIXME: how to handle t - ref_time differences ???
   SGVec3d cpos;
-  double ref_time, radius;
+  double ref_time = 0, radius;
   // Prepare the ground cache for that position.
   if (!is_valid_m(&ref_time, cpos.data(), &radius)) {
-    bool ok = prepare_ground_cache_m(ref_time, pos.data(), 10);
+    double startTime = ref_time;
+    double endTime = startTime + 1;
+    bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), 10);
     /// This is most likely the case when the given altitude is
     /// too low, try with a new altitude of 10000m, that should be
     /// sufficient to find a ground level below everywhere on our planet
     if (!ok) {
-      pos = SGVec3d::fromGeod(SGGeod::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000));
+      pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
       /// If there is still no ground, return sea level radius
-      if (!prepare_ground_cache_m(ref_time, pos.data(), 10))
+      if (!prepare_ground_cache_m(startTime, endTime, pos.data(), 10))
         return 0;
     }
   } else if (radius*radius <= distSqr(pos, cpos)) {
+    double startTime = ref_time;
+    double endTime = startTime + 1;
+
     /// We reuse the old radius value, but only if it is at least 10 Meters ..
     if (!(10 < radius)) // Well this strange compare is nan safe
       radius = 10;
 
-    bool ok = prepare_ground_cache_m(ref_time, pos.data(), radius);
+    bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), radius);
     /// This is most likely the case when the given altitude is
     /// too low, try with a new altitude of 10000m, that should be
     /// sufficient to find a ground level below everywhere on our planet
     if (!ok) {
-      pos = SGVec3d::fromGeod(SGGeod::fromRadM(geod.getLongitudeRad(), geod.getLatitudeRad(), 10000));
+      pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
       /// If there is still no ground, return sea level radius
-      if (!prepare_ground_cache_m(ref_time, pos.data(), radius))
+      if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius))
         return 0;
     }
   }