- return true;
- }
-
- bool enterNode(osg::Node& node)
- {
- if (!enterBoundingSphere(node.getBound()))
- return false;
-
- updateCullMode(node.getStateSet());
-
- FGGroundCache::GroundProperty& gp = mGroundProperty;
- // get some material information for use in the gear model
- gp.material = globals->get_matlib()->findMaterial(&node);
- if (gp.material) {
- gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
- return true;
- }
- osg::Referenced* base = node.getUserData();
- if (!base)
- return true;
- FGAICarrierHardware *ud =
- dynamic_cast<FGAICarrierHardware*>(base);
- if (!ud)
- return true;
-
- switch (ud->type) {
- case FGAICarrierHardware::Wire:
- gp.type = FGInterface::Wire;
- gp.wire_id = ud->id;
- break;
- case FGAICarrierHardware::Catapult:
- gp.type = FGInterface::Catapult;
- break;
- default:
- gp.type = FGInterface::Solid;
- break;
- }
- // Copy the velocity from the carrier class.
- ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
-
- return true;
- }
-
- void fillWith(osg::Drawable* drawable)
- {
- bool oldSphIsec = sphIsec;
- if (!enterBoundingSphere(drawable->getBound()))
- return;
-
- bool oldBackfaceCulling = mBackfaceCulling;
- updateCullMode(drawable->getStateSet());
-
- drawable->accept(mTriangleFunctor);
-
- mBackfaceCulling = oldBackfaceCulling;
- sphIsec = oldSphIsec;
- }
-
- virtual void apply(osg::Geode& geode)
- {
- bool oldBackfaceCulling = mBackfaceCulling;
- bool oldSphIsec = sphIsec;
- FGGroundCache::GroundProperty oldGp = mGroundProperty;
- if (!enterNode(geode))
- return;
-
- for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
- fillWith(geode.getDrawable(i));
- sphIsec = oldSphIsec;
- mGroundProperty = oldGp;
- mBackfaceCulling = oldBackfaceCulling;
- }
-
- virtual void apply(osg::Group& group)
- {
- bool oldBackfaceCulling = mBackfaceCulling;
- bool oldSphIsec = sphIsec;
- FGGroundCache::GroundProperty oldGp = mGroundProperty;
- if (!enterNode(group))
- return;
- traverse(group);
- sphIsec = oldSphIsec;
- mBackfaceCulling = oldBackfaceCulling;
- mGroundProperty = oldGp;
- }
-
- virtual void apply(osg::Transform& transform)
- {
- if (!enterNode(transform))
- return;
- bool oldBackfaceCulling = mBackfaceCulling;
- bool oldSphIsec = sphIsec;
- FGGroundCache::GroundProperty oldGp = mGroundProperty;
- /// transform the caches center to local coords
- osg::Matrix oldLocalToGlobal = mLocalToGlobal;
- transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
-
- // walk the children
- traverse(transform);
-
- // Restore that one
- mLocalToGlobal = oldLocalToGlobal;
- sphIsec = oldSphIsec;
- mBackfaceCulling = oldBackfaceCulling;
- mGroundProperty = oldGp;
- }
-
- void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
- const osg::Vec3& v3)
- {
- FGGroundCache::Triangle t;
- osg::Vec3d gv1 = osg::Vec3d(v1)*mLocalToGlobal;
- osg::Vec3d gv2 = osg::Vec3d(v2)*mLocalToGlobal;
- osg::Vec3d gv3 = osg::Vec3d(v3)*mLocalToGlobal;
- for (unsigned i = 0; i < 3; ++i) {
- t.vertices[0][i] = gv1[i];
- t.vertices[1][i] = gv2[i];
- t.vertices[2][i] = gv3[i];
- }
- // FIXME: can do better ...
- t.boundCenter = (1.0/3)*(t.vertices[0] + t.vertices[1] + t.vertices[2]);
- t.boundRadius = std::max(length(t.vertices[0] - t.boundCenter),
- length(t.vertices[1] - t.boundCenter));
- t.boundRadius = std::max(t.boundRadius,
- length(t.vertices[2] - t.boundCenter));
-
- sgdMakePlane(t.plane.sg(), t.vertices[0].sg(), t.vertices[1].sg(),
- t.vertices[2].sg());
- double d = sgdScalarProductVec3(mDown.sg(), t.plane.sg());
- if (d > 0) {
- if (mBackfaceCulling) {
- // Surface points downwards, ignore for altitude computations.
- return;
- } else
- t.plane = -t.plane;
- }
-
- // Check if the sphere around the vehicle intersects the sphere
- // around that triangle. If so, put that triangle into the cache.
- if (sphIsec &&
- distSqr(t.boundCenter, mCacheReference)
- < (t.boundRadius + mCacheRadius)*(t.boundRadius + mCacheRadius) ) {
- t.velocity = mGroundProperty.vel;
- t.rotation = mGroundProperty.rot;
- t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
- t.type = mGroundProperty.type;
- mGroundCache->triangles.push_back(t);
- }
-
- // In case the cache is empty, we still provide agl computations.
- // But then we use the old way of having a fixed elevation value for
- // the whole lifetime of this cache.
- if ( fgdIsectSphereInfLine(t.boundCenter, t.boundRadius,
- mCacheReference, mDown) ) {
- SGVec3d isectpoint;
- if ( sgdIsectInfLinePlane( isectpoint.sg(), mCacheReference.sg(),
- mDown.sg(), t.plane.sg() ) &&
- fgdPointInTriangle( isectpoint, t.vertices ) ) {
- // Only accept the altitude if the intersection point is below the
- // ground cache midpoint
- if (0 < dot(isectpoint - mCacheReference, mDown)) {
- mGroundCache->found_ground = true;
- isectpoint += mGroundCache->cache_center;
- double this_radius = length(isectpoint);
- if (mGroundCache->ground_radius < this_radius)
- mGroundCache->ground_radius = this_radius;
+ // If we have an active wire, get some more area into the groundcache
+ if (_wire)
+ rad = SGMiscd::max(200, rad);
+
+ // Store the parameters we used to build up that cache.
+ reference_wgs84_point = pt;
+ reference_vehicle_radius = rad;
+ // Store the time reference used to compute movements of moving triangles.
+ 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
+ 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();
+
+ 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 (!lineSegmentVisitor.empty()) {
+ SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
+ _altitude = geodPt.getElevationM();
+ _material = lineSegmentVisitor.getMaterial();
+ found_ground = true;