+using namespace simgear;
+
+class FGGroundCache::CacheFill : public osg::NodeVisitor {
+public:
+ 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),
+ _sceneryHit(0, 0, 0),
+ _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
+ _material(0),
+ _haveHit(false)
+ {
+ setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
+ }
+ virtual void apply(osg::Node& node)
+ {
+ if (!testBoundingSphere(node.getBound()))
+ return;
+
+ addBoundingVolume(node);
+ }
+
+ virtual void apply(osg::Group& group)
+ {
+ if (!testBoundingSphere(group.getBound()))
+ return;
+
+ simgear::BVHSubTreeCollector::NodeList parentNodeList;
+ mSubTreeCollector.pushNodeList(parentNodeList);
+
+ traverse(group);
+ addBoundingVolume(group);
+
+ mSubTreeCollector.popNodeList(parentNodeList);
+ }
+
+ virtual void apply(osg::Transform& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::Camera& camera)
+ {
+ if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
+ return;
+ handleTransform(camera);
+ }
+ virtual void apply(osg::CameraView& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::MatrixTransform& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::PositionAttitudeTransform& transform)
+ { handleTransform(transform); }
+
+ void handleTransform(osg::Transform& transform)
+ {
+ // Hmm, may be this needs to be refined somehow ...
+ if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
+ return;
+
+ if (!testBoundingSphere(transform.getBound()))
+ return;
+
+ osg::Matrix inverseMatrix;
+ if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
+ return;
+ osg::Matrix matrix;
+ if (!transform.computeLocalToWorldMatrix(matrix, this))
+ return;
+
+ // Look for a velocity note
+ const SGSceneUserData::Velocity* velocity = getVelocity(transform);
+
+ SGVec3d center = _center;
+ SGVec3d down = _down;
+ double radius = _radius;
+ bool haveHit = _haveHit;
+ const SGMaterial* 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);
+
+ addBoundingVolume(transform);
+ traverse(transform);
+
+ if (mSubTreeCollector.haveChildren()) {
+ if (velocity) {
+ simgear::BVHMotionTransform* bvhTransform;
+ bvhTransform = new simgear::BVHMotionTransform;
+ bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
+ bvhTransform->setLinearVelocity(velocity->linear);
+ bvhTransform->setAngularVelocity(velocity->angular);
+ bvhTransform->setReferenceTime(velocity->referenceTime);
+ bvhTransform->setStartTime(_startTime);
+ bvhTransform->setEndTime(_endTime);
+ bvhTransform->setId(velocity->id);
+
+ mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
+ } else {
+ simgear::BVHTransform* bvhTransform;
+ bvhTransform = new simgear::BVHTransform;
+ bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
+
+ mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
+ }
+ } 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;
+ }
+
+ const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
+ {
+ SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
+ if (!userData)
+ return 0;
+ return userData->getVelocity();
+ }
+ simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
+ {
+ SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
+ if (!userData)
+ return 0;
+ return userData->getBVHNode();
+ }
+ void addBoundingVolume(osg::Node& node)
+ {
+ simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
+ 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));
+ bvNode->accept(mSubTreeCollector);
+ }
+
+ bool testBoundingSphere(const osg::BoundingSphere& bound) const
+ {
+ if (!bound.valid())
+ return false;
+
+ SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
+ double maxDist = bound._radius + _radius;
+ SGVec3d boundCenter(toVec3d(toSG(bound._center)));
+ return distSqr(downSeg, boundCenter) <= maxDist*maxDist;
+ }
+
+ SGSharedPtr<simgear::BVHNode> getBVHNode() const
+ { return mSubTreeCollector.getNode(); }
+
+ bool getHaveElevationBelowCache() const
+ { return _haveHit; }
+ double getElevationBelowCache() const
+ { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
+ const SGMaterial* getMaterialBelowCache() const
+ { return _material; }
+
+private:
+ SGVec3d _center;
+ SGVec3d _down;
+ double _radius;
+ double _startTime;
+ double _endTime;
+
+ simgear::BVHSubTreeCollector mSubTreeCollector;
+ SGVec3d _sceneryHit;
+ double _maxDown;
+ const SGMaterial* _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)