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