- 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;
- }
- gp.type = FGInterface::Unknown;
- 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;
- osg::Matrix oldGlobalToLocal = mGlobalToLocal;
- transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
- transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
-
- SGVec3d oldLocalCacheReference = mLocalCacheReference;
- mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
- SGVec3d oldLocalDown = mLocalDown;
- mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
-
- // walk the children
- traverse(transform);
-
- // Restore that one
- mLocalDown = oldLocalDown;
- mLocalCacheReference = oldLocalCacheReference;
- mLocalToGlobal = oldLocalToGlobal;
- mGlobalToLocal = oldGlobalToLocal;
- sphIsec = oldSphIsec;
- mBackfaceCulling = oldBackfaceCulling;
- mGroundProperty = oldGp;
- }
-
- void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
- const osg::Vec3& v3)
- {
- SGVec3d v[3] = {
- SGVec3d(v1),
- SGVec3d(v2),
- SGVec3d(v3)
- };
-
- // a bounding sphere in the node local system
- SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
- double boundRadius = std::max(distSqr(v[0], boundCenter),
- distSqr(v[1], boundCenter));
- boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
- boundRadius = sqrt(boundRadius);
-
- SGRayd ray(mLocalCacheReference, mLocalDown);
-
- // if we are not in the downward cylinder bail out
- if (!intersectsInf(ray, SGSphered(boundCenter, boundRadius + mCacheRadius)))
- return;
-
- SGTriangled triangle(v);
-
- // The normal and plane in the node local coordinate system
- SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
- if (0 < dot(mLocalDown, n)) {
- if (mBackfaceCulling) {
- // Surface points downwards, ignore for altitude computations.
- return;
- } else {
- triangle.flip();
- }
- }
-
- // Only check if the triangle is in the cache sphere if the plane
- // containing the triangle is near enough
- if (sphIsec) {
- double d = dot(n, v[0] - mLocalCacheReference);
- if (d*d < mCacheRadius*dot(n, n)) {
- // Check if the sphere around the vehicle intersects the sphere
- // around that triangle. If so, put that triangle into the cache.
- double r2 = boundRadius + mCacheRadius;
- if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
- FGGroundCache::Triangle t;
- t.triangle.setBaseVertex(SGVec3d(v[0].osg()*mLocalToGlobal));
- t.triangle.setEdge(0, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(0).osg(), mLocalToGlobal)));
- t.triangle.setEdge(1, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(1).osg(), mLocalToGlobal)));
-
- t.sphere.setCenter(SGVec3d(boundCenter.osg()*mLocalToGlobal));
- t.sphere.setRadius(boundRadius);
-
- t.velocity = mGroundProperty.vel;
- t.rotation = mGroundProperty.rot;
- t.rotation_pivot = mGroundProperty.pivot;
- t.type = mGroundProperty.type;
- t.material = mGroundProperty.material;
- mGroundCache->triangles.push_back(t);
+ 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 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);
+
+ 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);