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