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;