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