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