fly/flightgear
1
0
Fork 0
Code Activity

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

Browse source 
queries in there.

Modified Files:
 	flight.cxx flight.hxx groundcache.cxx
	groundcache.hxx JSBSim/JSBSim.cxx YASim/YASim.cxx
	YASim/YASim.hxx
This commit is contained in:
frohlich 2009-03-15 11:26:31 +00:00 committed by Tim Moore
parent da8b3f9ce5
commit db2d4a7a68
7 changed files with 60 additions and 34 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
src/FDM/JSBSim/JSBSim.cxx
View file

@ -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,

12
src/FDM/YASim/YASim.cxx
View file

@ -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.
gr->setTimeOffset(0.0);
// Increment the local sim time
_simTime += dt;
gr->setTimeOffset(_simTime);
}
void YASim::copyToYASim(bool copyState)

1
src/FDM/YASim/YASim.hxx
View file

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

32
src/FDM/flight.cxx
View file

@ -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;
}
}

8
src/FDM/flight.hxx
View file

@ -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.

34
src/FDM/groundcache.cxx
View file

@ -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);

4
src/FDM/groundcache.hxx
View file

@ -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