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         double radius = _radius;
 138         _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
 139         if (velocity) {
 140             SGVec3d staticCenter(_center);
 141
 142             double dtStart = velocity->referenceTime - _startTime;
 143             SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
 144             SGVec3d angle = dtStart*velocity->angular;
 145             startCenter = SGQuatd::fromAngleAxis(angle).transform(startCenter);
 146
 147             double dtEnd = velocity->referenceTime - _endTime;
 148             SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
 149             angle = dtEnd*velocity->angular;
 150             endCenter = SGQuatd::fromAngleAxis(angle).transform(endCenter);
 151
 152             _center = 0.5*(startCenter + endCenter);
 153             _radius += 0.5*dist(startCenter, endCenter);
 154         }
 155
 156         simgear::BVHSubTreeCollector::NodeList parentNodeList;
 157         mSubTreeCollector.pushNodeList(parentNodeList);
 158
 159         addBoundingVolume(transform);
 160         traverse(transform);
 161
 162         if (mSubTreeCollector.haveChildren()) {
 163             if (velocity) {
 164                 simgear::BVHMotionTransform* bvhTransform;
 165                 bvhTransform = new simgear::BVHMotionTransform;
 166                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
 167                 bvhTransform->setLinearVelocity(velocity->linear);
 168                 bvhTransform->setAngularVelocity(velocity->angular);
 169                 bvhTransform->setReferenceTime(velocity->referenceTime);
 170                 bvhTransform->setStartTime(_startTime);
 171                 bvhTransform->setEndTime(_endTime);
 172                 bvhTransform->setId(velocity->id);
 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     _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 startSimTime, double endSimTime,
 255                                     const SGVec3d& pt, 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, rad)) {
 264         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
 265                "returns false at " << geodPt << " " << pt << " " << rad);
 266         return false;
 267     }
 268     _material = 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 = startSimTime;
 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     CacheFill subtreeCollector(pt, rad, startSimTime, endSimTime);
 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 + 2*reference_vehicle_radius*down);
 291     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
 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         found_ground = true;
 301     } else {
 302         // Else do a crude scene query for the current point
 303         double alt = 0;
 304         found_ground = globals->get_scenery()->
 305             get_cart_elevation_m(pt, rad, alt, &_material);
 306         if (found_ground)
 307             _altitude = alt;
 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::BodyFinder : public BVHVisitor {
 328 public:
 329     BodyFinder(BVHNode::Id id, const double& t) :
 330         _id(id),
 331         _bodyToWorld(SGMatrixd::unit()),
 332         _linearVelocity(0, 0, 0),
 333         _angularVelocity(0, 0, 0),
 334         _time(t)
 335     { }
 336
 337     virtual void apply(BVHGroup& leaf)
 338     {
 339         if (_foundId)
 340             return;
 341         leaf.traverse(*this);
 342     }
 343     virtual void apply(BVHTransform& transform)
 344     {
 345         if (_foundId)
 346             return;
 347
 348         transform.traverse(*this);
 349
 350         if (_foundId) {
 351             _linearVelocity = transform.vecToWorld(_linearVelocity);
 352             _angularVelocity = transform.vecToWorld(_angularVelocity);
 353             _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
 354         }
 355     }
 356     virtual void apply(BVHMotionTransform& transform)
 357     {
 358         if (_foundId)
 359             return;
 360
 361         if (_id == transform.getId()) {
 362             _foundId = true;
 363             return;
 364         }
 365
 366         transform.traverse(*this);
 367
 368         if (_foundId) {
 369             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 370             SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
 371             _linearVelocity += transform.getLinearVelocityAt(referencePoint);
 372             _angularVelocity += transform.getAngularVelocity();
 373             _linearVelocity = toWorld.xformVec(_linearVelocity);
 374             _angularVelocity = toWorld.xformVec(_angularVelocity);
 375             _bodyToWorld = toWorld*_bodyToWorld;
 376         }
 377     }
 378     virtual void apply(BVHLineGeometry& node) { }
 379     virtual void apply(BVHStaticGeometry& node) { }
 380
 381     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 382     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 383
 384     const SGMatrixd& getBodyToWorld() const
 385     { return _bodyToWorld; }
 386     const SGVec3d& getLinearVelocity() const
 387     { return _linearVelocity; }
 388     const SGVec3d& getAngularVelocity() const
 389     { return _angularVelocity; }
 390
 391     bool empty() const
 392     { return !_foundId; }
 393
 394 protected:
 395     simgear::BVHNode::Id _id;
 396
 397     SGMatrixd _bodyToWorld;
 398
 399     SGVec3d _linearVelocity;
 400     SGVec3d _angularVelocity;
 401
 402     bool _foundId;
 403
 404     double _time;
 405 };
 406
 407 bool
 408 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
 409                         SGVec3d& angularVel, simgear::BVHNode::Id id)
 410 {
 411     // Get the transform matrix and velocities of a moving body with id at t.
 412     if (!_localBvhTree)
 413         return false;
 414     BodyFinder bodyFinder(id, t);
 415     _localBvhTree->accept(bodyFinder);
 416     if (bodyFinder.empty())
 417         return false;
 418
 419     bodyToWorld = bodyFinder.getBodyToWorld();
 420     linearVel = bodyFinder.getLinearVelocity();
 421     angularVel = bodyFinder.getAngularVelocity();
 422
 423     return true;
 424 }
 425
 426 class FGGroundCache::CatapultFinder : public BVHVisitor {
 427 public:
 428     CatapultFinder(const SGSphered& sphere, const double& t) :
 429         _sphere(sphere),
 430         _time(t),
 431         _haveLineSegment(false)
 432     { }
 433
 434     virtual void apply(BVHGroup& leaf)
 435     {
 436         if (!intersects(_sphere, leaf.getBoundingSphere()))
 437             return;
 438         leaf.traverse(*this);
 439     }
 440     virtual void apply(BVHTransform& transform)
 441     {
 442         if (!intersects(_sphere, transform.getBoundingSphere()))
 443             return;
 444
 445         SGSphered sphere = _sphere;
 446         _sphere = transform.sphereToLocal(sphere);
 447         bool haveLineSegment = _haveLineSegment;
 448         _haveLineSegment = false;
 449
 450         transform.traverse(*this);
 451
 452         if (_haveLineSegment) {
 453             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 454             _linearVelocity = transform.vecToWorld(_linearVelocity);
 455             _angularVelocity = transform.vecToWorld(_angularVelocity);
 456         }
 457         _haveLineSegment |= haveLineSegment;
 458         _sphere.setCenter(sphere.getCenter());
 459     }
 460     virtual void apply(BVHMotionTransform& transform)
 461     {
 462         if (!intersects(_sphere, transform.getBoundingSphere()))
 463             return;
 464
 465         SGSphered sphere = _sphere;
 466         _sphere = transform.sphereToLocal(sphere, _time);
 467         bool haveLineSegment = _haveLineSegment;
 468         _haveLineSegment = false;
 469
 470         transform.traverse(*this);
 471
 472         if (_haveLineSegment) {
 473             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 474             _linearVelocity
 475                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 476             _angularVelocity += transform.getAngularVelocity();
 477             _linearVelocity = toWorld.xformVec(_linearVelocity);
 478             _angularVelocity = toWorld.xformVec(_angularVelocity);
 479             _lineSegment = _lineSegment.transform(toWorld);
 480         }
 481         _haveLineSegment |= haveLineSegment;
 482         _sphere.setCenter(sphere.getCenter());
 483     }
 484     virtual void apply(BVHLineGeometry& node)
 485     {
 486         if (node.getType() != BVHLineGeometry::CarrierCatapult)
 487             return;
 488
 489         SGLineSegmentd lineSegment(node.getLineSegment());
 490         if (!intersects(_sphere, lineSegment))
 491             return;
 492
 493         _lineSegment = lineSegment;
 494         double dist = distSqr(lineSegment, getSphere().getCenter());
 495         _sphere.setRadius(sqrt(dist));
 496         _linearVelocity = SGVec3d::zeros();
 497         _angularVelocity = SGVec3d::zeros();
 498         _haveLineSegment = true;
 499     }
 500     virtual void apply(BVHStaticGeometry& node)
 501     { }
 502
 503     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 504     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 505
 506     void setSphere(const SGSphered& sphere)
 507     { _sphere = sphere; }
 508     const SGSphered& getSphere() const
 509     { return _sphere; }
 510
 511     const SGLineSegmentd& getLineSegment() const
 512     { return _lineSegment; }
 513     const SGVec3d& getLinearVelocity() const
 514     { return _linearVelocity; }
 515     const SGVec3d& getAngularVelocity() const
 516     { return _angularVelocity; }
 517
 518     bool getHaveLineSegment() const
 519     { return _haveLineSegment; }
 520
 521 protected:
 522     SGLineSegmentd _lineSegment;
 523     SGVec3d _linearVelocity;
 524     SGVec3d _angularVelocity;
 525
 526     bool _haveLineSegment;
 527
 528     SGSphered _sphere;
 529     double _time;
 530 };
 531
 532 double
 533 FGGroundCache::get_cat(double t, const SGVec3d& pt,
 534                        SGVec3d end[2], SGVec3d vel[2])
 535 {
 536     double maxDistance = 1000;
 537
 538     // Get the wire in question
 539     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
 540     if (_localBvhTree)
 541         _localBvhTree->accept(catapultFinder);
 542
 543     if (!catapultFinder.getHaveLineSegment())
 544         return maxDistance;
 545
 546     // prepare the returns
 547     end[0] = catapultFinder.getLineSegment().getStart();
 548     end[1] = catapultFinder.getLineSegment().getEnd();
 549
 550     // The linear velocity is the one at the start of the line segment ...
 551     vel[0] = catapultFinder.getLinearVelocity();
 552     // ... so the end point has the additional cross product.
 553     vel[1] = catapultFinder.getLinearVelocity();
 554     vel[1] += cross(catapultFinder.getAngularVelocity(),
 555                     catapultFinder.getLineSegment().getDirection());
 556
 557     // Return the distance to the cat
 558     return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
 559 }
 560
 561 bool
 562 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
 563                        SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
 564                        simgear::BVHNode::Id& id, const SGMaterial*& material)
 565 {
 566     // Just set up a ground intersection query for the given point
 567     SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
 568     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
 569     if (_localBvhTree)
 570         _localBvhTree->accept(lineSegmentVisitor);
 571
 572     if (!lineSegmentVisitor.empty()) {
 573         // Have an intersection
 574         contact = lineSegmentVisitor.getPoint();
 575         normal = lineSegmentVisitor.getNormal();
 576         if (0 < dot(normal, down))
 577             normal = -normal;
 578         linearVel = lineSegmentVisitor.getLinearVelocity();
 579         angularVel = lineSegmentVisitor.getAngularVelocity();
 580         material = lineSegmentVisitor.getMaterial();
 581         id = lineSegmentVisitor.getId();
 582
 583         return true;
 584     } else {
 585         // Whenever we did not have a ground triangle for the requested point,
 586         // take the ground level we found during the current cache build.
 587         // This is as good as what we had before for agl.
 588         SGGeod geodPt = SGGeod::fromCart(pt);
 589         geodPt.setElevationM(_altitude);
 590         contact = SGVec3d::fromGeod(geodPt);
 591         normal = -down;
 592         linearVel = SGVec3d(0, 0, 0);
 593         angularVel = SGVec3d(0, 0, 0);
 594         material = _material;
 595         id = 0;
 596
 597         return found_ground;
 598     }
 599 }
 600
 601
 602 bool
 603 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
 604                            SGVec3d& contact, SGVec3d& linearVel,
 605                            SGVec3d& angularVel, simgear::BVHNode::Id& id,
 606                            const SGMaterial*& material)
 607 {
 608     if (!_localBvhTree)
 609         return false;
 610
 611     // Just set up a ground intersection query for the given point
 612     SGSphered sphere(pt, maxDist);
 613     simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
 614     _localBvhTree->accept(nearestPointVisitor);
 615
 616     if (nearestPointVisitor.empty())
 617         return false;
 618
 619     // Have geometry in the range of maxDist
 620     contact = nearestPointVisitor.getPoint();
 621     linearVel = nearestPointVisitor.getLinearVelocity();
 622     angularVel = nearestPointVisitor.getAngularVelocity();
 623     material = nearestPointVisitor.getMaterial();
 624     id = nearestPointVisitor.getId();
 625
 626     return true;
 627 }
 628
 629
 630 class FGGroundCache::WireIntersector : public BVHVisitor {
 631 public:
 632     WireIntersector(const SGVec3d pt[4], const double& t) :
 633         _linearVelocity(SGVec3d::zeros()),
 634         _angularVelocity(SGVec3d::zeros()),
 635         _wire(0),
 636         _time(t)
 637     {
 638         // Build the two triangles spanning the area where the hook has moved
 639         // during the past step.
 640         _triangles[0].set(pt[0], pt[1], pt[2]);
 641         _triangles[1].set(pt[0], pt[2], pt[3]);
 642     }
 643
 644     virtual void apply(BVHGroup& leaf)
 645     {
 646         if (!_intersects(leaf.getBoundingSphere()))
 647             return;
 648
 649         leaf.traverse(*this);
 650     }
 651     virtual void apply(BVHTransform& transform)
 652     {
 653         if (!_intersects(transform.getBoundingSphere()))
 654             return;
 655
 656         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 657         _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
 658         _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
 659
 660         transform.traverse(*this);
 661
 662         if (_wire) {
 663             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 664             _linearVelocity = transform.vecToWorld(_linearVelocity);
 665             _angularVelocity = transform.vecToWorld(_angularVelocity);
 666         }
 667         _triangles[0] = triangles[0];
 668         _triangles[1] = triangles[1];
 669     }
 670     virtual void apply(BVHMotionTransform& transform)
 671     {
 672         if (!_intersects(transform.getBoundingSphere()))
 673             return;
 674
 675         SGMatrixd toLocal = transform.getToLocalTransform(_time);
 676
 677         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 678         _triangles[0] = triangles[0].transform(toLocal);
 679         _triangles[1] = triangles[1].transform(toLocal);
 680
 681         transform.traverse(*this);
 682
 683         if (_wire) {
 684             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 685             _linearVelocity
 686                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 687             _angularVelocity += transform.getAngularVelocity();
 688             _linearVelocity = toWorld.xformVec(_linearVelocity);
 689             _angularVelocity = toWorld.xformVec(_angularVelocity);
 690             _lineSegment = _lineSegment.transform(toWorld);
 691         }
 692         _triangles[0] = triangles[0];
 693         _triangles[1] = triangles[1];
 694     }
 695     virtual void apply(BVHLineGeometry& node)
 696     {
 697         if (node.getType() != BVHLineGeometry::CarrierWire)
 698             return;
 699         SGLineSegmentd lineSegment(node.getLineSegment());
 700         if (!_intersects(lineSegment))
 701             return;
 702
 703         _lineSegment = lineSegment;
 704         _linearVelocity = SGVec3d::zeros();
 705         _angularVelocity = SGVec3d::zeros();
 706         _wire = &node;
 707     }
 708     virtual void apply(BVHStaticGeometry& node)
 709     { }
 710
 711     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 712     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 713
 714     bool _intersects(const SGSphered& sphere) const
 715     {
 716         if (_wire)
 717             return false;
 718         if (intersects(_triangles[0], sphere))
 719             return true;
 720         if (intersects(_triangles[1], sphere))
 721             return true;
 722         return false;
 723     }
 724     bool _intersects(const SGLineSegmentd& lineSegment) const
 725     {
 726         if (_wire)
 727             return false;
 728         if (intersects(_triangles[0], lineSegment))
 729             return true;
 730         if (intersects(_triangles[1], lineSegment))
 731             return true;
 732         return false;
 733     }
 734
 735     const SGLineSegmentd& getLineSegment() const
 736     { return _lineSegment; }
 737     const SGVec3d& getLinearVelocity() const
 738     { return _linearVelocity; }
 739     const SGVec3d& getAngularVelocity() const
 740     { return _angularVelocity; }
 741
 742     const BVHLineGeometry* getWire() const
 743     { return _wire; }
 744
 745 private:
 746     SGLineSegmentd _lineSegment;
 747     SGVec3d _linearVelocity;
 748     SGVec3d _angularVelocity;
 749     const BVHLineGeometry* _wire;
 750
 751     SGTriangled _triangles[2];
 752     double _time;
 753 };
 754
 755 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 756 {
 757     // Get the wire in question
 758     WireIntersector wireIntersector(pt, t);
 759     if (_localBvhTree)
 760         _localBvhTree->accept(wireIntersector);
 761
 762     _wire = wireIntersector.getWire();
 763     return _wire;
 764 }
 765
 766 class FGGroundCache::WireFinder : public BVHVisitor {
 767 public:
 768     WireFinder(const BVHLineGeometry* wire, const double& t) :
 769         _wire(wire),
 770         _time(t),
 771         _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
 772         _linearVelocity(SGVec3d::zeros()),
 773         _angularVelocity(SGVec3d::zeros()),
 774         _haveLineSegment(false)
 775     { }
 776
 777     virtual void apply(BVHGroup& leaf)
 778     {
 779         if (_haveLineSegment)
 780             return;
 781         leaf.traverse(*this);
 782     }
 783     virtual void apply(BVHTransform& transform)
 784     {
 785         if (_haveLineSegment)
 786             return;
 787
 788         transform.traverse(*this);
 789
 790         if (_haveLineSegment) {
 791             _linearVelocity = transform.vecToWorld(_linearVelocity);
 792             _angularVelocity = transform.vecToWorld(_angularVelocity);
 793             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 794         }
 795     }
 796     virtual void apply(BVHMotionTransform& transform)
 797     {
 798         if (_haveLineSegment)
 799             return;
 800
 801         transform.traverse(*this);
 802
 803         if (_haveLineSegment) {
 804             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 805             _linearVelocity
 806                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 807             _angularVelocity += transform.getAngularVelocity();
 808             _linearVelocity = toWorld.xformVec(_linearVelocity);
 809             _angularVelocity = toWorld.xformVec(_angularVelocity);
 810             _lineSegment = _lineSegment.transform(toWorld);
 811         }
 812     }
 813     virtual void apply(BVHLineGeometry& node)
 814     {
 815         if (_haveLineSegment)
 816             return;
 817         if (_wire != &node)
 818             return;
 819         if (node.getType() != BVHLineGeometry::CarrierWire)
 820             return;
 821         _lineSegment = SGLineSegmentd(node.getLineSegment());
 822         _linearVelocity = SGVec3d::zeros();
 823         _angularVelocity = SGVec3d::zeros();
 824         _haveLineSegment = true;
 825     }
 826     virtual void apply(BVHStaticGeometry&) { }
 827
 828     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 829     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 830
 831     const SGLineSegmentd& getLineSegment() const
 832     { return _lineSegment; }
 833
 834     bool getHaveLineSegment() const
 835     { return _haveLineSegment; }
 836
 837     const SGVec3d& getLinearVelocity() const
 838     { return _linearVelocity; }
 839     const SGVec3d& getAngularVelocity() const
 840     { return _angularVelocity; }
 841
 842 private:
 843     const BVHLineGeometry* _wire;
 844     double _time;
 845
 846     SGLineSegmentd _lineSegment;
 847     SGVec3d _linearVelocity;
 848     SGVec3d _angularVelocity;
 849
 850     bool _haveLineSegment;
 851 };
 852
 853 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 854 {
 855     // Fast return if we do not have an active wire.
 856     if (!_wire)
 857         return false;
 858
 859     // Get the wire in question
 860     WireFinder wireFinder(_wire, t);
 861     if (_localBvhTree)
 862         _localBvhTree->accept(wireFinder);
 863
 864     if (!wireFinder.getHaveLineSegment())
 865         return false;
 866
 867     // prepare the returns
 868     end[0] = wireFinder.getLineSegment().getStart();
 869     end[1] = wireFinder.getLineSegment().getEnd();
 870
 871     // The linear velocity is the one at the start of the line segment ...
 872     vel[0] = wireFinder.getLinearVelocity();
 873     // ... so the end point has the additional cross product.
 874     vel[1] = wireFinder.getLinearVelocity();
 875     vel[1] += cross(wireFinder.getAngularVelocity(),
 876                     wireFinder.getLineSegment().getDirection());
 877
 878     return true;
 879 }
 880
 881 void FGGroundCache::release_wire(void)
 882 {
 883     _wire = 0;
 884 }