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/scene/bvh/BVHNode.hxx>
  49 #include <simgear/scene/bvh/BVHGroup.hxx>
  50 #include <simgear/scene/bvh/BVHTransform.hxx>
  51 #include <simgear/scene/bvh/BVHMotionTransform.hxx>
  52 #include <simgear/scene/bvh/BVHLineGeometry.hxx>
  53 #include <simgear/scene/bvh/BVHStaticGeometry.hxx>
  54 #include <simgear/scene/bvh/BVHStaticData.hxx>
  55 #include <simgear/scene/bvh/BVHStaticNode.hxx>
  56 #include <simgear/scene/bvh/BVHStaticTriangle.hxx>
  57 #include <simgear/scene/bvh/BVHStaticBinary.hxx>
  58 #include <simgear/scene/bvh/BVHSubTreeCollector.hxx>
  59 #include <simgear/scene/bvh/BVHLineSegmentVisitor.hxx>
  60 #include <simgear/scene/bvh/BVHNearestPointVisitor.hxx>
  61
  62 #ifdef GROUNDCACHE_DEBUG
  63 #include <simgear/scene/bvh/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 SGMaterial* 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 SGMaterial* 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 SGMaterial* _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         found_ground = globals->get_scenery()->
 385             get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
 386         if (found_ground)
 387             _altitude = alt;
 388     }
 389
 390     // Still not sucessful??
 391     if (!found_ground)
 392         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
 393                "cache without any scenery below the aircraft");
 394
 395 #ifdef GROUNDCACHE_DEBUG
 396     t0 = SGTimeStamp::now() - t0;
 397     _buildTime += t0;
 398     _buildCount++;
 399
 400     if (_buildCount > 60) {
 401         double buildTime = 0;
 402         if (_buildCount)
 403             buildTime = _buildTime.toSecs()/_buildCount;
 404         double lookupTime = 0;
 405         if (_lookupCount)
 406             lookupTime = _lookupTime.toSecs()/_lookupCount;
 407         _buildTime = SGTimeStamp::fromSec(0.0);
 408         _buildCount = 0;
 409         _lookupTime = SGTimeStamp::fromSec(0.0);
 410         _lookupCount = 0;
 411         SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
 412                << ", lookup Time = " << lookupTime);
 413     }
 414
 415     if (!_group.valid()) {
 416         _group = new osg::Group;
 417         globals->get_scenery()->get_scene_graph()->addChild(_group);
 418         fgSetInt("/fdm/groundcache-debug-level", -3);
 419     }
 420     _group->removeChildren(0, _group->getNumChildren());
 421     if (_localBvhTree) {
 422         int level = fgGetInt("/fdm/groundcache-debug-level");
 423         if (-2 <= level) {
 424             simgear::BVHDebugCollectVisitor debug(endSimTime, level);
 425             _localBvhTree->accept(debug);
 426             _group->addChild(debug.getNode());
 427         }
 428     }
 429 #endif
 430
 431     return found_ground;
 432 }
 433
 434 bool
 435 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 436 {
 437     pt = reference_wgs84_point;
 438     rad = reference_vehicle_radius;
 439     ref_time = cache_ref_time;
 440     return found_ground;
 441 }
 442
 443 class FGGroundCache::BodyFinder : public BVHVisitor {
 444 public:
 445     BodyFinder(BVHNode::Id id, const double& t) :
 446         _id(id),
 447         _bodyToWorld(SGMatrixd::unit()),
 448         _linearVelocity(0, 0, 0),
 449         _angularVelocity(0, 0, 0),
 450         _time(t)
 451     { }
 452
 453     virtual void apply(BVHGroup& leaf)
 454     {
 455         if (_foundId)
 456             return;
 457         leaf.traverse(*this);
 458     }
 459     virtual void apply(BVHTransform& transform)
 460     {
 461         if (_foundId)
 462             return;
 463
 464         transform.traverse(*this);
 465
 466         if (_foundId) {
 467             _linearVelocity = transform.vecToWorld(_linearVelocity);
 468             _angularVelocity = transform.vecToWorld(_angularVelocity);
 469             _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
 470         }
 471     }
 472     virtual void apply(BVHMotionTransform& transform)
 473     {
 474         if (_foundId)
 475             return;
 476
 477         if (_id == transform.getId()) {
 478             _foundId = true;
 479         } else {
 480             transform.traverse(*this);
 481         }
 482
 483         if (_foundId) {
 484             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 485             SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
 486             _linearVelocity += transform.getLinearVelocityAt(referencePoint);
 487             _angularVelocity += transform.getAngularVelocity();
 488             _linearVelocity = toWorld.xformVec(_linearVelocity);
 489             _angularVelocity = toWorld.xformVec(_angularVelocity);
 490             _bodyToWorld = toWorld*_bodyToWorld;
 491         }
 492     }
 493     virtual void apply(BVHLineGeometry& node) { }
 494     virtual void apply(BVHStaticGeometry& node) { }
 495
 496     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 497     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 498
 499     const SGMatrixd& getBodyToWorld() const
 500     { return _bodyToWorld; }
 501     const SGVec3d& getLinearVelocity() const
 502     { return _linearVelocity; }
 503     const SGVec3d& getAngularVelocity() const
 504     { return _angularVelocity; }
 505
 506     bool empty() const
 507     { return !_foundId; }
 508
 509 protected:
 510     simgear::BVHNode::Id _id;
 511
 512     SGMatrixd _bodyToWorld;
 513
 514     SGVec3d _linearVelocity;
 515     SGVec3d _angularVelocity;
 516
 517     bool _foundId;
 518
 519     double _time;
 520 };
 521
 522 bool
 523 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
 524                         SGVec3d& angularVel, simgear::BVHNode::Id id)
 525 {
 526     // Get the transform matrix and velocities of a moving body with id at t.
 527     if (!_localBvhTree)
 528         return false;
 529     t += cache_time_offset;
 530     BodyFinder bodyFinder(id, t);
 531     _localBvhTree->accept(bodyFinder);
 532     if (bodyFinder.empty())
 533         return false;
 534
 535     bodyToWorld = bodyFinder.getBodyToWorld();
 536     linearVel = bodyFinder.getLinearVelocity();
 537     angularVel = bodyFinder.getAngularVelocity();
 538
 539     return true;
 540 }
 541
 542 class FGGroundCache::CatapultFinder : public BVHVisitor {
 543 public:
 544     CatapultFinder(const SGSphered& sphere, const double& t) :
 545         _haveLineSegment(false),
 546         _sphere(sphere),
 547         _time(t)
 548     { }
 549
 550     virtual void apply(BVHGroup& leaf)
 551     {
 552         if (!intersects(_sphere, leaf.getBoundingSphere()))
 553             return;
 554         leaf.traverse(*this);
 555     }
 556     virtual void apply(BVHTransform& transform)
 557     {
 558         if (!intersects(_sphere, transform.getBoundingSphere()))
 559             return;
 560
 561         SGSphered sphere = _sphere;
 562         _sphere = transform.sphereToLocal(sphere);
 563         bool haveLineSegment = _haveLineSegment;
 564         _haveLineSegment = false;
 565
 566         transform.traverse(*this);
 567
 568         if (_haveLineSegment) {
 569             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 570             _linearVelocity = transform.vecToWorld(_linearVelocity);
 571             _angularVelocity = transform.vecToWorld(_angularVelocity);
 572         }
 573         _haveLineSegment |= haveLineSegment;
 574         _sphere.setCenter(sphere.getCenter());
 575     }
 576     virtual void apply(BVHMotionTransform& transform)
 577     {
 578         if (!intersects(_sphere, transform.getBoundingSphere()))
 579             return;
 580
 581         SGSphered sphere = _sphere;
 582         _sphere = transform.sphereToLocal(sphere, _time);
 583         bool haveLineSegment = _haveLineSegment;
 584         _haveLineSegment = false;
 585
 586         transform.traverse(*this);
 587
 588         if (_haveLineSegment) {
 589             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 590             _linearVelocity
 591                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 592             _angularVelocity += transform.getAngularVelocity();
 593             _linearVelocity = toWorld.xformVec(_linearVelocity);
 594             _angularVelocity = toWorld.xformVec(_angularVelocity);
 595             _lineSegment = _lineSegment.transform(toWorld);
 596         }
 597         _haveLineSegment |= haveLineSegment;
 598         _sphere.setCenter(sphere.getCenter());
 599     }
 600     virtual void apply(BVHLineGeometry& node)
 601     {
 602         if (node.getType() != BVHLineGeometry::CarrierCatapult)
 603             return;
 604
 605         SGLineSegmentd lineSegment(node.getLineSegment());
 606         if (!intersects(_sphere, lineSegment))
 607             return;
 608
 609         _lineSegment = lineSegment;
 610         double dist = distSqr(lineSegment, getSphere().getCenter());
 611         _sphere.setRadius(sqrt(dist));
 612         _linearVelocity = SGVec3d::zeros();
 613         _angularVelocity = SGVec3d::zeros();
 614         _haveLineSegment = true;
 615     }
 616     virtual void apply(BVHStaticGeometry& node)
 617     { }
 618
 619     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 620     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 621
 622     void setSphere(const SGSphered& sphere)
 623     { _sphere = sphere; }
 624     const SGSphered& getSphere() const
 625     { return _sphere; }
 626
 627     const SGLineSegmentd& getLineSegment() const
 628     { return _lineSegment; }
 629     const SGVec3d& getLinearVelocity() const
 630     { return _linearVelocity; }
 631     const SGVec3d& getAngularVelocity() const
 632     { return _angularVelocity; }
 633
 634     bool getHaveLineSegment() const
 635     { return _haveLineSegment; }
 636
 637 protected:
 638     SGLineSegmentd _lineSegment;
 639     SGVec3d _linearVelocity;
 640     SGVec3d _angularVelocity;
 641
 642     bool _haveLineSegment;
 643
 644     SGSphered _sphere;
 645     double _time;
 646 };
 647
 648 double
 649 FGGroundCache::get_cat(double t, const SGVec3d& pt,
 650                        SGVec3d end[2], SGVec3d vel[2])
 651 {
 652     double maxDistance = 1000;
 653
 654     // Get the wire in question
 655     t += cache_time_offset;
 656     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
 657     if (_localBvhTree)
 658         _localBvhTree->accept(catapultFinder);
 659
 660     if (!catapultFinder.getHaveLineSegment())
 661         return maxDistance;
 662
 663     // prepare the returns
 664     end[0] = catapultFinder.getLineSegment().getStart();
 665     end[1] = catapultFinder.getLineSegment().getEnd();
 666
 667     // The linear velocity is the one at the start of the line segment ...
 668     vel[0] = catapultFinder.getLinearVelocity();
 669     // ... so the end point has the additional cross product.
 670     vel[1] = catapultFinder.getLinearVelocity();
 671     vel[1] += cross(catapultFinder.getAngularVelocity(),
 672                     catapultFinder.getLineSegment().getDirection());
 673
 674     // Return the distance to the cat
 675     return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
 676 }
 677
 678 bool
 679 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
 680                        SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
 681                        simgear::BVHNode::Id& id, const SGMaterial*& material)
 682 {
 683 #ifdef GROUNDCACHE_DEBUG
 684     SGTimeStamp t0 = SGTimeStamp::now();
 685 #endif
 686
 687     // Just set up a ground intersection query for the given point
 688     SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
 689     t += cache_time_offset;
 690     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
 691     if (_localBvhTree)
 692         _localBvhTree->accept(lineSegmentVisitor);
 693
 694 #ifdef GROUNDCACHE_DEBUG
 695     t0 = SGTimeStamp::now() - t0;
 696     _lookupTime += t0;
 697     _lookupCount++;
 698 #endif
 699
 700     if (!lineSegmentVisitor.empty()) {
 701         // Have an intersection
 702         contact = lineSegmentVisitor.getPoint();
 703         normal = lineSegmentVisitor.getNormal();
 704         if (0 < dot(normal, down))
 705             normal = -normal;
 706         linearVel = lineSegmentVisitor.getLinearVelocity();
 707         angularVel = lineSegmentVisitor.getAngularVelocity();
 708         material = lineSegmentVisitor.getMaterial();
 709         id = lineSegmentVisitor.getId();
 710
 711         return true;
 712     } else {
 713         // Whenever we did not have a ground triangle for the requested point,
 714         // take the ground level we found during the current cache build.
 715         // This is as good as what we had before for agl.
 716         SGGeod geodPt = SGGeod::fromCart(pt);
 717         geodPt.setElevationM(_altitude);
 718         contact = SGVec3d::fromGeod(geodPt);
 719         normal = -down;
 720         linearVel = SGVec3d(0, 0, 0);
 721         angularVel = SGVec3d(0, 0, 0);
 722         material = _material;
 723         id = 0;
 724
 725         return found_ground;
 726     }
 727 }
 728
 729
 730 bool
 731 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
 732                            SGVec3d& contact, SGVec3d& linearVel,
 733                            SGVec3d& angularVel, simgear::BVHNode::Id& id,
 734                            const SGMaterial*& material)
 735 {
 736     if (!_localBvhTree)
 737         return false;
 738
 739 #ifdef GROUNDCACHE_DEBUG
 740     SGTimeStamp t0 = SGTimeStamp::now();
 741 #endif
 742
 743     // Just set up a ground intersection query for the given point
 744     SGSphered sphere(pt, maxDist);
 745     t += cache_time_offset;
 746     simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
 747     _localBvhTree->accept(nearestPointVisitor);
 748
 749 #ifdef GROUNDCACHE_DEBUG
 750     t0 = SGTimeStamp::now() - t0;
 751     _lookupTime += t0;
 752     _lookupCount++;
 753 #endif
 754
 755     if (nearestPointVisitor.empty())
 756         return false;
 757
 758     // Have geometry in the range of maxDist
 759     contact = nearestPointVisitor.getPoint();
 760     linearVel = nearestPointVisitor.getLinearVelocity();
 761     angularVel = nearestPointVisitor.getAngularVelocity();
 762     material = nearestPointVisitor.getMaterial();
 763     id = nearestPointVisitor.getId();
 764
 765     return true;
 766 }
 767
 768
 769 class FGGroundCache::WireIntersector : public BVHVisitor {
 770 public:
 771     WireIntersector(const SGVec3d pt[4], const double& t) :
 772         _linearVelocity(SGVec3d::zeros()),
 773         _angularVelocity(SGVec3d::zeros()),
 774         _wire(0),
 775         _time(t)
 776     {
 777         // Build the two triangles spanning the area where the hook has moved
 778         // during the past step.
 779         _triangles[0].set(pt[0], pt[1], pt[2]);
 780         _triangles[1].set(pt[0], pt[2], pt[3]);
 781     }
 782
 783     virtual void apply(BVHGroup& leaf)
 784     {
 785         if (!_intersects(leaf.getBoundingSphere()))
 786             return;
 787
 788         leaf.traverse(*this);
 789     }
 790     virtual void apply(BVHTransform& transform)
 791     {
 792         if (!_intersects(transform.getBoundingSphere()))
 793             return;
 794
 795         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 796         _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
 797         _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
 798
 799         transform.traverse(*this);
 800
 801         if (_wire) {
 802             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 803             _linearVelocity = transform.vecToWorld(_linearVelocity);
 804             _angularVelocity = transform.vecToWorld(_angularVelocity);
 805         }
 806         _triangles[0] = triangles[0];
 807         _triangles[1] = triangles[1];
 808     }
 809     virtual void apply(BVHMotionTransform& transform)
 810     {
 811         if (!_intersects(transform.getBoundingSphere()))
 812             return;
 813
 814         SGMatrixd toLocal = transform.getToLocalTransform(_time);
 815
 816         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
 817         _triangles[0] = triangles[0].transform(toLocal);
 818         _triangles[1] = triangles[1].transform(toLocal);
 819
 820         transform.traverse(*this);
 821
 822         if (_wire) {
 823             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 824             _linearVelocity
 825                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 826             _angularVelocity += transform.getAngularVelocity();
 827             _linearVelocity = toWorld.xformVec(_linearVelocity);
 828             _angularVelocity = toWorld.xformVec(_angularVelocity);
 829             _lineSegment = _lineSegment.transform(toWorld);
 830         }
 831         _triangles[0] = triangles[0];
 832         _triangles[1] = triangles[1];
 833     }
 834     virtual void apply(BVHLineGeometry& node)
 835     {
 836         if (node.getType() != BVHLineGeometry::CarrierWire)
 837             return;
 838         SGLineSegmentd lineSegment(node.getLineSegment());
 839         if (!_intersects(lineSegment))
 840             return;
 841
 842         _lineSegment = lineSegment;
 843         _linearVelocity = SGVec3d::zeros();
 844         _angularVelocity = SGVec3d::zeros();
 845         _wire = &node;
 846     }
 847     virtual void apply(BVHStaticGeometry& node)
 848     { }
 849
 850     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 851     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 852
 853     bool _intersects(const SGSphered& sphere) const
 854     {
 855         if (_wire)
 856             return false;
 857         if (intersects(_triangles[0], sphere))
 858             return true;
 859         if (intersects(_triangles[1], sphere))
 860             return true;
 861         return false;
 862     }
 863     bool _intersects(const SGLineSegmentd& lineSegment) const
 864     {
 865         if (_wire)
 866             return false;
 867         if (intersects(_triangles[0], lineSegment))
 868             return true;
 869         if (intersects(_triangles[1], lineSegment))
 870             return true;
 871         return false;
 872     }
 873
 874     const SGLineSegmentd& getLineSegment() const
 875     { return _lineSegment; }
 876     const SGVec3d& getLinearVelocity() const
 877     { return _linearVelocity; }
 878     const SGVec3d& getAngularVelocity() const
 879     { return _angularVelocity; }
 880
 881     const BVHLineGeometry* getWire() const
 882     { return _wire; }
 883
 884 private:
 885     SGLineSegmentd _lineSegment;
 886     SGVec3d _linearVelocity;
 887     SGVec3d _angularVelocity;
 888     const BVHLineGeometry* _wire;
 889
 890     SGTriangled _triangles[2];
 891     double _time;
 892 };
 893
 894 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 895 {
 896     // Get the wire in question
 897     t += cache_time_offset;
 898     WireIntersector wireIntersector(pt, t);
 899     if (_localBvhTree)
 900         _localBvhTree->accept(wireIntersector);
 901
 902     _wire = wireIntersector.getWire();
 903     return _wire;
 904 }
 905
 906 class FGGroundCache::WireFinder : public BVHVisitor {
 907 public:
 908     WireFinder(const BVHLineGeometry* wire, const double& t) :
 909         _wire(wire),
 910         _time(t),
 911         _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
 912         _linearVelocity(SGVec3d::zeros()),
 913         _angularVelocity(SGVec3d::zeros()),
 914         _haveLineSegment(false)
 915     { }
 916
 917     virtual void apply(BVHGroup& leaf)
 918     {
 919         if (_haveLineSegment)
 920             return;
 921         leaf.traverse(*this);
 922     }
 923     virtual void apply(BVHTransform& transform)
 924     {
 925         if (_haveLineSegment)
 926             return;
 927
 928         transform.traverse(*this);
 929
 930         if (_haveLineSegment) {
 931             _linearVelocity = transform.vecToWorld(_linearVelocity);
 932             _angularVelocity = transform.vecToWorld(_angularVelocity);
 933             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
 934         }
 935     }
 936     virtual void apply(BVHMotionTransform& transform)
 937     {
 938         if (_haveLineSegment)
 939             return;
 940
 941         transform.traverse(*this);
 942
 943         if (_haveLineSegment) {
 944             SGMatrixd toWorld = transform.getToWorldTransform(_time);
 945             _linearVelocity
 946                 += transform.getLinearVelocityAt(_lineSegment.getStart());
 947             _angularVelocity += transform.getAngularVelocity();
 948             _linearVelocity = toWorld.xformVec(_linearVelocity);
 949             _angularVelocity = toWorld.xformVec(_angularVelocity);
 950             _lineSegment = _lineSegment.transform(toWorld);
 951         }
 952     }
 953     virtual void apply(BVHLineGeometry& node)
 954     {
 955         if (_haveLineSegment)
 956             return;
 957         if (_wire != &node)
 958             return;
 959         if (node.getType() != BVHLineGeometry::CarrierWire)
 960             return;
 961         _lineSegment = SGLineSegmentd(node.getLineSegment());
 962         _linearVelocity = SGVec3d::zeros();
 963         _angularVelocity = SGVec3d::zeros();
 964         _haveLineSegment = true;
 965     }
 966     virtual void apply(BVHStaticGeometry&) { }
 967
 968     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
 969     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
 970
 971     const SGLineSegmentd& getLineSegment() const
 972     { return _lineSegment; }
 973
 974     bool getHaveLineSegment() const
 975     { return _haveLineSegment; }
 976
 977     const SGVec3d& getLinearVelocity() const
 978     { return _linearVelocity; }
 979     const SGVec3d& getAngularVelocity() const
 980     { return _angularVelocity; }
 981
 982 private:
 983     const BVHLineGeometry* _wire;
 984     double _time;
 985
 986     SGLineSegmentd _lineSegment;
 987     SGVec3d _linearVelocity;
 988     SGVec3d _angularVelocity;
 989
 990     bool _haveLineSegment;
 991 };
 992
 993 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 994 {
 995     // Fast return if we do not have an active wire.
 996     if (!_wire)
 997         return false;
 998
 999     // Get the wire in question
1000     t += cache_time_offset;
1001     WireFinder wireFinder(_wire, t);
1002     if (_localBvhTree)
1003         _localBvhTree->accept(wireFinder);
1004
1005     if (!wireFinder.getHaveLineSegment())
1006         return false;
1007
1008     // prepare the returns
1009     end[0] = wireFinder.getLineSegment().getStart();
1010     end[1] = wireFinder.getLineSegment().getEnd();
1011
1012     // The linear velocity is the one at the start of the line segment ...
1013     vel[0] = wireFinder.getLinearVelocity();
1014     // ... so the end point has the additional cross product.
1015     vel[1] = wireFinder.getLinearVelocity();
1016     vel[1] += cross(wireFinder.getAngularVelocity(),
1017                     wireFinder.getLineSegment().getDirection());
1018
1019     return true;
1020 }
1021
1022 void FGGroundCache::release_wire(void)
1023 {
1024     _wire = 0;
1025 }