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  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 <float.h>
  28
  29 #include <osg/CullFace>
  30 #include <osg/Drawable>
  31 #include <osg/Geode>
  32 #include <osg/Geometry>
  33 #include <osg/TriangleFunctor>
  34
  35 #include <simgear/sg_inlines.h>
  36 #include <simgear/constants.h>
  37 #include <simgear/debug/logstream.hxx>
  38 #include <simgear/math/sg_geodesy.hxx>
  39 #include <simgear/scene/material/mat.hxx>
  40 #include <simgear/scene/material/matlib.hxx>
  41 #include <simgear/scene/util/SGNodeMasks.hxx>
  42
  43 #include <Main/globals.hxx>
  44 #include <Scenery/scenery.hxx>
  45 #include <Scenery/tilemgr.hxx>
  46 #include <AIModel/AICarrier.hxx>
  47
  48 #include "flight.hxx"
  49 #include "groundcache.hxx"
  50
  51 /// Ok, variant that uses a infinite line istead of the ray.
  52 /// also not that this only works if the ray direction is normalized.
  53 static inline bool
  54 intersectsInf(const SGRayd& ray, const SGSphered& sphere)
  55 {
  56   SGVec3d r = sphere.getCenter() - ray.getOrigin();
  57   double projectedDistance = dot(r, ray.getDirection());
  58   double dist = dot(r, r) - projectedDistance * projectedDistance;
  59   return dist < sphere.getRadius2();
  60 }
  61
  62 template<typename T>
  63 class SGExtendedTriangleFunctor : public osg::TriangleFunctor<T> {
  64 public:
  65   // Ok, to be complete we should also implement the indexed variants
  66   // For now this one appears to be enough ...
  67   void drawArrays(GLenum mode, GLint first, GLsizei count)
  68   {
  69     if (_vertexArrayPtr==0 || count==0) return;
  70
  71     const osg::Vec3* vlast;
  72     const osg::Vec3* vptr;
  73     switch(mode) {
  74     case(GL_LINES):
  75       vlast = &_vertexArrayPtr[first+count];
  76       for(vptr=&_vertexArrayPtr[first];vptr<vlast;vptr+=2)
  77         this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
  78       break;
  79     case(GL_LINE_STRIP):
  80       vlast = &_vertexArrayPtr[first+count-1];
  81       for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
  82         this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
  83       break;
  84     case(GL_LINE_LOOP):
  85       vlast = &_vertexArrayPtr[first+count-1];
  86       for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
  87         this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
  88       this->operator()(_vertexArrayPtr[first+count-1],
  89                        _vertexArrayPtr[first],_treatVertexDataAsTemporary);
  90       break;
  91     default:
  92       osg::TriangleFunctor<T>::drawArrays(mode, first, count);
  93       break;
  94     }
  95   }
  96 protected:
  97   using osg::TriangleFunctor<T>::_vertexArrayPtr;
  98   using osg::TriangleFunctor<T>::_treatVertexDataAsTemporary;
  99 };
 100
 101 class GroundCacheFillVisitor : public osg::NodeVisitor {
 102 public:
 103
 104   /// class to just redirect triangles to the GroundCacheFillVisitor
 105   class GroundCacheFill {
 106   public:
 107     void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv)
 108     { mGroundCacheFillVisitor = gcfv; }
 109
 110     void operator () (const osg::Vec3& v1, const osg::Vec3& v2,
 111                       const osg::Vec3& v3, bool)
 112     { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); }
 113
 114     void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool)
 115     { mGroundCacheFillVisitor->addLine(v1, v2); }
 116
 117   private:
 118     GroundCacheFillVisitor* mGroundCacheFillVisitor;
 119   };
 120
 121
 122   GroundCacheFillVisitor(FGGroundCache* groundCache,
 123                          const SGVec3d& down,
 124                          const SGVec3d& cacheReference,
 125                          double cacheRadius,
 126                          double wireCacheRadius) :
 127     osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
 128     mGroundCache(groundCache)
 129   {
 130     setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
 131     mDown = down;
 132     mLocalDown = down;
 133     sphIsec = true;
 134     mBackfaceCulling = false;
 135     mCacheReference = cacheReference;
 136     mLocalCacheReference = cacheReference;
 137     mCacheRadius = cacheRadius;
 138     mWireCacheRadius = wireCacheRadius;
 139
 140     mTriangleFunctor.setGroundCacheFillVisitor(this);
 141
 142     mGroundProperty.wire_id = -1;
 143     mGroundProperty.vel = SGVec3d(0, 0, 0);
 144     mGroundProperty.rot = SGVec3d(0, 0, 0);
 145     mGroundProperty.pivot = SGVec3d(0, 0, 0);
 146   }
 147
 148   void updateCullMode(osg::StateSet* stateSet)
 149   {
 150     if (!stateSet)
 151       return;
 152
 153     osg::StateAttribute* stateAttribute;
 154     stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE);
 155     if (!stateAttribute)
 156       return;
 157     osg::CullFace* cullFace = static_cast<osg::CullFace*>(stateAttribute);
 158     mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK;
 159   }
 160
 161   bool enterBoundingSphere(const osg::BoundingSphere& bs)
 162   {
 163     if (!bs.valid())
 164       return false;
 165
 166     SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal);
 167     double rc = bs.radius() + mCacheRadius;
 168     // Ok, this node might intersect the cache. Visit it in depth.
 169     double centerDist2 = distSqr(mCacheReference, cntr);
 170     if (centerDist2 < rc*rc) {
 171       sphIsec = true;
 172     } else {
 173       // Check if the down direction touches the bounding sphere of the node
 174       // if so, do at least croase agl computations.
 175       // Ther other thing is that we must check if we are in range of
 176       // cats or wires
 177       double rw = bs.radius() + mWireCacheRadius;
 178       if (rw*rw < centerDist2 &&
 179           !intersectsInf(SGRayd(mCacheReference, mDown),
 180                          SGSphered(cntr, bs.radius())))
 181         return false;
 182       sphIsec = false;
 183     }
 184
 185     return true;
 186   }
 187
 188   bool enterNode(osg::Node& node)
 189   {
 190     if (!enterBoundingSphere(node.getBound()))
 191       return false;
 192
 193     updateCullMode(node.getStateSet());
 194
 195     FGGroundCache::GroundProperty& gp = mGroundProperty;
 196     // get some material information for use in the gear model
 197     gp.type = FGInterface::Unknown;
 198     osg::Referenced* base = node.getUserData();
 199     if (!base)
 200       return true;
 201     FGAICarrierHardware *ud =
 202       dynamic_cast<FGAICarrierHardware*>(base);
 203     if (!ud)
 204       return true;
 205
 206     switch (ud->type) {
 207     case FGAICarrierHardware::Wire:
 208       gp.type = FGInterface::Wire;
 209       gp.wire_id = ud->id;
 210       break;
 211     case FGAICarrierHardware::Catapult:
 212       gp.type = FGInterface::Catapult;
 213       break;
 214     default:
 215       gp.type = FGInterface::Solid;
 216       break;
 217     }
 218     // Copy the velocity from the carrier class.
 219     ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
 220
 221     return true;
 222   }
 223
 224   void fillWith(osg::Drawable* drawable)
 225   {
 226     bool oldSphIsec = sphIsec;
 227     if (!enterBoundingSphere(drawable->getBound()))
 228       return;
 229
 230     bool oldBackfaceCulling = mBackfaceCulling;
 231     updateCullMode(drawable->getStateSet());
 232
 233     FGGroundCache::GroundProperty& gp = mGroundProperty;
 234     // get some material information for use in the gear model
 235     gp.material = globals->get_matlib()->findMaterial(drawable->getStateSet());
 236     if (gp.material)
 237       gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
 238
 239     drawable->accept(mTriangleFunctor);
 240
 241     mBackfaceCulling = oldBackfaceCulling;
 242     sphIsec = oldSphIsec;
 243   }
 244
 245   virtual void apply(osg::Geode& geode)
 246   {
 247     bool oldBackfaceCulling = mBackfaceCulling;
 248     bool oldSphIsec = sphIsec;
 249     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 250     if (!enterNode(geode))
 251       return;
 252
 253     for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
 254       fillWith(geode.getDrawable(i));
 255     sphIsec = oldSphIsec;
 256     mGroundProperty = oldGp;
 257     mBackfaceCulling = oldBackfaceCulling;
 258   }
 259
 260   virtual void apply(osg::Group& group)
 261   {
 262     bool oldBackfaceCulling = mBackfaceCulling;
 263     bool oldSphIsec = sphIsec;
 264     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 265     if (!enterNode(group))
 266       return;
 267     traverse(group);
 268     sphIsec = oldSphIsec;
 269     mBackfaceCulling = oldBackfaceCulling;
 270     mGroundProperty = oldGp;
 271   }
 272
 273   virtual void apply(osg::Transform& transform)
 274   {
 275     if (!enterNode(transform))
 276       return;
 277     bool oldBackfaceCulling = mBackfaceCulling;
 278     bool oldSphIsec = sphIsec;
 279     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 280     /// transform the caches center to local coords
 281     osg::Matrix oldLocalToGlobal = mLocalToGlobal;
 282     osg::Matrix oldGlobalToLocal = mGlobalToLocal;
 283     transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
 284     transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
 285
 286     SGVec3d oldLocalCacheReference = mLocalCacheReference;
 287     mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
 288     SGVec3d oldLocalDown = mLocalDown;
 289     mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
 290
 291     // walk the children
 292     traverse(transform);
 293
 294     // Restore that one
 295     mLocalDown = oldLocalDown;
 296     mLocalCacheReference = oldLocalCacheReference;
 297     mLocalToGlobal = oldLocalToGlobal;
 298     mGlobalToLocal = oldGlobalToLocal;
 299     sphIsec = oldSphIsec;
 300     mBackfaceCulling = oldBackfaceCulling;
 301     mGroundProperty = oldGp;
 302   }
 303
 304   void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
 305                    const osg::Vec3& v3)
 306   {
 307     SGVec3d v[3] = {
 308       SGVec3d(v1),
 309       SGVec3d(v2),
 310       SGVec3d(v3)
 311     };
 312
 313     // a bounding sphere in the node local system
 314     SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
 315     double boundRadius = std::max(distSqr(v[0], boundCenter),
 316                                   distSqr(v[1], boundCenter));
 317     boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
 318     boundRadius = sqrt(boundRadius);
 319
 320     SGRayd ray(mLocalCacheReference, mLocalDown);
 321
 322     // if we are not in the downward cylinder bail out
 323     if (!intersectsInf(ray, SGSphered(boundCenter, boundRadius + mCacheRadius)))
 324       return;
 325
 326     SGTriangled triangle(v);
 327
 328     // The normal and plane in the node local coordinate system
 329     SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
 330     if (0 < dot(mLocalDown, n)) {
 331       if (mBackfaceCulling) {
 332         // Surface points downwards, ignore for altitude computations.
 333         return;
 334       } else {
 335         triangle.flip();
 336       }
 337     }
 338
 339     // Only check if the triangle is in the cache sphere if the plane
 340     // containing the triangle is near enough
 341     if (sphIsec) {
 342       double d = dot(n, v[0] - mLocalCacheReference);
 343       if (d*d < mCacheRadius*dot(n, n)) {
 344         // Check if the sphere around the vehicle intersects the sphere
 345         // around that triangle. If so, put that triangle into the cache.
 346         double r2 = boundRadius + mCacheRadius;
 347         if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
 348           FGGroundCache::Triangle t;
 349           t.triangle.setBaseVertex(SGVec3d(v[0].osg()*mLocalToGlobal));
 350           t.triangle.setEdge(0, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(0).osg(), mLocalToGlobal)));
 351           t.triangle.setEdge(1, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(1).osg(), mLocalToGlobal)));
 352
 353           t.sphere.setCenter(SGVec3d(boundCenter.osg()*mLocalToGlobal));
 354           t.sphere.setRadius(boundRadius);
 355
 356           t.velocity = mGroundProperty.vel;
 357           t.rotation = mGroundProperty.rot;
 358           t.rotation_pivot = mGroundProperty.pivot;
 359           t.type = mGroundProperty.type;
 360           t.material = mGroundProperty.material;
 361           mGroundCache->triangles.push_back(t);
 362         }
 363       }
 364     }
 365
 366     // In case the cache is empty, we still provide agl computations.
 367     // But then we use the old way of having a fixed elevation value for
 368     // the whole lifetime of this cache.
 369     SGVec3d isectpoint;
 370     if (intersects(isectpoint, triangle, ray, 1e-4)) {
 371       mGroundCache->found_ground = true;
 372       isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
 373       double this_radius = length(isectpoint);
 374       if (mGroundCache->ground_radius < this_radius) {
 375         mGroundCache->ground_radius = this_radius;
 376         mGroundCache->_type = mGroundProperty.type;
 377         mGroundCache->_material = mGroundProperty.material;
 378       }
 379     }
 380   }
 381
 382   void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
 383   {
 384     SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
 385     SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
 386
 387     SGVec3d boundCenter = 0.5*(gv1 + gv2);
 388     double boundRadius = length(gv1 - boundCenter);
 389
 390     if (distSqr(boundCenter, mCacheReference)
 391         < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
 392       if (mGroundProperty.type == FGInterface::Wire) {
 393         FGGroundCache::Wire wire;
 394         wire.ends[0] = gv1;
 395         wire.ends[1] = gv2;
 396         wire.velocity = mGroundProperty.vel;
 397         wire.rotation = mGroundProperty.rot;
 398         wire.rotation_pivot = mGroundProperty.pivot;
 399         wire.wire_id = mGroundProperty.wire_id;
 400
 401         mGroundCache->wires.push_back(wire);
 402       }
 403       if (mGroundProperty.type == FGInterface::Catapult) {
 404         FGGroundCache::Catapult cat;
 405         // Trick to get the ends in the right order.
 406         // Use the x axis in the original coordinate system. Choose the
 407         // most negative x-axis as the one pointing forward
 408         if (v1[0] > v2[0]) {
 409           cat.start = gv1;
 410           cat.end = gv2;
 411         } else {
 412           cat.start = gv2;
 413           cat.end = gv1;
 414         }
 415         cat.velocity = mGroundProperty.vel;
 416         cat.rotation = mGroundProperty.rot;
 417         cat.rotation_pivot = mGroundProperty.pivot;
 418
 419         mGroundCache->catapults.push_back(cat);
 420       }
 421     }
 422   }
 423
 424   SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
 425   FGGroundCache* mGroundCache;
 426   SGVec3d mCacheReference;
 427   double mCacheRadius;
 428   double mWireCacheRadius;
 429   osg::Matrix mLocalToGlobal;
 430   osg::Matrix mGlobalToLocal;
 431   SGVec3d mDown;
 432   SGVec3d mLocalDown;
 433   SGVec3d mLocalCacheReference;
 434   bool sphIsec;
 435   bool mBackfaceCulling;
 436   FGGroundCache::GroundProperty mGroundProperty;
 437 };
 438
 439 FGGroundCache::FGGroundCache()
 440 {
 441   ground_radius = 0.0;
 442   cache_ref_time = 0.0;
 443   wire_id = 0;
 444   reference_wgs84_point = SGVec3d(0, 0, 0);
 445   reference_vehicle_radius = 0.0;
 446   found_ground = false;
 447 }
 448
 449 FGGroundCache::~FGGroundCache()
 450 {
 451 }
 452
 453 inline void
 454 FGGroundCache::velocityTransformTriangle(double dt,
 455                                          SGTriangled& dst, SGSphered& sdst,
 456                                          const FGGroundCache::Triangle& src)
 457 {
 458   dst = src.triangle;
 459   sdst = src.sphere;
 460
 461   if (dt*dt*dot(src.velocity, src.velocity) < SGLimitsd::epsilon())
 462     return;
 463
 464   SGVec3d baseVert = dst.getBaseVertex();
 465   SGVec3d pivotoff = baseVert - src.rotation_pivot;
 466   baseVert += dt*(src.velocity + cross(src.rotation, pivotoff));
 467   dst.setBaseVertex(baseVert);
 468   dst.setEdge(0, dst.getEdge(0) + dt*cross(src.rotation, dst.getEdge(0)));
 469   dst.setEdge(1, dst.getEdge(1) + dt*cross(src.rotation, dst.getEdge(1)));
 470 }
 471
 472
 473 bool
 474 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
 475                                     double rad)
 476 {
 477   // Empty cache.
 478   ground_radius = 0.0;
 479   found_ground = false;
 480   triangles.resize(0);
 481   catapults.resize(0);
 482   wires.resize(0);
 483
 484   // Store the parameters we used to build up that cache.
 485   reference_wgs84_point = pt;
 486   reference_vehicle_radius = rad;
 487   // Store the time reference used to compute movements of moving triangles.
 488   cache_ref_time = ref_time;
 489
 490   // Get a normalized down vector valid for the whole cache
 491   SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
 492   down = hlToEc.rotate(SGVec3d(0, 0, 1));
 493
 494   // Prepare sphere around the aircraft.
 495   double cacheRadius = rad;
 496
 497   // Prepare bigger sphere around the aircraft.
 498   // This one is required for reliably finding wires we have caught but
 499   // have already left the hopefully smaller sphere for the ground reactions.
 500   const double max_wire_dist = 300.0;
 501   double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
 502
 503   // Walk the scene graph and extract solid ground triangles and carrier data.
 504   GroundCacheFillVisitor gcfv(this, down, pt, cacheRadius, wireCacheRadius);
 505   globals->get_scenery()->get_scene_graph()->accept(gcfv);
 506
 507   // some stats
 508   SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
 509          << ", # triangles = " << triangles.size()
 510          << ", # wires = " << wires.size()
 511          << ", # catapults = " << catapults.size()
 512          << ", ground_radius = " << ground_radius );
 513
 514   // If the ground radius is still below 5e6 meters, then we do not yet have
 515   // any scenery.
 516   found_ground = found_ground && 5e6 < ground_radius;
 517   if (!found_ground)
 518     SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
 519            "without any scenery below the aircraft" );
 520
 521   return found_ground;
 522 }
 523
 524 bool
 525 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 526 {
 527   pt = reference_wgs84_point;
 528   rad = reference_vehicle_radius;
 529   ref_time = cache_ref_time;
 530   return found_ground;
 531 }
 532
 533 double
 534 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
 535                        SGVec3d end[2], SGVec3d vel[2])
 536 {
 537   // start with a distance of 1e10 meters...
 538   double dist = 1e10;
 539
 540   // Time difference to the reference time.
 541   t -= cache_ref_time;
 542
 543   size_t sz = catapults.size();
 544   for (size_t i = 0; i < sz; ++i) {
 545     SGVec3d pivotoff, rvel[2];
 546     pivotoff = catapults[i].start - catapults[i].rotation_pivot;
 547     rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 548     pivotoff = catapults[i].end - catapults[i].rotation_pivot;
 549     rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 550
 551     SGVec3d thisEnd[2];
 552     thisEnd[0] = catapults[i].start + t*rvel[0];
 553     thisEnd[1] = catapults[i].end + t*rvel[1];
 554
 555     double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
 556     if (this_dist < dist) {
 557       SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
 558              << this_dist << " meters away");
 559       dist = this_dist;
 560
 561       end[0] = thisEnd[0];
 562       end[1] = thisEnd[1];
 563       vel[0] = rvel[0];
 564       vel[1] = rvel[1];
 565     }
 566   }
 567
 568   // At the end take the root, we only computed squared distances ...
 569   return sqrt(dist);
 570 }
 571
 572 bool
 573 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
 574                        SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
 575                        int *type, const SGMaterial** material, double *agl)
 576 {
 577   bool ret = false;
 578
 579   *type = FGInterface::Unknown;
 580 //   *agl = 0.0;
 581   if (material)
 582     *material = 0;
 583   vel = SGVec3d(0, 0, 0);
 584   contact = SGVec3d(0, 0, 0);
 585   normal = SGVec3d(0, 0, 0);
 586
 587   // Time difference to th reference time.
 588   t -= cache_ref_time;
 589
 590   // The double valued point we start to search for intersection.
 591   SGVec3d pt = dpt;
 592   // shift the start of our ray by maxaltoff upwards
 593   SGRayd ray(pt - max_altoff*down, down);
 594
 595   // Initialize to something sensible
 596   double current_radius = 0.0;
 597
 598   size_t sz = triangles.size();
 599   for (size_t i = 0; i < sz; ++i) {
 600     SGSphered sphere;
 601     SGTriangled triangle;
 602     velocityTransformTriangle(t, triangle, sphere, triangles[i]);
 603     if (!intersectsInf(ray, sphere))
 604       continue;
 605
 606     // Check for intersection.
 607     SGVec3d isecpoint;
 608     if (intersects(isecpoint, triangle, ray, 1e-4)) {
 609       // Compute the vector from pt to the intersection point ...
 610       SGVec3d off = isecpoint - pt;
 611       // ... and check if it is too high or not
 612
 613       // compute the radius, good enough approximation to take the geocentric radius
 614       double radius = dot(isecpoint, isecpoint);
 615       if (current_radius < radius) {
 616         current_radius = radius;
 617         ret = true;
 618         // Save the new potential intersection point.
 619         contact = isecpoint;
 620         // The first three values in the vector are the plane normal.
 621         normal = triangle.getNormal();
 622         // The velocity wrt earth.
 623         SGVec3d pivotoff = pt - triangles[i].rotation_pivot;
 624         vel = triangles[i].velocity + cross(triangles[i].rotation, pivotoff);
 625         // Save the ground type.
 626         *type = triangles[i].type;
 627         *agl = dot(down, contact - dpt);
 628         if (material)
 629           *material = triangles[i].material;
 630       }
 631     }
 632   }
 633
 634   if (ret)
 635     return true;
 636
 637   // Whenever we did not have a ground triangle for the requested point,
 638   // take the ground level we found during the current cache build.
 639   // This is as good as what we had before for agl.
 640   double r = length(dpt);
 641   contact = dpt;
 642   contact *= ground_radius/r;
 643   normal = -down;
 644   vel = SGVec3d(0, 0, 0);
 645
 646   // The altitude is the distance of the requested point from the
 647   // contact point.
 648   *agl = dot(down, contact - dpt);
 649   *type = _type;
 650   if (material)
 651     *material = _material;
 652
 653   return ret;
 654 }
 655
 656 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 657 {
 658   size_t sz = wires.size();
 659   if (sz == 0)
 660     return false;
 661
 662   // Time difference to the reference time.
 663   t -= cache_ref_time;
 664
 665   // Build the two triangles spanning the area where the hook has moved
 666   // during the past step.
 667   SGTriangled triangle[2];
 668   triangle[0].set(pt[0], pt[1], pt[2]);
 669   triangle[1].set(pt[0], pt[2], pt[3]);
 670
 671   // Intersect the wire lines with each of these triangles.
 672   // You have caught a wire if they intersect.
 673   for (size_t i = 0; i < sz; ++i) {
 674     SGVec3d le[2];
 675     for (int k = 0; k < 2; ++k) {
 676       le[k] = wires[i].ends[k];
 677       SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
 678       SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 679       le[k] += t*vel;
 680     }
 681     SGLineSegmentd lineSegment(le[0], le[1]);
 682
 683     for (int k=0; k<2; ++k) {
 684       if (intersects(triangle[k], lineSegment)) {
 685         SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
 686         // Store the wire id.
 687         wire_id = wires[i].wire_id;
 688         return true;
 689       }
 690     }
 691   }
 692
 693   return false;
 694 }
 695
 696 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 697 {
 698   // Fast return if we do not have an active wire.
 699   if (wire_id < 0)
 700     return false;
 701
 702   // Time difference to the reference time.
 703   t -= cache_ref_time;
 704
 705   // Search for the wire with the matching wire id.
 706   size_t sz = wires.size();
 707   for (size_t i = 0; i < sz; ++i) {
 708     if (wires[i].wire_id == wire_id) {
 709       for (size_t k = 0; k < 2; ++k) {
 710         SGVec3d pivotoff = wires[i].ends[k] - wires[i].rotation_pivot;
 711         vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 712         end[k] = wires[i].ends[k] + t*vel[k];
 713       }
 714       return true;
 715     }
 716   }
 717
 718   return false;
 719 }
 720
 721 void FGGroundCache::release_wire(void)
 722 {
 723   wire_id = -1;
 724 }