X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=0b341159aadd04bae91e51e22971dc989d2fd461;hb=2314ccfe13e5e175763000e4c8ba86be79aa3a97;hp=90e79b8643b21cbd3d70ac0d196765e26c818667;hpb=fc35624a4fd06753cb370b9601d7fd65269ae0d0;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 90e79b864..0b341159a 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -2,7 +2,7 @@ // // Written by Mathias Froehlich, started Nov 2004. // -// Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de +// Copyright (C) 2004, 2009 Mathias Froehlich - Mathias.Froehlich@web.de // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as @@ -24,652 +24,1029 @@ # include "config.h" #endif -#include +#include "groundcache.hxx" #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 +#include +#include + +#ifdef GROUNDCACHE_DEBUG +#include +#include
+#endif #include
#include #include -#include #include "flight.hxx" -#include "groundcache.hxx" -using namespace osg; -using namespace osgUtil; using namespace simgear; -void makePolytopeShaft(Polytope& polyt, const Vec3d& refPoint, - const Vec3d& direction, double radius) -{ - polyt.clear(); - // Choose best principal axis to start making orthogonal axis. - Vec3d majorAxis; - if (fabs(direction.x()) <= fabs(direction.y())) { - if (fabs(direction.z()) <= fabs(direction.x())) - majorAxis = Vec3d(0.0, 0.0, 1.0); - else - majorAxis = Vec3d(1.0, 0.0, 0.0); - } else { - if (fabs(direction.z()) <= fabs(direction.y())) - majorAxis = Vec3d(0.0, 0.0, 1.0); - else - majorAxis = Vec3d(0.0, 1.0, 0.0); - } - Vec3d axis1 = majorAxis ^ direction; - axis1.normalize(); - Vec3d axis2 = direction ^ axis1; - - polyt.add(Plane(-axis1, refPoint + axis1 * radius)); - polyt.add(Plane(axis1, refPoint - axis1 * radius)); - polyt.add(Plane(-axis2, refPoint + axis2 * radius)); - polyt.add(Plane(axis2 , refPoint - axis2 * radius)); -} - -void makePolytopeBox(Polytope& polyt, const Vec3d& center, - const Vec3d& direction, double radius) -{ - makePolytopeShaft(polyt, center, direction, radius); - polyt.add(Plane(-direction, center + direction * radius)); - polyt.add(Plane(direction, center - direction * radius)); -} - -// Intersector for finding catapults and wires - -class WireIntersector : public PolytopeIntersector -{ +class FGGroundCache::CacheFill : public osg::NodeVisitor { public: - WireIntersector(const Polytope& polytope) - : PolytopeIntersector(polytope), depth(0) + 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) { - setDimensionMask(DimOne); + setTraversalMask(SG_NODEMASK_TERRAIN_BIT); } + virtual void apply(osg::Node& node) + { + if (!testBoundingSphere(node.getBound())) + return; - bool enter(const osg::Node& node) + addBoundingVolume(node); + } + + virtual void apply(osg::Group& group) { - if (!PolytopeIntersector::enter(node)) - return false; - const Referenced* base = node.getUserData(); - if (base) { - const FGAICarrierHardware *ud - = dynamic_cast(base); - if (ud) - carriers.push_back(depth); + 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 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); } - depth++; - return true; + + 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; } - void leave() + const SGSceneUserData::Velocity* getVelocity(osg::Node& node) { - depth--; - if (!carriers.empty() && depth == carriers.back()) - carriers.pop_back(); + SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node); + if (!userData) + return 0; + return userData->getVelocity(); } - - void intersect(IntersectionVisitor& iv, Drawable* drawable) + simgear::BVHNode* getNodeBoundingVolume(osg::Node& node) { - if (carriers.empty()) - return; - PolytopeIntersector::intersect(iv, drawable); + 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; + } - void reset() + // 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 { - carriers.clear(); + 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; } - std::vector carriers; - int depth; + + 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; }; -/// Ok, variant that uses a infinite line istead of the ray. -/// also not that this only works if the ray direction is normalized. -static inline bool -intersectsInf(const SGRayd& ray, const SGSphered& sphere) -{ - SGVec3d r = sphere.getCenter() - ray.getOrigin(); - double projectedDistance = dot(r, ray.getDirection()); - double dist = dot(r, r) - projectedDistance * projectedDistance; - return dist < sphere.getRadius2(); -} - FGGroundCache::FGGroundCache() : - ground_radius(0.0), - _type(0), - _material(0), - cache_ref_time(0.0), - wire_id(0), - reference_wgs84_point(SGVec3d(0, 0, 0)), - reference_vehicle_radius(0.0), - down(0.0, 0.0, 0.0), - found_ground(false) + _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() { } -inline void -FGGroundCache::velocityTransformTriangle(double dt, - SGTriangled& dst, SGSphered& sdst, - const FGGroundCache::Triangle& src) +bool +FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime, + const SGVec3d& pt, double rad) { - dst = src.triangle; - sdst = src.sphere; - - if (dt*dt*dot(src.gp.vel, src.gp.vel) < SGLimitsd::epsilon()) - return; - - SGVec3d baseVert = dst.getBaseVertex(); - SGVec3d pivotoff = baseVert - src.gp.pivot; - baseVert += dt*(src.gp.vel + cross(src.gp.rot, pivotoff)); - dst.setBaseVertex(baseVert); - dst.setEdge(0, dst.getEdge(0) + dt*cross(src.gp.rot, dst.getEdge(0))); - dst.setEdge(1, dst.getEdge(1) + dt*cross(src.gp.rot, dst.getEdge(1))); -} +#ifdef GROUNDCACHE_DEBUG + SGTimeStamp t0 = SGTimeStamp::now(); +#endif -void FGGroundCache::getGroundProperty(Drawable* drawable, - const NodePath& nodePath, - FGGroundCache::GroundProperty& gp, - bool& backfaceCulling) -{ - gp.type = FGInterface::Unknown; - gp.wire_id = 0; - gp.vel = SGVec3d(0.0, 0.0, 0.0); - gp.rot = SGVec3d(0.0, 0.0, 0.0); - gp.pivot = SGVec3d(0.0, 0.0, 0.0); - gp.material = 0; - backfaceCulling = false; - // XXX state set might be higher up in scene graph - gp.material = SGMaterialLib::findMaterial(drawable->getStateSet()); - if (gp.material) - gp.type = (gp.material->get_solid() ? FGInterface::Solid - : FGInterface::Water); - for (NodePath::const_iterator iter = nodePath.begin(), e = nodePath.end(); - iter != e; - ++iter) { - Node* node = *iter; - StateSet* stateSet = node->getStateSet(); - StateAttribute* stateAttribute = 0; - if (stateSet && (stateAttribute - = stateSet->getAttribute(StateAttribute::CULLFACE))) { - backfaceCulling - = (static_cast(stateAttribute)->getMode() - == CullFace::BACK); + // 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()->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; + } + _material = 0; + + // 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; } - - // get some material information for use in the gear model - Referenced* base = node->getUserData(); - if (!base) - continue; - FGAICarrierHardware *ud = dynamic_cast(base); - if (!ud) - continue; - 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; + } + + if (!found_ground) { + // Ok, still nothing here?? Last resort ... + double alt = 0; + _material = 0; + found_ground = globals->get_scenery()-> + get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material); + if (found_ground) + _altitude = alt; + } + + // Still not sucessful?? + if (!found_ground) + 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()); } - // Copy the velocity from the carrier class. - ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot); - break; } +#endif + + return found_ground; } -void FGGroundCache::getTriIntersectorResults(PolytopeIntersector* triInt) +bool +FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) { - const PolytopeIntersector::Intersections& intersections - = triInt->getIntersections(); - Drawable* lastDrawable = 0; - RefMatrix* lastMatrix = 0; - Matrix worldToLocal; - GroundProperty gp; - bool backfaceCulling = false; - for (PolytopeIntersector::Intersections::const_iterator - itr = intersections.begin(), e = intersections.end(); - itr != e; - ++itr) { - const PolytopeIntersector::Intersection& intr = *itr; - if (intr.drawable.get() != lastDrawable) { - getGroundProperty(intr.drawable.get(), intr.nodePath, gp, - backfaceCulling); - lastDrawable = intr.drawable.get(); - } - Primitive triPrim = getPrimitive(intr.drawable, intr.primitiveIndex); - if (triPrim.numVerts != 3) - continue; - SGVec3d v[3] = { SGVec3d(triPrim.vertices[0]), - SGVec3d(triPrim.vertices[1]), - SGVec3d(triPrim.vertices[2]) - }; - RefMatrix* mat = intr.matrix.get(); - // If the drawable is the same then the intersection model - // matrix should be the same, because it is only set by nodes - // in the scene graph. However, do an extra test in case - // something funny is going on with the drawable. - if (mat != lastMatrix) { - lastMatrix = mat; - worldToLocal = Matrix::inverse(*mat); + pt = reference_wgs84_point; + rad = reference_vehicle_radius; + ref_time = cache_ref_time; + return found_ground; +} + +class FGGroundCache::BodyFinder : public BVHVisitor { +public: + BodyFinder(BVHNode::Id id, const double& t) : + _id(id), + _bodyToWorld(SGMatrixd::unit()), + _linearVelocity(0, 0, 0), + _angularVelocity(0, 0, 0), + _time(t) + { } + + virtual void apply(BVHGroup& leaf) + { + if (_foundId) + return; + leaf.traverse(*this); + } + virtual void apply(BVHPageNode& leaf) + { + if (_foundId) + return; + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (_foundId) + return; + + transform.traverse(*this); + + if (_foundId) { + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld; } - SGVec3d localCacheReference; - localCacheReference.osg() = reference_wgs84_point.osg() * worldToLocal; - SGVec3d localDown; - localDown.osg() = Matrixd::transform3x3(down.osg(), worldToLocal); - // 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(localCacheReference, localDown); - 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(localDown, n)) { - if (backfaceCulling) { - // Surface points downwards, ignore for altitude computations. - continue; - } else { - triangle.flip(); - } + } + virtual void apply(BVHMotionTransform& transform) + { + if (_foundId) + return; + + if (_id == transform.getId()) { + _foundId = true; + } else { + transform.traverse(*this); } - - // Only check if the triangle is in the cache sphere if the plane - // containing the triangle is near enough - double d = dot(n, v[0] - localCacheReference); - if (d*d < reference_vehicle_radius*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 + reference_vehicle_radius; - if (distSqr(boundCenter, localCacheReference) < r2*r2) { - FGGroundCache::Triangle t; - t.triangle.setBaseVertex(SGVec3d(v[0].osg() * *mat)); - t.triangle.setEdge(0, SGVec3d(Matrixd:: - transform3x3(triangle - .getEdge(0).osg(), - *mat))); - t.triangle.setEdge(1, SGVec3d(Matrixd:: - transform3x3(triangle - .getEdge(1).osg(), - *mat))); - t.sphere.setCenter(SGVec3d(boundCenter.osg()* *mat)); - t.sphere.setRadius(boundRadius); - t.gp = gp; - triangles.push_back(t); - } + + if (_foundId) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros()); + _linearVelocity += transform.getLinearVelocityAt(referencePoint); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _bodyToWorld = toWorld*_bodyToWorld; } - // 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. - SGVec3d isectpoint; - if (intersects(isectpoint, triangle, ray, 1e-4)) { - found_ground = true; - isectpoint.osg() = isectpoint.osg() * *mat; - double this_radius = length(isectpoint); - if (ground_radius < this_radius) { - ground_radius = this_radius; - _type = gp.type; - _material = gp.material; - } - } } -} + virtual void apply(BVHLineGeometry& node) { } + virtual void apply(BVHStaticGeometry& node) { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + const SGMatrixd& getBodyToWorld() const + { return _bodyToWorld; } + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + + bool empty() const + { return !_foundId; } + +protected: + simgear::BVHNode::Id _id; + + SGMatrixd _bodyToWorld; + + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _foundId; + + double _time; +}; -void FGGroundCache::getWireIntersectorResults(WireIntersector* wireInt, - double wireCacheRadius) +bool +FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel, + SGVec3d& angularVel, simgear::BVHNode::Id id) { - const WireIntersector::Intersections& intersections - = wireInt->getIntersections(); - Drawable* lastDrawable = 0; - GroundProperty gp; - bool backfaceCulling = false; - for (PolytopeIntersector::Intersections::const_iterator - itr = intersections.begin(), e = intersections.end(); - itr != e; - ++itr) { - if (itr->drawable.get() != lastDrawable) { - getGroundProperty(itr->drawable.get(), itr->nodePath, gp, - backfaceCulling); - lastDrawable = itr->drawable.get(); - } - Primitive linePrim = getPrimitive(itr->drawable, itr->primitiveIndex); - if (linePrim.numVerts != 2) - continue; - RefMatrix* mat = itr->matrix.get(); - SGVec3d gv1(osg::Vec3d(linePrim.vertices[0]) * *mat); - SGVec3d gv2(osg::Vec3d(linePrim.vertices[1]) * *mat); - - SGVec3d boundCenter = 0.5*(gv1 + gv2); - double boundRadius = length(gv1 - boundCenter); - - if (distSqr(boundCenter, reference_wgs84_point) - < (boundRadius + wireCacheRadius)*(boundRadius + wireCacheRadius)) { - if (gp.type == FGInterface::Wire) { - FGGroundCache::Wire wire; - wire.ends[0] = gv1; - wire.ends[1] = gv2; - wire.gp = gp; - wires.push_back(wire); - } else if (gp.type == FGInterface::Catapult) { - FGGroundCache::Catapult cat; - // Trick to get the ends in the right order. - // Use the x axis in the original coordinate system. Choose the - // most negative x-axis as the one pointing forward - if (linePrim.vertices[0][0] > linePrim.vertices[1][0]) { - cat.start = gv1; - cat.end = gv2; - } else { - cat.start = gv2; - cat.end = gv1; - } - cat.gp = gp; - catapults.push_back(cat); - } + // 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()) + return false; + + bodyToWorld = bodyFinder.getBodyToWorld(); + linearVel = bodyFinder.getLinearVelocity(); + angularVel = bodyFinder.getAngularVelocity(); + + return true; +} + +class FGGroundCache::CatapultFinder : public BVHVisitor { +public: + CatapultFinder(const SGSphered& sphere, const double& t) : + _haveLineSegment(false), + _sphere(sphere), + _time(t) + { } + + virtual void apply(BVHGroup& leaf) + { + if (!intersects(_sphere, leaf.getBoundingSphere())) + 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())) + return; + + SGSphered sphere = _sphere; + _sphere = transform.sphereToLocal(sphere); + bool haveLineSegment = _haveLineSegment; + _haveLineSegment = false; + + transform.traverse(*this); + + if (_haveLineSegment) { + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); } + _haveLineSegment |= haveLineSegment; + _sphere.setCenter(sphere.getCenter()); + } + virtual void apply(BVHMotionTransform& transform) + { + if (!intersects(_sphere, transform.getBoundingSphere())) + return; + + SGSphered sphere = _sphere; + _sphere = transform.sphereToLocal(sphere, _time); + bool haveLineSegment = _haveLineSegment; + _haveLineSegment = false; + + transform.traverse(*this); + if (_haveLineSegment) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + _haveLineSegment |= haveLineSegment; + _sphere.setCenter(sphere.getCenter()); } + virtual void apply(BVHLineGeometry& node) + { + if (node.getType() != BVHLineGeometry::CarrierCatapult) + return; + + SGLineSegmentd lineSegment(node.getLineSegment()); + if (!intersects(_sphere, lineSegment)) + return; + + _lineSegment = lineSegment; + double dist = distSqr(lineSegment, getSphere().getCenter()); + _sphere.setRadius(sqrt(dist)); + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _haveLineSegment = true; + } + virtual void apply(BVHStaticGeometry& node) + { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + void setSphere(const SGSphered& sphere) + { _sphere = sphere; } + const SGSphered& getSphere() const + { return _sphere; } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + + bool getHaveLineSegment() const + { return _haveLineSegment; } + +protected: + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _haveLineSegment; + + SGSphered _sphere; + double _time; +}; + +double +FGGroundCache::get_cat(double t, const SGVec3d& pt, + SGVec3d end[2], SGVec3d vel[2]) +{ + double maxDistance = 1000; + + // Get the wire in question + t += cache_time_offset; + CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t); + if (_localBvhTree) + _localBvhTree->accept(catapultFinder); + + if (!catapultFinder.getHaveLineSegment()) + return maxDistance; + + // prepare the returns + end[0] = catapultFinder.getLineSegment().getStart(); + end[1] = catapultFinder.getLineSegment().getEnd(); + + // The linear velocity is the one at the start of the line segment ... + vel[0] = catapultFinder.getLinearVelocity(); + // ... so the end point has the additional cross product. + vel[1] = catapultFinder.getLinearVelocity(); + vel[1] += cross(catapultFinder.getAngularVelocity(), + catapultFinder.getLineSegment().getDirection()); + + // Return the distance to the cat + return sqrt(distSqr(catapultFinder.getLineSegment(), pt)); } bool -FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, - double rad) +FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact, + SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel, + simgear::BVHNode::Id& id, const simgear::BVHMaterial*& material) { - // 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.getLatitudeDeg(), - geodPt.getLongitudeDeg(), - rad)) - return false; - ground_radius = 0.0; - triangles.resize(0); - catapults.resize(0); - wires.resize(0); - - // 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 = ref_time; - - // Get a normalized down vector valid for the whole cache - SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt); - down = hlToEc.rotate(SGVec3d(0, 0, 1)); - - // Prepare sphere around the aircraft. - double cacheRadius = rad; - - // Prepare bigger sphere around the aircraft. - // This one is required for reliably finding wires we have caught but - // have already left the hopefully smaller sphere for the ground reactions. - const double max_wire_dist = 300.0; - double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist; - - Polytope triPolytope; - makePolytopeShaft(triPolytope, pt.osg(), down.osg(), cacheRadius); - ref_ptr triIntersector - = new PolytopeIntersector(triPolytope); - triIntersector->setDimensionMask(PolytopeIntersector::DimTwo); - Polytope wirePolytope; - makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius); - ref_ptr wireIntersector = new WireIntersector(wirePolytope); - wireIntersector->setDimensionMask(PolytopeIntersector::DimOne); - ref_ptr intersectors = new IntersectorGroup; - intersectors->addIntersector(triIntersector.get()); - intersectors->addIntersector(wireIntersector.get()); - - // Walk the scene graph and extract solid ground triangles and - // carrier data. - IntersectionVisitor iv(intersectors); - iv.setTraversalMask(SG_NODEMASK_TERRAIN_BIT); - globals->get_scenery()->get_scene_graph()->accept(iv); - getTriIntersectorResults(triIntersector.get()); - getWireIntersectorResults(wireIntersector.get(), wireCacheRadius); - - // some stats - SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad - << ", # triangles = " << triangles.size() - << ", # wires = " << wires.size() - << ", # catapults = " << catapults.size() - << ", ground_radius = " << ground_radius ); - - // If the ground radius is still below 5e6 meters, then we do not yet have - // any scenery. - found_ground = found_ground && 5e6 < ground_radius; - if (!found_ground) - SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache " - "without any scenery below the aircraft" ); - - return found_ground; +#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(); + normal = lineSegmentVisitor.getNormal(); + if (0 < dot(normal, down)) + normal = -normal; + linearVel = lineSegmentVisitor.getLinearVelocity(); + angularVel = lineSegmentVisitor.getAngularVelocity(); + material = lineSegmentVisitor.getMaterial(); + id = lineSegmentVisitor.getId(); + + return true; + } else { + // Whenever we did not have a ground triangle for the requested point, + // take the ground level we found during the current cache build. + // This is as good as what we had before for agl. + SGGeod geodPt = SGGeod::fromCart(pt); + geodPt.setElevationM(_altitude); + contact = SGVec3d::fromGeod(geodPt); + normal = -down; + linearVel = SGVec3d(0, 0, 0); + angularVel = SGVec3d(0, 0, 0); + material = _material; + id = 0; + + return found_ground; + } } + bool -FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) +FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist, + SGVec3d& contact, SGVec3d& linearVel, + SGVec3d& angularVel, simgear::BVHNode::Id& id, + const simgear::BVHMaterial*& material) { - pt = reference_wgs84_point; - rad = reference_vehicle_radius; - ref_time = cache_ref_time; - return found_ground; + 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; + + // Have geometry in the range of maxDist + contact = nearestPointVisitor.getPoint(); + linearVel = nearestPointVisitor.getLinearVelocity(); + angularVel = nearestPointVisitor.getAngularVelocity(); + material = nearestPointVisitor.getMaterial(); + id = nearestPointVisitor.getId(); + + return true; } -double -FGGroundCache::get_cat(double t, const SGVec3d& dpt, - SGVec3d end[2], SGVec3d vel[2]) -{ - // start with a distance of 1e10 meters... - double dist = 1e10; - - // Time difference to the reference time. - t -= cache_ref_time; - - size_t sz = catapults.size(); - for (size_t i = 0; i < sz; ++i) { - SGVec3d pivotoff, rvel[2]; - pivotoff = catapults[i].start - catapults[i].gp.pivot; - rvel[0] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff); - pivotoff = catapults[i].end - catapults[i].gp.pivot; - rvel[1] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff); - - SGVec3d thisEnd[2]; - thisEnd[0] = catapults[i].start + t*rvel[0]; - thisEnd[1] = catapults[i].end + t*rvel[1]; - - double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt); - if (this_dist < dist) { - SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult " - << this_dist << " meters away"); - dist = this_dist; + +class FGGroundCache::WireIntersector : public BVHVisitor { +public: + WireIntersector(const SGVec3d pt[4], const double& t) : + _linearVelocity(SGVec3d::zeros()), + _angularVelocity(SGVec3d::zeros()), + _wire(0), + _time(t) + { + // Build the two triangles spanning the area where the hook has moved + // during the past step. + _triangles[0].set(pt[0], pt[1], pt[2]); + _triangles[1].set(pt[0], pt[2], pt[3]); + } + + virtual void apply(BVHGroup& leaf) + { + if (!_intersects(leaf.getBoundingSphere())) + return; + + 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())) + return; + + SGTriangled triangles[2] = { _triangles[0], _triangles[1] }; + _triangles[0] = triangles[0].transform(transform.getToLocalTransform()); + _triangles[1] = triangles[1].transform(transform.getToLocalTransform()); + + transform.traverse(*this); - end[0] = thisEnd[0]; - end[1] = thisEnd[1]; - vel[0] = rvel[0]; - vel[1] = rvel[1]; + if (_wire) { + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + } + _triangles[0] = triangles[0]; + _triangles[1] = triangles[1]; } - } + virtual void apply(BVHMotionTransform& transform) + { + if (!_intersects(transform.getBoundingSphere())) + return; + + SGMatrixd toLocal = transform.getToLocalTransform(_time); - // At the end take the root, we only computed squared distances ... - return sqrt(dist); -} + SGTriangled triangles[2] = { _triangles[0], _triangles[1] }; + _triangles[0] = triangles[0].transform(toLocal); + _triangles[1] = triangles[1].transform(toLocal); + + transform.traverse(*this); + + if (_wire) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + _triangles[0] = triangles[0]; + _triangles[1] = triangles[1]; + } + virtual void apply(BVHLineGeometry& node) + { + if (node.getType() != BVHLineGeometry::CarrierWire) + return; + SGLineSegmentd lineSegment(node.getLineSegment()); + if (!_intersects(lineSegment)) + return; -bool -FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, - SGVec3d& contact, SGVec3d& normal, SGVec3d& vel, - int *type, const SGMaterial** material, double *agl) -{ - bool ret = false; - - *type = FGInterface::Unknown; -// *agl = 0.0; - if (material) - *material = 0; - vel = SGVec3d(0, 0, 0); - contact = SGVec3d(0, 0, 0); - normal = SGVec3d(0, 0, 0); - - // Time difference to th reference time. - t -= cache_ref_time; - - // The double valued point we start to search for intersection. - SGVec3d pt = dpt; - // shift the start of our ray by maxaltoff upwards - SGRayd ray(pt - max_altoff*down, down); - - // Initialize to something sensible - double current_radius = 0.0; - - size_t sz = triangles.size(); - for (size_t i = 0; i < sz; ++i) { - SGSphered sphere; - SGTriangled triangle; - velocityTransformTriangle(t, triangle, sphere, triangles[i]); - if (!intersectsInf(ray, sphere)) - continue; - - // Check for intersection. - SGVec3d isecpoint; - if (intersects(isecpoint, triangle, ray, 1e-4)) { - // Compute the vector from pt to the intersection point ... - SGVec3d off = isecpoint - pt; - // ... and check if it is too high or not - - // compute the radius, good enough approximation to take the geocentric radius - double radius = dot(isecpoint, isecpoint); - if (current_radius < radius) { - current_radius = radius; - ret = true; - // Save the new potential intersection point. - contact = isecpoint; - // The first three values in the vector are the plane normal. - normal = triangle.getNormal(); - // The velocity wrt earth. - SGVec3d pivotoff = pt - triangles[i].gp.pivot; - vel = triangles[i].gp.vel + cross(triangles[i].gp.rot, pivotoff); - // Save the ground type. - *type = triangles[i].gp.type; - *agl = dot(down, contact - dpt); - if (material) - *material = triangles[i].gp.material; - } - } - } - - if (ret) - return true; + _lineSegment = lineSegment; + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _wire = &node; + } + virtual void apply(BVHStaticGeometry& node) + { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } - // Whenever we did not have a ground triangle for the requested point, - // take the ground level we found during the current cache build. - // This is as good as what we had before for agl. - double r = length(dpt); - contact = dpt; - contact *= ground_radius/r; - normal = -down; - vel = SGVec3d(0, 0, 0); - - // The altitude is the distance of the requested point from the - // contact point. - *agl = dot(down, contact - dpt); - *type = _type; - if (material) - *material = _material; - - return ret; -} + bool _intersects(const SGSphered& sphere) const + { + if (_wire) + return false; + if (intersects(_triangles[0], sphere)) + return true; + if (intersects(_triangles[1], sphere)) + return true; + return false; + } + bool _intersects(const SGLineSegmentd& lineSegment) const + { + if (_wire) + return false; + if (intersects(_triangles[0], lineSegment)) + return true; + if (intersects(_triangles[1], lineSegment)) + return true; + return false; + } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + + const BVHLineGeometry* getWire() const + { return _wire; } + +private: + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + const BVHLineGeometry* _wire; + + SGTriangled _triangles[2]; + double _time; +}; bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4]) { - size_t sz = wires.size(); - if (sz == 0) - return false; - - // Time difference to the reference time. - t -= cache_ref_time; - - // Build the two triangles spanning the area where the hook has moved - // during the past step. - SGTriangled triangle[2]; - triangle[0].set(pt[0], pt[1], pt[2]); - triangle[1].set(pt[0], pt[2], pt[3]); - - // Intersect the wire lines with each of these triangles. - // You have caught a wire if they intersect. - for (size_t i = 0; i < sz; ++i) { - SGVec3d le[2]; - for (int k = 0; k < 2; ++k) { - le[k] = wires[i].ends[k]; - SGVec3d pivotoff = le[k] - wires[i].gp.pivot; - SGVec3d vel = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff); - le[k] += t*vel; - } - SGLineSegmentd lineSegment(le[0], le[1]); - - for (int k=0; k<2; ++k) { - if (intersects(triangle[k], lineSegment)) { - SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire"); - // Store the wire id. - wire_id = wires[i].gp.wire_id; - return true; - } + // Get the wire in question + t += cache_time_offset; + WireIntersector wireIntersector(pt, t); + if (_localBvhTree) + _localBvhTree->accept(wireIntersector); + + _wire = wireIntersector.getWire(); + return (_wire != NULL); +} + +class FGGroundCache::WireFinder : public BVHVisitor { +public: + WireFinder(const BVHLineGeometry* wire, const double& t) : + _wire(wire), + _time(t), + _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()), + _linearVelocity(SGVec3d::zeros()), + _angularVelocity(SGVec3d::zeros()), + _haveLineSegment(false) + { } + + virtual void apply(BVHGroup& leaf) + { + if (_haveLineSegment) + return; + leaf.traverse(*this); } - } + virtual void apply(BVHPageNode& leaf) + { + if (_haveLineSegment) + return; + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (_haveLineSegment) + return; - return false; -} + transform.traverse(*this); + + if (_haveLineSegment) { + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + } + } + virtual void apply(BVHMotionTransform& transform) + { + if (_haveLineSegment) + return; + + transform.traverse(*this); + + if (_haveLineSegment) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + } + virtual void apply(BVHLineGeometry& node) + { + if (_haveLineSegment) + return; + if (_wire != &node) + return; + if (node.getType() != BVHLineGeometry::CarrierWire) + return; + _lineSegment = SGLineSegmentd(node.getLineSegment()); + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _haveLineSegment = true; + } + virtual void apply(BVHStaticGeometry&) { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + + bool getHaveLineSegment() const + { return _haveLineSegment; } + + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + +private: + const BVHLineGeometry* _wire; + double _time; + + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _haveLineSegment; +}; bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2]) { - // Fast return if we do not have an active wire. - if (wire_id < 0) - return false; - - // Time difference to the reference time. - t -= cache_ref_time; - - // Search for the wire with the matching wire id. - size_t sz = wires.size(); - for (size_t i = 0; i < sz; ++i) { - if (wires[i].gp.wire_id == wire_id) { - for (size_t k = 0; k < 2; ++k) { - SGVec3d pivotoff = wires[i].ends[k] - wires[i].gp.pivot; - vel[k] = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff); - end[k] = wires[i].ends[k] + t*vel[k]; - } - return true; - } - } - - return false; + // Fast return if we do not have an active wire. + if (!_wire) + return false; + + // Get the wire in question + t += cache_time_offset; + WireFinder wireFinder(_wire, t); + if (_localBvhTree) + _localBvhTree->accept(wireFinder); + + if (!wireFinder.getHaveLineSegment()) + return false; + + // prepare the returns + end[0] = wireFinder.getLineSegment().getStart(); + end[1] = wireFinder.getLineSegment().getEnd(); + + // The linear velocity is the one at the start of the line segment ... + vel[0] = wireFinder.getLinearVelocity(); + // ... so the end point has the additional cross product. + vel[1] = wireFinder.getLinearVelocity(); + vel[1] += cross(wireFinder.getAngularVelocity(), + wireFinder.getLineSegment().getDirection()); + + return true; } void FGGroundCache::release_wire(void) { - wire_id = -1; + _wire = 0; }