queries in there.
Modified Files:
flight.cxx flight.hxx groundcache.cxx
groundcache.hxx JSBSim/JSBSim.cxx YASim/YASim.cxx
YASim/YASim.hxx
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,
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();
// 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
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)
yasim::FGFDM* _fdm;
float _dt;
+ double _simTime;
enum {
NED,
UVW,
}
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
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;
}
}
// 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.
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);
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);
}
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;
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);
// 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
projects / flightgear.git / commitdiff