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