X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=0b341159aadd04bae91e51e22971dc989d2fd461;hb=2314ccfe13e5e175763000e4c8ba86be79aa3a97;hp=0d5f4e09274b8c3bf77ce4ec527a3a6c85ba8091;hpb=2229491ffe4823d21d6c87211d9bb9e266d5078c;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 0d5f4e092..0b341159a 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -24,6 +24,8 @@ # include "config.h" #endif +#include "groundcache.hxx" + #include #include @@ -38,42 +40,53 @@ #include #include #include -#include +#include +#include #include #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef GROUNDCACHE_DEBUG +#include +#include
+#endif #include
#include #include #include "flight.hxx" -#include "groundcache.hxx" using namespace simgear; class FGGroundCache::CacheFill : public osg::NodeVisitor { public: - CacheFill(const SGVec3d& center, const double& radius, + CacheFill(const SGVec3d& center, const SGVec3d& down, const double& radius, const double& startTime, const double& endTime) : osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN), _center(center), + _down(down), _radius(radius), _startTime(startTime), - _endTime(endTime) + _endTime(endTime), + _sceneryHit(0, 0, 0), + _maxDown(SGGeod::fromCart(center).getElevationM() + 9999), + _material(0), + _haveHit(false) { setTraversalMask(SG_NODEMASK_TERRAIN_BIT); } @@ -134,10 +147,31 @@ public: const SGSceneUserData::Velocity* velocity = getVelocity(transform); SGVec3d center = _center; - _center = SGVec3d(inverseMatrix.preMult(_center.osg())); + SGVec3d down = _down; double radius = _radius; - if (velocity) - _radius += (_endTime - _startTime)*norm(velocity->linear); + bool haveHit = _haveHit; + const simgear::BVHMaterial* material = _material; + + _haveHit = false; + _center = toSG(inverseMatrix.preMult(toOsg(_center))); + _down = toSG(osg::Matrix::transform3x3(toOsg(_down), inverseMatrix)); + if (velocity) { + SGVec3d staticCenter(_center); + + double dtStart = velocity->referenceTime - _startTime; + SGVec3d startCenter = staticCenter + dtStart*velocity->linear; + SGQuatd startOr(SGQuatd::fromAngleAxis(dtStart*velocity->angular)); + startCenter = startOr.transform(startCenter); + + double dtEnd = velocity->referenceTime - _endTime; + SGVec3d endCenter = staticCenter + dtEnd*velocity->linear; + SGQuatd endOr(SGQuatd::fromAngleAxis(dtEnd*velocity->angular)); + endCenter = endOr.transform(endCenter); + + _center = 0.5*(startCenter + endCenter); + _down = startOr.transform(_down); + _radius += 0.5*dist(startCenter, endCenter); + } simgear::BVHSubTreeCollector::NodeList parentNodeList; mSubTreeCollector.pushNodeList(parentNodeList); @@ -152,7 +186,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); @@ -167,7 +202,22 @@ public: } else { mSubTreeCollector.popNodeList(parentNodeList); } + + if (_haveHit) { + if (velocity) { + double dt = _startTime - velocity->referenceTime; + SGQuatd ori(SGQuatd::fromAngleAxis(dt*velocity->angular)); + _sceneryHit = ori.transform(_sceneryHit); + _sceneryHit += dt*velocity->linear; + } + _sceneryHit = toSG(matrix.preMult(toOsg(_sceneryHit))); + } else { + _material = material; + _haveHit = haveHit; + } + _center = center; + _down = down; _radius = radius; } @@ -191,6 +241,17 @@ public: if (!bvNode) return; + // Find a croase ground intersection + SGLineSegmentd line(_center + _radius*_down, _center + _maxDown*_down); + simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, _startTime); + bvNode->accept(lineSegmentVisitor); + if (!lineSegmentVisitor.empty()) { + _sceneryHit = lineSegmentVisitor.getPoint(); + _material = lineSegmentVisitor.getMaterial(); + _maxDown = SGMiscd::max(_radius, dot(_down, _sceneryHit - _center)); + _haveHit = true; + } + // Get that part of the local bv tree that intersects our sphere // of interrest. mSubTreeCollector.setSphere(SGSphered(_center, _radius)); @@ -202,33 +263,53 @@ public: if (!bound.valid()) return false; + SGLineSegmentd downSeg(_center, _center + _maxDown*_down); double maxDist = bound._radius + _radius; - return distSqr(SGVec3d(bound._center), _center) <= maxDist*maxDist; + SGVec3d boundCenter(toVec3d(toSG(bound._center))); + return distSqr(downSeg, boundCenter) <= maxDist*maxDist; } SGSharedPtr getBVHNode() const { return mSubTreeCollector.getNode(); } + + bool getHaveElevationBelowCache() const + { return _haveHit; } + double getElevationBelowCache() const + { return SGGeod::fromCart(_sceneryHit).getElevationM(); } + const simgear::BVHMaterial* getMaterialBelowCache() const + { return _material; } private: - SGVec3d _center; + SGVec3d _down; double _radius; double _startTime; double _endTime; simgear::BVHSubTreeCollector mSubTreeCollector; + SGVec3d _sceneryHit; + double _maxDown; + const simgear::BVHMaterial* _material; + bool _haveHit; }; 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), down(0.0, 0.0, 0.0), found_ground(false) { +#ifdef GROUNDCACHE_DEBUG + _lookupTime = SGTimeStamp::fromSec(0.0); + _lookupCount = 0; + _buildTime = SGTimeStamp::fromSec(0.0); + _buildCount = 0; +#endif } FGGroundCache::~FGGroundCache() @@ -236,18 +317,25 @@ 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) { +#ifdef GROUNDCACHE_DEBUG + SGTimeStamp t0 = SGTimeStamp::now(); +#endif + // Empty cache. found_ground = false; SGGeod geodPt = SGGeod::fromCart(pt); // Don't blow away the cache ground_radius and stuff if there's no // scenery - if (!globals->get_tile_mgr()->scenery_available(geodPt, rad)) + if (!globals->get_tile_mgr()->schedule_scenery(geodPt, rad, 1.0)) { + SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available " + "returns false at " << geodPt << " " << pt << " " << rad); return false; - _altitude = 0; + } + _material = 0; // If we have an active wire, get some more area into the groundcache if (_wire) @@ -257,34 +345,48 @@ 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); + startSimTime += cache_time_offset; + endSimTime += cache_time_offset; + CacheFill subtreeCollector(pt, down, 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); - if (_localBvhTree) + if (subtreeCollector.getHaveElevationBelowCache()) { + // Use the altitude value below the cache that we gathered during + // cache collection + _altitude = subtreeCollector.getElevationBelowCache(); + _material = subtreeCollector.getMaterialBelowCache(); + found_ground = true; + } else if (_localBvhTree) { + // We have nothing below us, so try starting with the lowest point + // upwards for a croase altitude value + SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down); + simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime); _localBvhTree->accept(lineSegmentVisitor); - // If this is successful, store this altitude for croase altitude values - if (!lineSegmentVisitor.empty()) { - SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint()); - _altitude = geodPt.getElevationM(); - _material = lineSegmentVisitor.getMaterial(); - found_ground = true; - } else { - // Else do a crude scene query for the current point + if (!lineSegmentVisitor.empty()) { + SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint()); + _altitude = geodPt.getElevationM(); + _material = lineSegmentVisitor.getMaterial(); + found_ground = true; + } + } + + if (!found_ground) { + // Ok, still nothing here?? Last resort ... + double alt = 0; + _material = 0; found_ground = globals->get_scenery()-> - get_cart_elevation_m(pt, rad, _altitude, &_material); + get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material); + if (found_ground) + _altitude = alt; } // Still not sucessful?? @@ -292,6 +394,42 @@ FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build " "cache without any scenery below the aircraft"); +#ifdef GROUNDCACHE_DEBUG + t0 = SGTimeStamp::now() - t0; + _buildTime += t0; + _buildCount++; + + if (_buildCount > 60) { + double buildTime = 0; + if (_buildCount) + buildTime = _buildTime.toSecs()/_buildCount; + double lookupTime = 0; + if (_lookupCount) + lookupTime = _lookupTime.toSecs()/_lookupCount; + _buildTime = SGTimeStamp::fromSec(0.0); + _buildCount = 0; + _lookupTime = SGTimeStamp::fromSec(0.0); + _lookupCount = 0; + SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime + << ", lookup Time = " << lookupTime); + } + + if (!_group.valid()) { + _group = new osg::Group; + globals->get_scenery()->get_scene_graph()->addChild(_group); + fgSetInt("/fdm/groundcache-debug-level", -3); + } + _group->removeChildren(0, _group->getNumChildren()); + if (_localBvhTree) { + int level = fgGetInt("/fdm/groundcache-debug-level"); + if (-2 <= level) { + simgear::BVHDebugCollectVisitor debug(endSimTime, level); + _localBvhTree->accept(debug); + _group->addChild(debug.getNode()); + } + } +#endif + return found_ground; } @@ -320,6 +458,12 @@ public: return; leaf.traverse(*this); } + virtual void apply(BVHPageNode& leaf) + { + if (_foundId) + return; + leaf.traverse(*this); + } virtual void apply(BVHTransform& transform) { if (_foundId) @@ -340,11 +484,10 @@ public: if (_id == transform.getId()) { _foundId = true; - return; + } else { + transform.traverse(*this); } - transform.traverse(*this); - if (_foundId) { SGMatrixd toWorld = transform.getToWorldTransform(_time); SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros()); @@ -391,6 +534,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()) @@ -406,9 +550,9 @@ FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel, class FGGroundCache::CatapultFinder : public BVHVisitor { public: CatapultFinder(const SGSphered& sphere, const double& t) : + _haveLineSegment(false), _sphere(sphere), - _time(t), - _haveLineSegment(false) + _time(t) { } virtual void apply(BVHGroup& leaf) @@ -417,6 +561,12 @@ public: return; leaf.traverse(*this); } + virtual void apply(BVHPageNode& leaf) + { + if (!intersects(_sphere, leaf.getBoundingSphere())) + return; + leaf.traverse(*this); + } virtual void apply(BVHTransform& transform) { if (!intersects(_sphere, transform.getBoundingSphere())) @@ -516,6 +666,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); @@ -541,14 +692,25 @@ FGGroundCache::get_cat(double t, const SGVec3d& pt, bool FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact, SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel, - simgear::BVHNode::Id& id, const SGMaterial*& material) + simgear::BVHNode::Id& id, const simgear::BVHMaterial*& material) { +#ifdef GROUNDCACHE_DEBUG + SGTimeStamp t0 = SGTimeStamp::now(); +#endif + // 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); +#ifdef GROUNDCACHE_DEBUG + t0 = SGTimeStamp::now() - t0; + _lookupTime += t0; + _lookupCount++; +#endif + if (!lineSegmentVisitor.empty()) { // Have an intersection contact = lineSegmentVisitor.getPoint(); @@ -583,16 +745,27 @@ bool FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist, SGVec3d& contact, SGVec3d& linearVel, SGVec3d& angularVel, simgear::BVHNode::Id& id, - const SGMaterial*& material) + const simgear::BVHMaterial*& material) { if (!_localBvhTree) return false; +#ifdef GROUNDCACHE_DEBUG + SGTimeStamp t0 = SGTimeStamp::now(); +#endif + // 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); +#ifdef GROUNDCACHE_DEBUG + t0 = SGTimeStamp::now() - t0; + _lookupTime += t0; + _lookupCount++; +#endif + if (nearestPointVisitor.empty()) return false; @@ -628,6 +801,13 @@ public: leaf.traverse(*this); } + virtual void apply(BVHPageNode& leaf) + { + if (!_intersects(leaf.getBoundingSphere())) + return; + + leaf.traverse(*this); + } virtual void apply(BVHTransform& transform) { if (!_intersects(transform.getBoundingSphere())) @@ -735,12 +915,13 @@ 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); _wire = wireIntersector.getWire(); - return _wire; + return (_wire != NULL); } class FGGroundCache::WireFinder : public BVHVisitor { @@ -760,6 +941,12 @@ public: return; leaf.traverse(*this); } + virtual void apply(BVHPageNode& leaf) + { + if (_haveLineSegment) + return; + leaf.traverse(*this); + } virtual void apply(BVHTransform& transform) { if (_haveLineSegment) @@ -837,6 +1024,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);