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