From 8f660b97ad95664c9eed11bf9a2afb247bba9fd9 Mon Sep 17 00:00:00 2001
From: frohlich <frohlich>
Date: Sun, 15 Mar 2009 18:18:21 +0000
Subject: [PATCH] Make a simulation reset on the carrier work again.

Modified Files:
	src/FDM/flight.cxx src/FDM/groundcache.cxx
	src/FDM/groundcache.hxx
---
 src/FDM/flight.cxx      | 8 +++++---
 src/FDM/groundcache.cxx | 9 +++++++++
 src/FDM/groundcache.hxx | 6 ++++++
 3 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx
index 50ebaed51..0e4ff912a 100644
--- 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 );
 
@@ -825,10 +827,10 @@ 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)) {
-    double startTime = globals->get_sim_time_sec();
+    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
@@ -841,7 +843,7 @@ FGInterface::get_groundlevel_m(const SGGeod& geod)
         return 0;
     }
   } else if (radius*radius <= distSqr(pos, cpos)) {
-    double startTime = globals->get_sim_time_sec();
+    double startTime = ref_time;
     double endTime = startTime + 1;
 
     /// We reuse the old radius value, but only if it is at least 10 Meters ..
diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx
index ff36e6088..2daba8b1d 100644
--- a/src/FDM/groundcache.cxx
+++ b/src/FDM/groundcache.cxx
@@ -238,6 +238,7 @@ FGGroundCache::FGGroundCache() :
     _altitude(0),
     _material(0),
     cache_ref_time(0),
+    cache_time_offset(0),
     _wire(0),
     reference_wgs84_point(SGVec3d(0, 0, 0)),
     reference_vehicle_radius(0),
@@ -282,6 +283,8 @@ FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
     down = hlToEc.rotate(SGVec3d(0, 0, 1));
     
     // Get the ground cache, that is a local collision tree of the environment
+    startSimTime += cache_time_offset;
+    endSimTime += cache_time_offset;
     CacheFill subtreeCollector(pt, rad, startSimTime, endSimTime);
     globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
     _localBvhTree = subtreeCollector.getBVHNode();
@@ -411,6 +414,7 @@ FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
     // Get the transform matrix and velocities of a moving body with id at t.
     if (!_localBvhTree)
         return false;
+    t += cache_time_offset;
     BodyFinder bodyFinder(id, t);
     _localBvhTree->accept(bodyFinder);
     if (bodyFinder.empty())
@@ -536,6 +540,7 @@ FGGroundCache::get_cat(double t, const SGVec3d& pt,
     double maxDistance = 1000;
 
     // Get the wire in question
+    t += cache_time_offset;
     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
     if (_localBvhTree)
         _localBvhTree->accept(catapultFinder);
@@ -565,6 +570,7 @@ FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
 {
     // Just set up a ground intersection query for the given point
     SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
+    t += cache_time_offset;
     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
     if (_localBvhTree)
         _localBvhTree->accept(lineSegmentVisitor);
@@ -610,6 +616,7 @@ FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
 
     // Just set up a ground intersection query for the given point
     SGSphered sphere(pt, maxDist);
+    t += cache_time_offset;
     simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
     _localBvhTree->accept(nearestPointVisitor);
 
@@ -755,6 +762,7 @@ private:
 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 {
     // Get the wire in question
+    t += cache_time_offset;
     WireIntersector wireIntersector(pt, t);
     if (_localBvhTree)
         _localBvhTree->accept(wireIntersector);
@@ -857,6 +865,7 @@ bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
         return false;
 
     // Get the wire in question
+    t += cache_time_offset;
     WireFinder wireFinder(_wire, t);
     if (_localBvhTree)
         _localBvhTree->accept(wireFinder);
diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx
index 14aeb171c..1db7fb0b6 100644
--- a/src/FDM/groundcache.hxx
+++ b/src/FDM/groundcache.hxx
@@ -62,6 +62,10 @@ public:
     const SGVec3d& get_down() const
     { return down; }
 
+    double get_cache_time_offset() const
+    { return cache_time_offset; }
+    void set_cache_time_offset(double time_offset)
+    { cache_time_offset = time_offset; }
 
     bool get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
                   SGVec3d& angularVel, simgear::BVHNode::Id id);
@@ -114,6 +118,8 @@ private:
     // The time reference for later call to intersection test routines.
     // Is required since we will have moving triangles in carriers.
     double cache_ref_time;
+    // The time the cache was initialized.
+    double cache_time_offset;
     // The wire to track.
     const simgear::BVHLineGeometry* _wire;