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     cache_time_offset(0),
 242     _wire(0),
 243     reference_wgs84_point(SGVec3d(0, 0, 0)),
 244     reference_vehicle_radius(0),
 245     down(0.0, 0.0, 0.0),
 246     found_ground(false)
 247 {
 248 }
 249
 250 FGGroundCache::~FGGroundCache()
 251 {
 252 }
 253
 254 bool
 255 FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
 256                                     const SGVec3d& pt, double rad)
 257 {
 258     // Empty cache.
 259     found_ground = false;
 260
 261     SGGeod geodPt = SGGeod::fromCart(pt);
 262     // Don't blow away the cache ground_radius and stuff if there's no
 263     // scenery
 264     if (!globals->get_tile_mgr()->scenery_available(geodPt, rad)) {
 265         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
 266                "returns false at " << geodPt << " " << pt << " " << rad);
 267         return false;
 268     }
 269     _material = 0;
 270
 271     // If we have an active wire, get some more area into the groundcache
 272     if (_wire)
 273         rad = SGMiscd::max(200, rad);
 274
 275     // Store the parameters we used to build up that cache.
 276     reference_wgs84_point = pt;
 277     reference_vehicle_radius = rad;
 278     // Store the time reference used to compute movements of moving triangles.
 279     cache_ref_time = startSimTime;
 280
 281     // Get a normalized down vector valid for the whole cache
 282     SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
 283     down = hlToEc.rotate(SGVec3d(0, 0, 1));
 284
 285     // Get the ground cache, that is a local collision tree of the environment
 286     startSimTime += cache_time_offset;
 287     endSimTime += cache_time_offset;
 288     CacheFill subtreeCollector(pt, rad, startSimTime, endSimTime);
 289     globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
 290     _localBvhTree = subtreeCollector.getBVHNode();
 291
 292     // Try to get a croase altitude value for the ground cache
 293     SGLineSegmentd line(pt, pt + 2*reference_vehicle_radius*down);
 294     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
 295     if (_localBvhTree)
 296         _localBvhTree->accept(lineSegmentVisitor);
 297
 298     // If this is successful, store this altitude for croase altitude values
 299     if (!lineSegmentVisitor.empty()) {
 300         SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
 301         _altitude = geodPt.getElevationM();
 302         _material = lineSegmentVisitor.getMaterial();
 303         found_ground = true;
 304     } else {
 305         // Else do a crude scene query for the current point
 306         double alt = 0;
 307         found_ground = globals->get_scenery()->
 308             get_cart_elevation_m(pt, rad, alt, &_material);
 309         if (found_ground)
 310             _altitude = alt;
 311     }
 312
 313     // Still not sucessful??
 314     if (!found_ground)
 315         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
 316                "cache without any scenery below the aircraft");
 317
 318     return found_ground;
 319 }
 320
 321 bool
 322 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 323 {
 324     pt = reference_wgs84_point;
 325     rad = reference_vehicle_radius;
 326     ref_time = cache_ref_time;
 327     return found_ground;
 328 }
 329
 330 class FGGroundCache::BodyFinder : public BVHVisitor {
 331 public:
 332     BodyFinder(BVHNode::Id id, const double& t) :
 333         _id(id),
 334         _bodyToWorld(SGMatrixd::unit()),
 335         _linearVelocity(0, 0, 0),
 336         _angularVelocity(0, 0, 0),
 337         _time(t)
 338     { }
 339
 340     virtual void apply(BVHGroup& leaf)
 341     {
 342         if (_foundId)
 343             return;
 344         leaf.traverse(*this);
 345     }
 346     virtual void apply(BVHTransform& transform)
 347     {
 348         if (_foundId)
 349             return;
 350
 351         transform.traverse(*this);
 352
 353         if (_foundId) {
 354             _linearVelocity = transform.vecToWorld(_linearVelocity);
 355             _angularVelocity = transform.vecToWorld(_angularVelocity);
 356             _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
 357         }
 358     }
 359     virtual void apply(BVHMotionTransform& transform)
 360     {
 361         if (_foundId)
 362             return;
 363
 364         if (_id == transform.getId()) {
 365             _foundId = true;
 366             return;
 367         }
 368
 369         transform.traverse(*this);
 370
 371         if (_foundId) {
 372             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 373             SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
 374             _linearVelocity += transform.getLinearVelocityAt(referencePoint);
 375             _angularVelocity += transform.getAngularVelocity();
 376             _linearVelocity = toWorld.xformVec(_linearVelocity);
 377             _angularVelocity = toWorld.xformVec(_angularVelocity);
 378             _bodyToWorld = toWorld*_bodyToWorld;
 379         }
 380     }
 381     virtual void apply(BVHLineGeometry& node) { }
 382     virtual void apply(BVHStaticGeometry& node) { }
 383
 384     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 385     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 386
 387     const SGMatrixd& getBodyToWorld() const
 388     { return _bodyToWorld; }
 389     const SGVec3d& getLinearVelocity() const
 390     { return _linearVelocity; }
 391     const SGVec3d& getAngularVelocity() const
 392     { return _angularVelocity; }
 393
 394     bool empty() const
 395     { return !_foundId; }
 396
 397 protected:
 398     simgear::BVHNode::Id _id;
 399
 400     SGMatrixd _bodyToWorld;
 401
 402     SGVec3d _linearVelocity;
 403     SGVec3d _angularVelocity;
 404
 405     bool _foundId;
 406
 407     double _time;
 408 };
 409
 410 bool
 411 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
 412                         SGVec3d& angularVel, simgear::BVHNode::Id id)
 413 {
 414     // Get the transform matrix and velocities of a moving body with id at t.
 415     if (!_localBvhTree)
 416         return false;
 417     t += cache_time_offset;
 418     BodyFinder bodyFinder(id, t);
 419     _localBvhTree->accept(bodyFinder);
 420     if (bodyFinder.empty())
 421         return false;
 422
 423     bodyToWorld = bodyFinder.getBodyToWorld();
 424     linearVel = bodyFinder.getLinearVelocity();
 425     angularVel = bodyFinder.getAngularVelocity();
 426
 427     return true;
 428 }
 429
 430 class FGGroundCache::CatapultFinder : public BVHVisitor {
 431 public:
 432     CatapultFinder(const SGSphered& sphere, const double& t) :
 433         _sphere(sphere),
 434         _time(t),
 435         _haveLineSegment(false)
 436     { }
 437
 438     virtual void apply(BVHGroup& leaf)
 439     {
 440         if (!intersects(_sphere, leaf.getBoundingSphere()))
 441             return;
 442         leaf.traverse(*this);
 443     }
 444     virtual void apply(BVHTransform& transform)
 445     {
 446         if (!intersects(_sphere, transform.getBoundingSphere()))
 447             return;
 448
 449         SGSphered sphere = _sphere;
 450         _sphere = transform.sphereToLocal(sphere);
 451         bool haveLineSegment = _haveLineSegment;
 452         _haveLineSegment = false;
 453
 454         transform.traverse(*this);
 455
 456         if (_haveLineSegment) {
 457             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 458             _linearVelocity = transform.vecToWorld(_linearVelocity);
 459             _angularVelocity = transform.vecToWorld(_angularVelocity);
 460         }
 461         _haveLineSegment |= haveLineSegment;
 462         _sphere.setCenter(sphere.getCenter());
 463     }
 464     virtual void apply(BVHMotionTransform& transform)
 465     {
 466         if (!intersects(_sphere, transform.getBoundingSphere()))
 467             return;
 468
 469         SGSphered sphere = _sphere;
 470         _sphere = transform.sphereToLocal(sphere, _time);
 471         bool haveLineSegment = _haveLineSegment;
 472         _haveLineSegment = false;
 473
 474         transform.traverse(*this);
 475
 476         if (_haveLineSegment) {
 477             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 478             _linearVelocity
 479                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 480             _angularVelocity += transform.getAngularVelocity();
 481             _linearVelocity = toWorld.xformVec(_linearVelocity);
 482             _angularVelocity = toWorld.xformVec(_angularVelocity);
 483             _lineSegment = _lineSegment.transform(toWorld);
 484         }
 485         _haveLineSegment |= haveLineSegment;
 486         _sphere.setCenter(sphere.getCenter());
 487     }
 488     virtual void apply(BVHLineGeometry& node)
 489     {
 490         if (node.getType() != BVHLineGeometry::CarrierCatapult)
 491             return;
 492
 493         SGLineSegmentd lineSegment(node.getLineSegment());
 494         if (!intersects(_sphere, lineSegment))
 495             return;
 496
 497         _lineSegment = lineSegment;
 498         double dist = distSqr(lineSegment, getSphere().getCenter());
 499         _sphere.setRadius(sqrt(dist));
 500         _linearVelocity = SGVec3d::zeros();
 501         _angularVelocity = SGVec3d::zeros();
 502         _haveLineSegment = true;
 503     }
 504     virtual void apply(BVHStaticGeometry& node)
 505     { }
 506
 507     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 508     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 509
 510     void setSphere(const SGSphered& sphere)
 511     { _sphere = sphere; }
 512     const SGSphered& getSphere() const
 513     { return _sphere; }
 514
 515     const SGLineSegmentd& getLineSegment() const
 516     { return _lineSegment; }
 517     const SGVec3d& getLinearVelocity() const
 518     { return _linearVelocity; }
 519     const SGVec3d& getAngularVelocity() const
 520     { return _angularVelocity; }
 521
 522     bool getHaveLineSegment() const
 523     { return _haveLineSegment; }
 524
 525 protected:
 526     SGLineSegmentd _lineSegment;
 527     SGVec3d _linearVelocity;
 528     SGVec3d _angularVelocity;
 529
 530     bool _haveLineSegment;
 531
 532     SGSphered _sphere;
 533     double _time;
 534 };
 535
 536 double
 537 FGGroundCache::get_cat(double t, const SGVec3d& pt,
 538                        SGVec3d end[2], SGVec3d vel[2])
 539 {
 540     double maxDistance = 1000;
 541
 542     // Get the wire in question
 543     t += cache_time_offset;
 544     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
 545     if (_localBvhTree)
 546         _localBvhTree->accept(catapultFinder);
 547
 548     if (!catapultFinder.getHaveLineSegment())
 549         return maxDistance;
 550
 551     // prepare the returns
 552     end[0] = catapultFinder.getLineSegment().getStart();
 553     end[1] = catapultFinder.getLineSegment().getEnd();
 554
 555     // The linear velocity is the one at the start of the line segment ...
 556     vel[0] = catapultFinder.getLinearVelocity();
 557     // ... so the end point has the additional cross product.
 558     vel[1] = catapultFinder.getLinearVelocity();
 559     vel[1] += cross(catapultFinder.getAngularVelocity(),
 560                     catapultFinder.getLineSegment().getDirection());
 561
 562     // Return the distance to the cat
 563     return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
 564 }
 565
 566 bool
 567 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
 568                        SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
 569                        simgear::BVHNode::Id& id, const SGMaterial*& material)
 570 {
 571     // Just set up a ground intersection query for the given point
 572     SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
 573     t += cache_time_offset;
 574     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
 575     if (_localBvhTree)
 576         _localBvhTree->accept(lineSegmentVisitor);
 577
 578     if (!lineSegmentVisitor.empty()) {
 579         // Have an intersection
 580         contact = lineSegmentVisitor.getPoint();
 581         normal = lineSegmentVisitor.getNormal();
 582         if (0 < dot(normal, down))
 583             normal = -normal;
 584         linearVel = lineSegmentVisitor.getLinearVelocity();
 585         angularVel = lineSegmentVisitor.getAngularVelocity();
 586         material = lineSegmentVisitor.getMaterial();
 587         id = lineSegmentVisitor.getId();
 588
 589         return true;
 590     } else {
 591         // Whenever we did not have a ground triangle for the requested point,
 592         // take the ground level we found during the current cache build.
 593         // This is as good as what we had before for agl.
 594         SGGeod geodPt = SGGeod::fromCart(pt);
 595         geodPt.setElevationM(_altitude);
 596         contact = SGVec3d::fromGeod(geodPt);
 597         normal = -down;
 598         linearVel = SGVec3d(0, 0, 0);
 599         angularVel = SGVec3d(0, 0, 0);
 600         material = _material;
 601         id = 0;
 602
 603         return found_ground;
 604     }
 605 }
 606
 607
 608 bool
 609 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
 610                            SGVec3d& contact, SGVec3d& linearVel,
 611                            SGVec3d& angularVel, simgear::BVHNode::Id& id,
 612                            const SGMaterial*& material)
 613 {
 614     if (!_localBvhTree)
 615         return false;
 616
 617     // Just set up a ground intersection query for the given point
 618     SGSphered sphere(pt, maxDist);
 619     t += cache_time_offset;
 620     simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
 621     _localBvhTree->accept(nearestPointVisitor);
 622
 623     if (nearestPointVisitor.empty())
 624         return false;
 625
 626     // Have geometry in the range of maxDist
 627     contact = nearestPointVisitor.getPoint();
 628     linearVel = nearestPointVisitor.getLinearVelocity();
 629     angularVel = nearestPointVisitor.getAngularVelocity();
 630     material = nearestPointVisitor.getMaterial();
 631     id = nearestPointVisitor.getId();
 632
 633     return true;
 634 }
 635
 636
 637 class FGGroundCache::WireIntersector : public BVHVisitor {
 638 public:
 639     WireIntersector(const SGVec3d pt[4], const double& t) :
 640         _linearVelocity(SGVec3d::zeros()),
 641         _angularVelocity(SGVec3d::zeros()),
 642         _wire(0),
 643         _time(t)
 644     {
 645         // Build the two triangles spanning the area where the hook has moved
 646         // during the past step.
 647         _triangles[0].set(pt[0], pt[1], pt[2]);
 648         _triangles[1].set(pt[0], pt[2], pt[3]);
 649     }
 650
 651     virtual void apply(BVHGroup& leaf)
 652     {
 653         if (!_intersects(leaf.getBoundingSphere()))
 654             return;
 655
 656         leaf.traverse(*this);
 657     }
 658     virtual void apply(BVHTransform& transform)
 659     {
 660         if (!_intersects(transform.getBoundingSphere()))
 661             return;
 662
 663         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 664         _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
 665         _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
 666
 667         transform.traverse(*this);
 668
 669         if (_wire) {
 670             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 671             _linearVelocity = transform.vecToWorld(_linearVelocity);
 672             _angularVelocity = transform.vecToWorld(_angularVelocity);
 673         }
 674         _triangles[0] = triangles[0];
 675         _triangles[1] = triangles[1];
 676     }
 677     virtual void apply(BVHMotionTransform& transform)
 678     {
 679         if (!_intersects(transform.getBoundingSphere()))
 680             return;
 681
 682         SGMatrixd toLocal = transform.getToLocalTransform(_time);
 683
 684         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 685         _triangles[0] = triangles[0].transform(toLocal);
 686         _triangles[1] = triangles[1].transform(toLocal);
 687
 688         transform.traverse(*this);
 689
 690         if (_wire) {
 691             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 692             _linearVelocity
 693                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 694             _angularVelocity += transform.getAngularVelocity();
 695             _linearVelocity = toWorld.xformVec(_linearVelocity);
 696             _angularVelocity = toWorld.xformVec(_angularVelocity);
 697             _lineSegment = _lineSegment.transform(toWorld);
 698         }
 699         _triangles[0] = triangles[0];
 700         _triangles[1] = triangles[1];
 701     }
 702     virtual void apply(BVHLineGeometry& node)
 703     {
 704         if (node.getType() != BVHLineGeometry::CarrierWire)
 705             return;
 706         SGLineSegmentd lineSegment(node.getLineSegment());
 707         if (!_intersects(lineSegment))
 708             return;
 709
 710         _lineSegment = lineSegment;
 711         _linearVelocity = SGVec3d::zeros();
 712         _angularVelocity = SGVec3d::zeros();
 713         _wire = &node;
 714     }
 715     virtual void apply(BVHStaticGeometry& node)
 716     { }
 717
 718     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 719     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 720
 721     bool _intersects(const SGSphered& sphere) const
 722     {
 723         if (_wire)
 724             return false;
 725         if (intersects(_triangles[0], sphere))
 726             return true;
 727         if (intersects(_triangles[1], sphere))
 728             return true;
 729         return false;
 730     }
 731     bool _intersects(const SGLineSegmentd& lineSegment) const
 732     {
 733         if (_wire)
 734             return false;
 735         if (intersects(_triangles[0], lineSegment))
 736             return true;
 737         if (intersects(_triangles[1], lineSegment))
 738             return true;
 739         return false;
 740     }
 741
 742     const SGLineSegmentd& getLineSegment() const
 743     { return _lineSegment; }
 744     const SGVec3d& getLinearVelocity() const
 745     { return _linearVelocity; }
 746     const SGVec3d& getAngularVelocity() const
 747     { return _angularVelocity; }
 748
 749     const BVHLineGeometry* getWire() const
 750     { return _wire; }
 751
 752 private:
 753     SGLineSegmentd _lineSegment;
 754     SGVec3d _linearVelocity;
 755     SGVec3d _angularVelocity;
 756     const BVHLineGeometry* _wire;
 757
 758     SGTriangled _triangles[2];
 759     double _time;
 760 };
 761
 762 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 763 {
 764     // Get the wire in question
 765     t += cache_time_offset;
 766     WireIntersector wireIntersector(pt, t);
 767     if (_localBvhTree)
 768         _localBvhTree->accept(wireIntersector);
 769
 770     _wire = wireIntersector.getWire();
 771     return _wire;
 772 }
 773
 774 class FGGroundCache::WireFinder : public BVHVisitor {
 775 public:
 776     WireFinder(const BVHLineGeometry* wire, const double& t) :
 777         _wire(wire),
 778         _time(t),
 779         _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
 780         _linearVelocity(SGVec3d::zeros()),
 781         _angularVelocity(SGVec3d::zeros()),
 782         _haveLineSegment(false)
 783     { }
 784
 785     virtual void apply(BVHGroup& leaf)
 786     {
 787         if (_haveLineSegment)
 788             return;
 789         leaf.traverse(*this);
 790     }
 791     virtual void apply(BVHTransform& transform)
 792     {
 793         if (_haveLineSegment)
 794             return;
 795
 796         transform.traverse(*this);
 797
 798         if (_haveLineSegment) {
 799             _linearVelocity = transform.vecToWorld(_linearVelocity);
 800             _angularVelocity = transform.vecToWorld(_angularVelocity);
 801             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 802         }
 803     }
 804     virtual void apply(BVHMotionTransform& transform)
 805     {
 806         if (_haveLineSegment)
 807             return;
 808
 809         transform.traverse(*this);
 810
 811         if (_haveLineSegment) {
 812             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 813             _linearVelocity
 814                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 815             _angularVelocity += transform.getAngularVelocity();
 816             _linearVelocity = toWorld.xformVec(_linearVelocity);
 817             _angularVelocity = toWorld.xformVec(_angularVelocity);
 818             _lineSegment = _lineSegment.transform(toWorld);
 819         }
 820     }
 821     virtual void apply(BVHLineGeometry& node)
 822     {
 823         if (_haveLineSegment)
 824             return;
 825         if (_wire != &node)
 826             return;
 827         if (node.getType() != BVHLineGeometry::CarrierWire)
 828             return;
 829         _lineSegment = SGLineSegmentd(node.getLineSegment());
 830         _linearVelocity = SGVec3d::zeros();
 831         _angularVelocity = SGVec3d::zeros();
 832         _haveLineSegment = true;
 833     }
 834     virtual void apply(BVHStaticGeometry&) { }
 835
 836     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 837     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 838
 839     const SGLineSegmentd& getLineSegment() const
 840     { return _lineSegment; }
 841
 842     bool getHaveLineSegment() const
 843     { return _haveLineSegment; }
 844
 845     const SGVec3d& getLinearVelocity() const
 846     { return _linearVelocity; }
 847     const SGVec3d& getAngularVelocity() const
 848     { return _angularVelocity; }
 849
 850 private:
 851     const BVHLineGeometry* _wire;
 852     double _time;
 853
 854     SGLineSegmentd _lineSegment;
 855     SGVec3d _linearVelocity;
 856     SGVec3d _angularVelocity;
 857
 858     bool _haveLineSegment;
 859 };
 860
 861 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 862 {
 863     // Fast return if we do not have an active wire.
 864     if (!_wire)
 865         return false;
 866
 867     // Get the wire in question
 868     t += cache_time_offset;
 869     WireFinder wireFinder(_wire, t);
 870     if (_localBvhTree)
 871         _localBvhTree->accept(wireFinder);
 872
 873     if (!wireFinder.getHaveLineSegment())
 874         return false;
 875
 876     // prepare the returns
 877     end[0] = wireFinder.getLineSegment().getStart();
 878     end[1] = wireFinder.getLineSegment().getEnd();
 879
 880     // The linear velocity is the one at the start of the line segment ...
 881     vel[0] = wireFinder.getLinearVelocity();
 882     // ... so the end point has the additional cross product.
 883     vel[1] = wireFinder.getLinearVelocity();
 884     vel[1] += cross(wireFinder.getAngularVelocity(),
 885                     wireFinder.getLineSegment().getDirection());
 886
 887     return true;
 888 }
 889
 890 void FGGroundCache::release_wire(void)
 891 {
 892     _wire = 0;
 893 }