From db2d4a7a68bde08a2b80496d9842e78b7df55fe6 Mon Sep 17 00:00:00 2001 From: frohlich Date: Sun, 15 Mar 2009 11:26:31 +0000 Subject: [PATCH] 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 | 3 ++- src/FDM/YASim/YASim.cxx | 12 +++++++----- src/FDM/YASim/YASim.hxx | 1 + src/FDM/flight.cxx | 32 ++++++++++++++++++++------------ src/FDM/flight.hxx | 8 ++++---- src/FDM/groundcache.cxx | 34 ++++++++++++++++++++++++---------- src/FDM/groundcache.hxx | 4 ++-- 7 files changed, 60 insertions(+), 34 deletions(-) diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index 83a8eac66..9267f757e 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/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, diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index f874cd330..70c7459d2 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/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; isetTimeOffset(i*_dt); + gr->setTimeOffset(_simTime + i*_dt); copyToYASim(false); _fdm->iterate(_dt); copyFromYASim(); } - // Reset the time increment. - gr->setTimeOffset(0.0); + // Increment the local sim time + _simTime += dt; + gr->setTimeOffset(_simTime); } void YASim::copyToYASim(bool copyState) diff --git a/src/FDM/YASim/YASim.hxx b/src/FDM/YASim/YASim.hxx index 86e9e36b3..2e8f77732 100644 --- a/src/FDM/YASim/YASim.hxx +++ b/src/FDM/YASim/YASim.hxx @@ -25,6 +25,7 @@ public: yasim::FGFDM* _fdm; float _dt; + double _simTime; enum { NED, UVW, diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 37cb97689..50ebaed51 100644 --- a/src/FDM/flight.cxx +++ b/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], - 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 @@ -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; } } diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index 658835f55..1ce5ab5d5 100644 --- a/src/FDM/flight.hxx +++ b/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], - double rad); - bool prepare_ground_cache_ft(double ref_time, const double pt[3], - double rad); + bool prepare_ground_cache_m(double startSimTime, double endSimTime, + const double pt[3], double rad); + bool prepare_ground_cache_ft(double startSimTime, double endSimTime, + const double pt[3], double rad); // Returns true if the cache is valid. diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 9b17eef53..ff36e6088 100644 --- a/src/FDM/groundcache.cxx +++ b/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) - _radius += (_endTime - _startTime)*norm(velocity->linear); + _center = SGVec3d(inverseMatrix.preMult(_center.osg())); + 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, - double rad) +FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime, + 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, cache_ref_time, endTime); + CacheFill subtreeCollector(pt, rad, startSimTime, endSimTime); 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); diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx index c548679a2..14aeb171c 100644 --- a/src/FDM/groundcache.hxx +++ b/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, - double rad); + bool prepare_ground_cache(double startSimTime, double endSimTime, + const SGVec3d& pt, double rad); // Returns true if the cache is valid. // Also the reference time, point and radius values where the cache -- 2.39.5