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