Fix some sim time issues with the locations of the ground cache and the

queries in there.

Modified Files:
 	flight.cxx flight.hxx groundcache.cxx
	groundcache.hxx JSBSim/JSBSim.cxx YASim/YASim.cxx
	YASim/YASim.hxx

src/FDM/JSBSim/JSBSim.cxx

@@ -442,7 +442,8 @@ void FGJSBsim::update( double dt )
       cart = FGLocation(lon, lat, alt+slr);
     }
     double cart_pos[3] = { cart(1), cart(2), cart(3) };
-    bool cache_ok = prepare_ground_cache_ft( State->Getsim_time(), cart_pos,
+    double t0 = State->Getsim_time();
+    bool cache_ok = prepare_ground_cache_ft( t0, t0 + dt, cart_pos,
                                              groundCacheRadius );
     if (!cache_ok) {
       SG_LOG(SG_FLIGHT, SG_WARN,

src/FDM/YASim/YASim.cxx

@@ -43,7 +43,8 @@ static const float W2HP = 1.3416e-3;
 static const float INHG2PA = 3386.389;
 static const float SLUG2KG = 14.59390;
 
-YASim::YASim(double dt)
+YASim::YASim(double dt) :
+    _simTime(0)
 {
 //     set_delta_t(dt);
     _fdm = new FGFDM();
@@ -209,7 +210,7 @@ void YASim::update(double dt)
     // build the environment cache.
     float vr = _fdm->getVehicleRadius();
     vr += 2.0*FT2M*dt*Math::mag3(v);
-    prepare_ground_cache_m( 0.0, xyz, vr );
+    prepare_ground_cache_m( _simTime, _simTime + dt, xyz, vr );
 
     // Track time increments.
     FGGround* gr
@@ -217,14 +218,15 @@ void YASim::update(double dt)
 
     int i;
     for(i=0; i<iterations; i++) {
-        gr->setTimeOffset(i*_dt);
+        gr->setTimeOffset(_simTime + i*_dt);
         copyToYASim(false);
         _fdm->iterate(_dt);
         copyFromYASim();
     }
 
-    // Reset the time increment.
+    // Increment the local sim time
-    gr->setTimeOffset(0.0);
+    _simTime += dt;
+    gr->setTimeOffset(_simTime);
 }
 
 void YASim::copyToYASim(bool copyState)

src/FDM/YASim/YASim.hxx

@@ -25,6 +25,7 @@ public:
 
     yasim::FGFDM* _fdm;
     float _dt;
+    double _simTime;
     enum {
         NED,
         UVW,

src/FDM/flight.cxx

@@ -643,18 +643,21 @@ void FGInterface::_busdump(void) {
 }
 
 bool
-FGInterface::prepare_ground_cache_m(double ref_time, const double pt[3],
+FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime,
-                                    double rad)
+                                    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],
+bool
-                                          double rad)
+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
@@ -825,29 +828,34 @@ FGInterface::get_groundlevel_m(const SGGeod& geod)
   double ref_time, 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 = globals->get_sim_time_sec();
+    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 = globals->get_sim_time_sec();
+    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;
     }
   }

src/FDM/flight.hxx

@@ -603,10 +603,10 @@ public:
     // Prepare the ground cache for the wgs84 position pt_*.
     // That is take all vertices in the ball with radius rad around the
     // position given by the pt_* and store them in a local scene graph.
-    bool prepare_ground_cache_m(double ref_time, const double pt[3],
+    bool prepare_ground_cache_m(double startSimTime, double endSimTime,
-                                double rad);
+                                const double pt[3], double rad);
-    bool prepare_ground_cache_ft(double ref_time, const double pt[3],
+    bool prepare_ground_cache_ft(double startSimTime, double endSimTime,
-                                 double rad);
+                                 const double pt[3], double rad);
 
 
     // Returns true if the cache is valid.

src/FDM/groundcache.cxx

@@ -134,10 +134,24 @@ public:
         const SGSceneUserData::Velocity* velocity = getVelocity(transform);
 
         SGVec3d center = _center;
-        _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
         double radius = _radius;
-        if (velocity)
+        _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
-            _radius += (_endTime - _startTime)*norm(velocity->linear);
+        if (velocity) {
+            SGVec3d staticCenter(_center);
+
+            double dtStart = velocity->referenceTime - _startTime;
+            SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
+            SGVec3d angle = dtStart*velocity->angular;
+            startCenter = SGQuatd::fromAngleAxis(angle).transform(startCenter);
+            
+            double dtEnd = velocity->referenceTime - _endTime;
+            SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
+            angle = dtEnd*velocity->angular;
+            endCenter = SGQuatd::fromAngleAxis(angle).transform(endCenter);
+
+            _center = 0.5*(startCenter + endCenter);
+            _radius += 0.5*dist(startCenter, endCenter);
+        }
         
         simgear::BVHSubTreeCollector::NodeList parentNodeList;
         mSubTreeCollector.pushNodeList(parentNodeList);
@@ -152,7 +166,8 @@ public:
                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
                 bvhTransform->setLinearVelocity(velocity->linear);
                 bvhTransform->setAngularVelocity(velocity->angular);
-                bvhTransform->setReferenceTime(_startTime);
+                bvhTransform->setReferenceTime(velocity->referenceTime);
+                bvhTransform->setStartTime(_startTime);
                 bvhTransform->setEndTime(_endTime);
                 bvhTransform->setId(velocity->id);
 
@@ -236,8 +251,8 @@ FGGroundCache::~FGGroundCache()
 }
 
 bool
-FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
+FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
-                                    double rad)
+                                    const SGVec3d& pt, double rad)
 {
     // Empty cache.
     found_ground = false;
@@ -260,21 +275,20 @@ FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
     reference_wgs84_point = pt;
     reference_vehicle_radius = rad;
     // Store the time reference used to compute movements of moving triangles.
-    cache_ref_time = ref_time;
+    cache_ref_time = startSimTime;
     
     // Get a normalized down vector valid for the whole cache
     SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
     down = hlToEc.rotate(SGVec3d(0, 0, 1));
     
     // Get the ground cache, that is a local collision tree of the environment
-    double endTime = cache_ref_time + 1; //FIXME??
+    CacheFill subtreeCollector(pt, rad, startSimTime, endSimTime);
-    CacheFill subtreeCollector(pt, rad, cache_ref_time, endTime);
     globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
     _localBvhTree = subtreeCollector.getBVHNode();
 
     // Try to get a croase altitude value for the ground cache
     SGLineSegmentd line(pt, pt + 2*reference_vehicle_radius*down);
-    simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time);
+    simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
     if (_localBvhTree)
         _localBvhTree->accept(lineSegmentVisitor);
 

src/FDM/groundcache.hxx

@@ -50,8 +50,8 @@ public:
     // Prepare the ground cache for the wgs84 position pt_*.
     // That is take all vertices in the ball with radius rad around the
     // position given by the pt_* and store them in a local scene graph.
-    bool prepare_ground_cache(double ref_time, const SGVec3d& pt,
+    bool prepare_ground_cache(double startSimTime, double endSimTime,
-                              double rad);
+                              const SGVec3d& pt, double rad);
 
     // Returns true if the cache is valid.
     // Also the reference time, point and radius values where the cache