src/FDM/groundcache.cxx

   1 // groundcache.cxx -- carries a small subset of the scenegraph near the vehicle
   2 //
   3 // Written by Mathias Froehlich, started Nov 2004.
   4 //
   5 // Copyright (C) 2004  Mathias Froehlich - Mathias.Froehlich@web.de
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include "config.h"
  25 #endif
  26
  27 #include <utility>
  28
  29 #include <osg/Drawable>
  30 #include <osg/Geode>
  31 #include <osg/Geometry>
  32 #include <osg/Camera>
  33 #include <osg/Transform>
  34 #include <osg/MatrixTransform>
  35 #include <osg/PositionAttitudeTransform>
  36 #include <osg/CameraView>
  37
  38 #include <simgear/sg_inlines.h>
  39 #include <simgear/constants.h>
  40 #include <simgear/debug/logstream.hxx>
  41 #include <simgear/math/sg_geodesy.hxx>
  42 #include <simgear/scene/material/mat.hxx>
  43 #include <simgear/scene/util/SGNodeMasks.hxx>
  44 #include <simgear/scene/util/SGSceneUserData.hxx>
  45 #include <simgear/scene/model/placementtrans.hxx>
  46
  47 #include <simgear/scene/bvh/BVHNode.hxx>
  48 #include <simgear/scene/bvh/BVHGroup.hxx>
  49 #include <simgear/scene/bvh/BVHTransform.hxx>
  50 #include <simgear/scene/bvh/BVHMotionTransform.hxx>
  51 #include <simgear/scene/bvh/BVHLineGeometry.hxx>
  52 #include <simgear/scene/bvh/BVHStaticGeometry.hxx>
  53 #include <simgear/scene/bvh/BVHStaticData.hxx>
  54 #include <simgear/scene/bvh/BVHStaticNode.hxx>
  55 #include <simgear/scene/bvh/BVHStaticLeaf.hxx>
  56 #include <simgear/scene/bvh/BVHStaticTriangle.hxx>
  57 #include <simgear/scene/bvh/BVHStaticBinary.hxx>
  58 #include <simgear/scene/bvh/BVHSubTreeCollector.hxx>
  59 #include <simgear/scene/bvh/BVHLineSegmentVisitor.hxx>
  60
  61 #include <Main/globals.hxx>
  62 #include <Scenery/scenery.hxx>
  63 #include <Scenery/tilemgr.hxx>
  64
  65 #include "flight.hxx"
  66 #include "groundcache.hxx"
  67
  68 using namespace simgear;
  69
  70 static FGInterface::GroundType
  71 materialToGroundType(const SGMaterial* material)
  72 {
  73     if (!material)
  74         return FGInterface::Solid;
  75     if (material->get_solid())
  76         return FGInterface::Solid;
  77     return FGInterface::Water;
  78 }
  79
  80 class FGGroundCache::CacheFill : public osg::NodeVisitor {
  81 public:
  82     CacheFill(const SGVec3d& center, const double& radius,
  83               const double& startTime, const double& endTime) :
  84         osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
  85         _center(center),
  86         _radius(radius),
  87         _startTime(startTime),
  88         _endTime(endTime)
  89     {
  90         setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
  91     }
  92     virtual void apply(osg::Node& node)
  93     {
  94         if (!testBoundingSphere(node.getBound()))
  95             return;
  96
  97         addBoundingVolume(node);
  98     }
  99
 100     virtual void apply(osg::Group& group)
 101     {
 102         if (!testBoundingSphere(group.getBound()))
 103             return;
 104
 105         simgear::BVHSubTreeCollector::NodeList parentNodeList;
 106         mSubTreeCollector.pushNodeList(parentNodeList);
 107
 108         traverse(group);
 109         addBoundingVolume(group);
 110
 111         mSubTreeCollector.popNodeList(parentNodeList);
 112     }
 113
 114     virtual void apply(osg::Transform& transform)
 115     { handleTransform(transform); }
 116     virtual void apply(osg::Camera& camera)
 117     {
 118         if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
 119             return;
 120         handleTransform(camera);
 121     }
 122     virtual void apply(osg::CameraView& transform)
 123     { handleTransform(transform); }
 124     virtual void apply(osg::MatrixTransform& transform)
 125     { handleTransform(transform); }
 126     virtual void apply(osg::PositionAttitudeTransform& transform)
 127     { handleTransform(transform); }
 128
 129     void handleTransform(osg::Transform& transform)
 130     {
 131         // Hmm, may be this needs to be refined somehow ...
 132         if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
 133             return;
 134
 135         if (!testBoundingSphere(transform.getBound()))
 136             return;
 137
 138         osg::Matrix inverseMatrix;
 139         if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
 140             return;
 141         osg::Matrix matrix;
 142         if (!transform.computeLocalToWorldMatrix(matrix, this))
 143             return;
 144
 145         // Look for a velocity note
 146         const SGSceneUserData::Velocity* velocity = getVelocity(transform);
 147         // ... no velocity of there is only zero velocity
 148         if (velocity && velocity->linear == SGVec3d::zeros() &&
 149             velocity->angular == SGVec3d::zeros())
 150             velocity = 0;
 151
 152         SGVec3d center = _center;
 153         _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
 154         double radius = _radius;
 155         if (velocity)
 156             _radius += (_endTime - _startTime)*norm(velocity->linear);
 157
 158         simgear::BVHSubTreeCollector::NodeList parentNodeList;
 159         mSubTreeCollector.pushNodeList(parentNodeList);
 160
 161         addBoundingVolume(transform);
 162         traverse(transform);
 163
 164         if (mSubTreeCollector.haveChildren()) {
 165             if (velocity) {
 166                 simgear::BVHMotionTransform* bvhTransform;
 167                 bvhTransform = new simgear::BVHMotionTransform;
 168                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
 169                 bvhTransform->setLinearVelocity(velocity->linear);
 170                 bvhTransform->setAngularVelocity(velocity->angular);
 171                 bvhTransform->setReferenceTime(_startTime);
 172                 bvhTransform->setEndTime(_endTime);
 173
 174                 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
 175             } else {
 176                 simgear::BVHTransform* bvhTransform;
 177                 bvhTransform = new simgear::BVHTransform;
 178                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
 179
 180                 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
 181             }
 182         } else {
 183             mSubTreeCollector.popNodeList(parentNodeList);
 184         }
 185         _center = center;
 186         _radius = radius;
 187     }
 188
 189     const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
 190     {
 191         SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
 192         if (!userData)
 193             return 0;
 194         return userData->getVelocity();
 195     }
 196     simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
 197     {
 198         SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
 199         if (!userData)
 200             return 0;
 201         return userData->getBVHNode();
 202     }
 203     void addBoundingVolume(osg::Node& node)
 204     {
 205         simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
 206         if (!bvNode)
 207             return;
 208
 209         // Get that part of the local bv tree that intersects our sphere
 210         // of interrest.
 211         mSubTreeCollector.setSphere(SGSphered(_center, _radius));
 212         bvNode->accept(mSubTreeCollector);
 213     }
 214
 215     bool testBoundingSphere(const osg::BoundingSphere& bound) const
 216     {
 217         if (!bound.valid())
 218             return false;
 219
 220         double maxDist = bound._radius + _radius;
 221         return distSqr(SGVec3d(bound._center), _center) <= maxDist*maxDist;
 222     }
 223
 224     SGSharedPtr<simgear::BVHNode> getBVHNode() const
 225     { return mSubTreeCollector.getNode(); }
 226
 227 private:
 228
 229     SGVec3d _center;
 230     double _radius;
 231     double _startTime;
 232     double _endTime;
 233
 234     simgear::BVHSubTreeCollector mSubTreeCollector;
 235 };
 236
 237 FGGroundCache::FGGroundCache() :
 238     _altitude(0),
 239     _type(0),
 240     _material(0),
 241     cache_ref_time(0),
 242     _wire(0),
 243     reference_wgs84_point(SGVec3d(0, 0, 0)),
 244     reference_vehicle_radius(0),
 245     down(0.0, 0.0, 0.0),
 246     found_ground(false)
 247 {
 248 }
 249
 250 FGGroundCache::~FGGroundCache()
 251 {
 252 }
 253
 254 bool
 255 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
 256                                     double rad)
 257 {
 258     // Empty cache.
 259     found_ground = false;
 260
 261     SGGeod geodPt = SGGeod::fromCart(pt);
 262     // Don't blow away the cache ground_radius and stuff if there's no
 263     // scenery
 264     if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
 265                                                     geodPt.getLongitudeDeg(),
 266                                                     rad))
 267         return false;
 268     _altitude = 0;
 269
 270     // If we have an active wire, get some more area into the groundcache
 271     if (_wire)
 272         rad = SGMiscd::max(200, rad);
 273
 274     // Store the parameters we used to build up that cache.
 275     reference_wgs84_point = pt;
 276     reference_vehicle_radius = rad;
 277     // Store the time reference used to compute movements of moving triangles.
 278     cache_ref_time = ref_time;
 279
 280     // Get a normalized down vector valid for the whole cache
 281     SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
 282     down = hlToEc.rotate(SGVec3d(0, 0, 1));
 283
 284     // Get the ground cache, that is a local collision tree of the environment
 285     double endTime = cache_ref_time + 1; //FIXME??
 286     CacheFill subtreeCollector(pt, rad, cache_ref_time, endTime);
 287     globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
 288     _localBvhTree = subtreeCollector.getBVHNode();
 289
 290     // Try to get a croase altitude value for the ground cache
 291     SGLineSegmentd line(pt, pt + 1e4*down);
 292     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time);
 293     if (_localBvhTree)
 294         _localBvhTree->accept(lineSegmentVisitor);
 295
 296     // If this is successful, store this altitude for croase altitude values
 297     if (!lineSegmentVisitor.empty()) {
 298         SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
 299         _altitude = geodPt.getElevationM();
 300         _material = lineSegmentVisitor.getMaterial();
 301         _type = materialToGroundType(_material);
 302         found_ground = true;
 303     } else {
 304         // Else do a crude scene query for the current point
 305         found_ground = globals->get_scenery()->
 306             get_cart_elevation_m(pt, rad, _altitude, &_material);
 307         _type = materialToGroundType(_material);
 308     }
 309
 310     // Still not sucessful??
 311     if (!found_ground)
 312         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
 313                "cache without any scenery below the aircraft");
 314
 315     return found_ground;
 316 }
 317
 318 bool
 319 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 320 {
 321     pt = reference_wgs84_point;
 322     rad = reference_vehicle_radius;
 323     ref_time = cache_ref_time;
 324     return found_ground;
 325 }
 326
 327 class FGGroundCache::CatapultFinder : public BVHVisitor {
 328 public:
 329     CatapultFinder(const SGSphered& sphere, const double& t) :
 330         _sphere(sphere),
 331         _time(t),
 332         _haveLineSegment(false)
 333     { }
 334
 335     virtual void apply(BVHGroup& leaf)
 336     {
 337         if (!intersects(_sphere, leaf.getBoundingSphere()))
 338             return;
 339         leaf.traverse(*this);
 340     }
 341     virtual void apply(BVHTransform& transform)
 342     {
 343         if (!intersects(_sphere, transform.getBoundingSphere()))
 344             return;
 345
 346         SGSphered sphere = _sphere;
 347         _sphere = transform.sphereToLocal(sphere);
 348         bool haveLineSegment = _haveLineSegment;
 349         _haveLineSegment = false;
 350
 351         transform.traverse(*this);
 352
 353         if (_haveLineSegment) {
 354             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 355             _linearVelocity = transform.vecToWorld(_linearVelocity);
 356             _angularVelocity = transform.vecToWorld(_angularVelocity);
 357         }
 358         _haveLineSegment |= haveLineSegment;
 359         _sphere.setCenter(sphere.getCenter());
 360     }
 361     virtual void apply(BVHMotionTransform& transform)
 362     {
 363         if (!intersects(_sphere, transform.getBoundingSphere()))
 364             return;
 365
 366         SGSphered sphere = _sphere;
 367         _sphere = transform.sphereToLocal(sphere, _time);
 368         bool haveLineSegment = _haveLineSegment;
 369         _haveLineSegment = false;
 370
 371         transform.traverse(*this);
 372
 373         if (_haveLineSegment) {
 374             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 375             _linearVelocity
 376                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 377             _angularVelocity += transform.getAngularVelocity();
 378             _linearVelocity = toWorld.xformVec(_linearVelocity);
 379             _angularVelocity = toWorld.xformVec(_angularVelocity);
 380             _lineSegment = _lineSegment.transform(toWorld);
 381         }
 382         _haveLineSegment |= haveLineSegment;
 383         _sphere.setCenter(sphere.getCenter());
 384     }
 385     virtual void apply(BVHLineGeometry& node)
 386     {
 387         if (node.getType() != BVHLineGeometry::CarrierCatapult)
 388             return;
 389
 390         SGLineSegmentd lineSegment(node.getLineSegment());
 391         if (!intersects(_sphere, lineSegment))
 392             return;
 393
 394         _lineSegment = lineSegment;
 395         double dist = distSqr(lineSegment, getSphere().getCenter());
 396         _sphere.setRadius(sqrt(dist));
 397         _linearVelocity = SGVec3d::zeros();
 398         _angularVelocity = SGVec3d::zeros();
 399         _haveLineSegment = true;
 400     }
 401     virtual void apply(BVHStaticGeometry& node)
 402     { }
 403
 404     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 405     virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { }
 406     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 407
 408     void setSphere(const SGSphered& sphere)
 409     { _sphere = sphere; }
 410     const SGSphered& getSphere() const
 411     { return _sphere; }
 412
 413     const SGLineSegmentd& getLineSegment() const
 414     { return _lineSegment; }
 415     const SGVec3d& getLinearVelocity() const
 416     { return _linearVelocity; }
 417     const SGVec3d& getAngularVelocity() const
 418     { return _angularVelocity; }
 419
 420     bool getHaveLineSegment() const
 421     { return _haveLineSegment; }
 422
 423 protected:
 424     SGLineSegmentd _lineSegment;
 425     SGVec3d _linearVelocity;
 426     SGVec3d _angularVelocity;
 427
 428     bool _haveLineSegment;
 429
 430     SGSphered _sphere;
 431     double _time;
 432 };
 433
 434 double
 435 FGGroundCache::get_cat(double t, const SGVec3d& pt,
 436                        SGVec3d end[2], SGVec3d vel[2])
 437 {
 438     double maxDistance = 1000;
 439
 440     // Get the wire in question
 441     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
 442     if (_localBvhTree)
 443         _localBvhTree->accept(catapultFinder);
 444
 445     if (!catapultFinder.getHaveLineSegment())
 446         return maxDistance;
 447
 448     // prepare the returns
 449     end[0] = catapultFinder.getLineSegment().getStart();
 450     end[1] = catapultFinder.getLineSegment().getEnd();
 451
 452     // The linear velocity is the one at the start of the line segment ...
 453     vel[0] = catapultFinder.getLinearVelocity();
 454     // ... so the end point has the additional cross product.
 455     vel[1] = catapultFinder.getLinearVelocity();
 456     vel[1] += cross(catapultFinder.getAngularVelocity(),
 457                     catapultFinder.getLineSegment().getDirection());
 458
 459     // Return the distance to the cat
 460     return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
 461 }
 462
 463 bool
 464 FGGroundCache::get_agl(double t, const SGVec3d& pt, double max_altoff,
 465                        SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
 466                        int *type, const SGMaterial** material, double *agl)
 467 {
 468     // Just set up a ground intersection query for the given point
 469     SGLineSegmentd line(pt - max_altoff*down, pt + 1e4*down);
 470     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
 471     if (_localBvhTree)
 472         _localBvhTree->accept(lineSegmentVisitor);
 473
 474     if (!lineSegmentVisitor.empty()) {
 475         // Have an intersection
 476         contact = lineSegmentVisitor.getPoint();
 477         normal = lineSegmentVisitor.getNormal();
 478         if (0 < dot(normal, down))
 479             normal = -normal;
 480         *agl = dot(down, contact - pt);
 481         vel = lineSegmentVisitor.getLinearVelocity();
 482         // correct the linear velocity, since the line intersector delivers
 483         // values for the start point and the get_agl function should
 484         // traditionally deliver for the contact point
 485         vel += cross(lineSegmentVisitor.getAngularVelocity(),
 486                      contact - line.getStart());
 487         *type = materialToGroundType(lineSegmentVisitor.getMaterial());
 488         if (material)
 489             *material = lineSegmentVisitor.getMaterial();
 490
 491         return true;
 492     } else {
 493         // Whenever we did not have a ground triangle for the requested point,
 494         // take the ground level we found during the current cache build.
 495         // This is as good as what we had before for agl.
 496         SGGeod geodPt = SGGeod::fromCart(pt);
 497         *agl = geodPt.getElevationM() - _altitude;
 498         geodPt.setElevationM(_altitude);
 499         contact = SGVec3d::fromGeod(geodPt);
 500         normal = -down;
 501         vel = SGVec3d(0, 0, 0);
 502         *type = _type;
 503         if (material)
 504             *material = _material;
 505
 506         return found_ground;
 507     }
 508 }
 509
 510 class FGGroundCache::WireIntersector : public BVHVisitor {
 511 public:
 512     WireIntersector(const SGVec3d pt[4], const double& t) :
 513         _linearVelocity(SGVec3d::zeros()),
 514         _angularVelocity(SGVec3d::zeros()),
 515         _wire(0),
 516         _time(t)
 517     {
 518         // Build the two triangles spanning the area where the hook has moved
 519         // during the past step.
 520         _triangles[0].set(pt[0], pt[1], pt[2]);
 521         _triangles[1].set(pt[0], pt[2], pt[3]);
 522     }
 523
 524     virtual void apply(BVHGroup& leaf)
 525     {
 526         if (!_intersects(leaf.getBoundingSphere()))
 527             return;
 528
 529         leaf.traverse(*this);
 530     }
 531     virtual void apply(BVHTransform& transform)
 532     {
 533         if (!_intersects(transform.getBoundingSphere()))
 534             return;
 535
 536         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 537         _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
 538         _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
 539
 540         transform.traverse(*this);
 541
 542         if (_wire) {
 543             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 544             _linearVelocity = transform.vecToWorld(_linearVelocity);
 545             _angularVelocity = transform.vecToWorld(_angularVelocity);
 546         }
 547         _triangles[0] = triangles[0];
 548         _triangles[1] = triangles[1];
 549     }
 550     virtual void apply(BVHMotionTransform& transform)
 551     {
 552         if (!_intersects(transform.getBoundingSphere()))
 553             return;
 554
 555         SGMatrixd toLocal = transform.getToLocalTransform(_time);
 556
 557         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 558         _triangles[0] = triangles[0].transform(toLocal);
 559         _triangles[1] = triangles[1].transform(toLocal);
 560
 561         transform.traverse(*this);
 562
 563         if (_wire) {
 564             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 565             _linearVelocity
 566                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 567             _angularVelocity += transform.getAngularVelocity();
 568             _linearVelocity = toWorld.xformVec(_linearVelocity);
 569             _angularVelocity = toWorld.xformVec(_angularVelocity);
 570             _lineSegment = _lineSegment.transform(toWorld);
 571         }
 572         _triangles[0] = triangles[0];
 573         _triangles[1] = triangles[1];
 574     }
 575     virtual void apply(BVHLineGeometry& node)
 576     {
 577         if (node.getType() != BVHLineGeometry::CarrierWire)
 578             return;
 579         SGLineSegmentd lineSegment(node.getLineSegment());
 580         if (!_intersects(lineSegment))
 581             return;
 582
 583         _lineSegment = lineSegment;
 584         _linearVelocity = SGVec3d::zeros();
 585         _angularVelocity = SGVec3d::zeros();
 586         _wire = &node;
 587     }
 588     virtual void apply(BVHStaticGeometry& node)
 589     { }
 590
 591     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 592     virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { }
 593     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 594
 595     bool _intersects(const SGSphered& sphere) const
 596     {
 597         if (_wire)
 598             return false;
 599         if (intersects(_triangles[0], sphere))
 600             return true;
 601         if (intersects(_triangles[1], sphere))
 602             return true;
 603         return false;
 604     }
 605     bool _intersects(const SGLineSegmentd& lineSegment) const
 606     {
 607         if (_wire)
 608             return false;
 609         if (intersects(_triangles[0], lineSegment))
 610             return true;
 611         if (intersects(_triangles[1], lineSegment))
 612             return true;
 613         return false;
 614     }
 615
 616     const SGLineSegmentd& getLineSegment() const
 617     { return _lineSegment; }
 618     const SGVec3d& getLinearVelocity() const
 619     { return _linearVelocity; }
 620     const SGVec3d& getAngularVelocity() const
 621     { return _angularVelocity; }
 622
 623     const BVHLineGeometry* getWire() const
 624     { return _wire; }
 625
 626 private:
 627     SGLineSegmentd _lineSegment;
 628     SGVec3d _linearVelocity;
 629     SGVec3d _angularVelocity;
 630     const BVHLineGeometry* _wire;
 631
 632     SGTriangled _triangles[2];
 633     double _time;
 634 };
 635
 636 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 637 {
 638     // Get the wire in question
 639     WireIntersector wireIntersector(pt, t);
 640     if (_localBvhTree)
 641         _localBvhTree->accept(wireIntersector);
 642
 643     _wire = wireIntersector.getWire();
 644     return _wire;
 645 }
 646
 647 class FGGroundCache::WireFinder : public BVHVisitor {
 648 public:
 649     WireFinder(const BVHLineGeometry* wire, const double& t) :
 650         _wire(wire),
 651         _time(t),
 652         _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
 653         _linearVelocity(SGVec3d::zeros()),
 654         _angularVelocity(SGVec3d::zeros()),
 655         _haveLineSegment(false)
 656     { }
 657
 658     virtual void apply(BVHGroup& leaf)
 659     {
 660         if (_haveLineSegment)
 661             return;
 662         leaf.traverse(*this);
 663     }
 664     virtual void apply(BVHTransform& transform)
 665     {
 666         if (_haveLineSegment)
 667             return;
 668
 669         transform.traverse(*this);
 670
 671         if (_haveLineSegment) {
 672             _linearVelocity = transform.vecToWorld(_linearVelocity);
 673             _angularVelocity = transform.vecToWorld(_angularVelocity);
 674             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 675         }
 676     }
 677     virtual void apply(BVHMotionTransform& transform)
 678     {
 679         if (_haveLineSegment)
 680             return;
 681
 682         transform.traverse(*this);
 683
 684         if (_haveLineSegment) {
 685             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 686             _linearVelocity
 687                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 688             _angularVelocity += transform.getAngularVelocity();
 689             _linearVelocity = toWorld.xformVec(_linearVelocity);
 690             _angularVelocity = toWorld.xformVec(_angularVelocity);
 691             _lineSegment = _lineSegment.transform(toWorld);
 692         }
 693     }
 694     virtual void apply(BVHLineGeometry& node)
 695     {
 696         if (_haveLineSegment)
 697             return;
 698         if (_wire != &node)
 699             return;
 700         if (node.getType() != BVHLineGeometry::CarrierWire)
 701             return;
 702         _lineSegment = SGLineSegmentd(node.getLineSegment());
 703         _linearVelocity = SGVec3d::zeros();
 704         _angularVelocity = SGVec3d::zeros();
 705         _haveLineSegment = true;
 706     }
 707     virtual void apply(BVHStaticGeometry&) { }
 708
 709     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 710     virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { }
 711     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 712
 713     const SGLineSegmentd& getLineSegment() const
 714     { return _lineSegment; }
 715
 716     bool getHaveLineSegment() const
 717     { return _haveLineSegment; }
 718
 719     const SGVec3d& getLinearVelocity() const
 720     { return _linearVelocity; }
 721     const SGVec3d& getAngularVelocity() const
 722     { return _angularVelocity; }
 723
 724 private:
 725     const BVHLineGeometry* _wire;
 726     double _time;
 727
 728     SGLineSegmentd _lineSegment;
 729     SGVec3d _linearVelocity;
 730     SGVec3d _angularVelocity;
 731
 732     bool _haveLineSegment;
 733 };
 734
 735 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 736 {
 737     // Fast return if we do not have an active wire.
 738     if (!_wire)
 739         return false;
 740
 741     // Get the wire in question
 742     WireFinder wireFinder(_wire, t);
 743     if (_localBvhTree)
 744         _localBvhTree->accept(wireFinder);
 745
 746     if (!wireFinder.getHaveLineSegment())
 747         return false;
 748
 749     // prepare the returns
 750     end[0] = wireFinder.getLineSegment().getStart();
 751     end[1] = wireFinder.getLineSegment().getEnd();
 752
 753     // The linear velocity is the one at the start of the line segment ...
 754     vel[0] = wireFinder.getLinearVelocity();
 755     // ... so the end point has the additional cross product.
 756     vel[1] = wireFinder.getLinearVelocity();
 757     vel[1] += cross(wireFinder.getAngularVelocity(),
 758                     wireFinder.getLineSegment().getDirection());
 759
 760     return true;
 761 }
 762
 763 void FGGroundCache::release_wire(void)
 764 {
 765     _wire = 0;
 766 }