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.material = globals->get_matlib()->findMaterial(&node);
 198     if (gp.material) {
 199       gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
 200       return true;
 201     }
 202     gp.type = FGInterface::Unknown;
 203     osg::Referenced* base = node.getUserData();
 204     if (!base)
 205       return true;
 206     FGAICarrierHardware *ud =
 207       dynamic_cast<FGAICarrierHardware*>(base);
 208     if (!ud)
 209       return true;
 210
 211     switch (ud->type) {
 212     case FGAICarrierHardware::Wire:
 213       gp.type = FGInterface::Wire;
 214       gp.wire_id = ud->id;
 215       break;
 216     case FGAICarrierHardware::Catapult:
 217       gp.type = FGInterface::Catapult;
 218       break;
 219     default:
 220       gp.type = FGInterface::Solid;
 221       break;
 222     }
 223     // Copy the velocity from the carrier class.
 224     ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
 225
 226     return true;
 227   }
 228
 229   void fillWith(osg::Drawable* drawable)
 230   {
 231     bool oldSphIsec = sphIsec;
 232     if (!enterBoundingSphere(drawable->getBound()))
 233       return;
 234
 235     bool oldBackfaceCulling = mBackfaceCulling;
 236     updateCullMode(drawable->getStateSet());
 237
 238     drawable->accept(mTriangleFunctor);
 239
 240     mBackfaceCulling = oldBackfaceCulling;
 241     sphIsec = oldSphIsec;
 242   }
 243
 244   virtual void apply(osg::Geode& geode)
 245   {
 246     bool oldBackfaceCulling = mBackfaceCulling;
 247     bool oldSphIsec = sphIsec;
 248     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 249     if (!enterNode(geode))
 250       return;
 251
 252     for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
 253       fillWith(geode.getDrawable(i));
 254     sphIsec = oldSphIsec;
 255     mGroundProperty = oldGp;
 256     mBackfaceCulling = oldBackfaceCulling;
 257   }
 258
 259   virtual void apply(osg::Group& group)
 260   {
 261     bool oldBackfaceCulling = mBackfaceCulling;
 262     bool oldSphIsec = sphIsec;
 263     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 264     if (!enterNode(group))
 265       return;
 266     traverse(group);
 267     sphIsec = oldSphIsec;
 268     mBackfaceCulling = oldBackfaceCulling;
 269     mGroundProperty = oldGp;
 270   }
 271
 272   virtual void apply(osg::Transform& transform)
 273   {
 274     if (!enterNode(transform))
 275       return;
 276     bool oldBackfaceCulling = mBackfaceCulling;
 277     bool oldSphIsec = sphIsec;
 278     FGGroundCache::GroundProperty oldGp = mGroundProperty;
 279     /// transform the caches center to local coords
 280     osg::Matrix oldLocalToGlobal = mLocalToGlobal;
 281     osg::Matrix oldGlobalToLocal = mGlobalToLocal;
 282     transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
 283     transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
 284
 285     SGVec3d oldLocalCacheReference = mLocalCacheReference;
 286     mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
 287     SGVec3d oldLocalDown = mLocalDown;
 288     mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
 289
 290     // walk the children
 291     traverse(transform);
 292
 293     // Restore that one
 294     mLocalDown = oldLocalDown;
 295     mLocalCacheReference = oldLocalCacheReference;
 296     mLocalToGlobal = oldLocalToGlobal;
 297     mGlobalToLocal = oldGlobalToLocal;
 298     sphIsec = oldSphIsec;
 299     mBackfaceCulling = oldBackfaceCulling;
 300     mGroundProperty = oldGp;
 301   }
 302
 303   void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
 304                    const osg::Vec3& v3)
 305   {
 306     SGVec3d v[3] = {
 307       SGVec3d(v1),
 308       SGVec3d(v2),
 309       SGVec3d(v3)
 310     };
 311
 312     // a bounding sphere in the node local system
 313     SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
 314     double boundRadius = std::max(distSqr(v[0], boundCenter),
 315                                   distSqr(v[1], boundCenter));
 316     boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
 317     boundRadius = sqrt(boundRadius);
 318
 319     SGRayd ray(mLocalCacheReference, mLocalDown);
 320
 321     // if we are not in the downward cylinder bail out
 322     if (!intersectsInf(ray, SGSphered(boundCenter, boundRadius + mCacheRadius)))
 323       return;
 324
 325     SGTriangled triangle(v);
 326
 327     // The normal and plane in the node local coordinate system
 328     SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
 329     if (0 < dot(mLocalDown, n)) {
 330       if (mBackfaceCulling) {
 331         // Surface points downwards, ignore for altitude computations.
 332         return;
 333       } else {
 334         triangle.flip();
 335       }
 336     }
 337
 338     // Only check if the triangle is in the cache sphere if the plane
 339     // containing the triangle is near enough
 340     if (sphIsec) {
 341       double d = dot(n, v[0] - mLocalCacheReference);
 342       if (d*d < mCacheRadius*dot(n, n)) {
 343         // Check if the sphere around the vehicle intersects the sphere
 344         // around that triangle. If so, put that triangle into the cache.
 345         double r2 = boundRadius + mCacheRadius;
 346         if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
 347           FGGroundCache::Triangle t;
 348           t.triangle.setBaseVertex(SGVec3d(v[0].osg()*mLocalToGlobal));
 349           t.triangle.setEdge(0, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(0).osg(), mLocalToGlobal)));
 350           t.triangle.setEdge(1, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(1).osg(), mLocalToGlobal)));
 351
 352           t.sphere.setCenter(SGVec3d(boundCenter.osg()*mLocalToGlobal));
 353           t.sphere.setRadius(boundRadius);
 354
 355           t.velocity = mGroundProperty.vel;
 356           t.rotation = mGroundProperty.rot;
 357           t.rotation_pivot = mGroundProperty.pivot;
 358           t.type = mGroundProperty.type;
 359           t.material = mGroundProperty.material;
 360           mGroundCache->triangles.push_back(t);
 361         }
 362       }
 363     }
 364
 365     // In case the cache is empty, we still provide agl computations.
 366     // But then we use the old way of having a fixed elevation value for
 367     // the whole lifetime of this cache.
 368     SGVec3d isectpoint;
 369     if (intersects(isectpoint, triangle, ray, 1e-4)) {
 370       mGroundCache->found_ground = true;
 371       isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
 372       double this_radius = length(isectpoint);
 373       if (mGroundCache->ground_radius < this_radius) {
 374         mGroundCache->ground_radius = this_radius;
 375         mGroundCache->_type = mGroundProperty.type;
 376         mGroundCache->_material = mGroundProperty.material;
 377       }
 378     }
 379   }
 380
 381   void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
 382   {
 383     SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
 384     SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
 385
 386     SGVec3d boundCenter = 0.5*(gv1 + gv2);
 387     double boundRadius = length(gv1 - boundCenter);
 388
 389     if (distSqr(boundCenter, mCacheReference)
 390         < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
 391       if (mGroundProperty.type == FGInterface::Wire) {
 392         FGGroundCache::Wire wire;
 393         wire.ends[0] = gv1;
 394         wire.ends[1] = gv2;
 395         wire.velocity = mGroundProperty.vel;
 396         wire.rotation = mGroundProperty.rot;
 397         wire.rotation_pivot = mGroundProperty.pivot;
 398         wire.wire_id = mGroundProperty.wire_id;
 399
 400         mGroundCache->wires.push_back(wire);
 401       }
 402       if (mGroundProperty.type == FGInterface::Catapult) {
 403         FGGroundCache::Catapult cat;
 404         // Trick to get the ends in the right order.
 405         // Use the x axis in the original coordinate system. Choose the
 406         // most negative x-axis as the one pointing forward
 407         if (v1[0] > v2[0]) {
 408           cat.start = gv1;
 409           cat.end = gv2;
 410         } else {
 411           cat.start = gv2;
 412           cat.end = gv1;
 413         }
 414         cat.velocity = mGroundProperty.vel;
 415         cat.rotation = mGroundProperty.rot;
 416         cat.rotation_pivot = mGroundProperty.pivot;
 417
 418         mGroundCache->catapults.push_back(cat);
 419       }
 420     }
 421   }
 422
 423   SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
 424   FGGroundCache* mGroundCache;
 425   SGVec3d mCacheReference;
 426   double mCacheRadius;
 427   double mWireCacheRadius;
 428   osg::Matrix mLocalToGlobal;
 429   osg::Matrix mGlobalToLocal;
 430   SGVec3d mDown;
 431   SGVec3d mLocalDown;
 432   SGVec3d mLocalCacheReference;
 433   bool sphIsec;
 434   bool mBackfaceCulling;
 435   FGGroundCache::GroundProperty mGroundProperty;
 436 };
 437
 438 FGGroundCache::FGGroundCache()
 439 {
 440   ground_radius = 0.0;
 441   cache_ref_time = 0.0;
 442   wire_id = 0;
 443   reference_wgs84_point = SGVec3d(0, 0, 0);
 444   reference_vehicle_radius = 0.0;
 445   found_ground = false;
 446 }
 447
 448 FGGroundCache::~FGGroundCache()
 449 {
 450 }
 451
 452 inline void
 453 FGGroundCache::velocityTransformTriangle(double dt,
 454                                          SGTriangled& dst, SGSphered& sdst,
 455                                          const FGGroundCache::Triangle& src)
 456 {
 457   dst = src.triangle;
 458   sdst = src.sphere;
 459
 460   if (dt*dt*dot(src.velocity, src.velocity) < SGLimitsd::epsilon())
 461     return;
 462
 463   SGVec3d baseVert = dst.getBaseVertex();
 464   SGVec3d pivotoff = baseVert - src.rotation_pivot;
 465   baseVert += dt*(src.velocity + cross(src.rotation, pivotoff));
 466   dst.setBaseVertex(baseVert);
 467   dst.setEdge(0, dst.getEdge(0) + dt*cross(src.rotation, dst.getEdge(0)));
 468   dst.setEdge(1, dst.getEdge(1) + dt*cross(src.rotation, dst.getEdge(1)));
 469 }
 470
 471
 472 bool
 473 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
 474                                     double rad)
 475 {
 476   // Empty cache.
 477   ground_radius = 0.0;
 478   found_ground = false;
 479   triangles.resize(0);
 480   catapults.resize(0);
 481   wires.resize(0);
 482
 483   // Store the parameters we used to build up that cache.
 484   reference_wgs84_point = pt;
 485   reference_vehicle_radius = rad;
 486   // Store the time reference used to compute movements of moving triangles.
 487   cache_ref_time = ref_time;
 488
 489   // Get a normalized down vector valid for the whole cache
 490   SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
 491   down = hlToEc.rotate(SGVec3d(0, 0, 1));
 492
 493   // Prepare sphere around the aircraft.
 494   double cacheRadius = rad;
 495
 496   // Prepare bigger sphere around the aircraft.
 497   // This one is required for reliably finding wires we have caught but
 498   // have already left the hopefully smaller sphere for the ground reactions.
 499   const double max_wire_dist = 300.0;
 500   double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
 501
 502   // Walk the scene graph and extract solid ground triangles and carrier data.
 503   GroundCacheFillVisitor gcfv(this, down, pt, cacheRadius, wireCacheRadius);
 504   globals->get_scenery()->get_scene_graph()->accept(gcfv);
 505
 506   // some stats
 507   SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
 508          << ", # triangles = " << triangles.size()
 509          << ", # wires = " << wires.size()
 510          << ", # catapults = " << catapults.size()
 511          << ", ground_radius = " << ground_radius );
 512
 513   // If the ground radius is still below 5e6 meters, then we do not yet have
 514   // any scenery.
 515   found_ground = found_ground && 5e6 < ground_radius;
 516   if (!found_ground)
 517     SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
 518            "without any scenery below the aircraft" );
 519
 520   return found_ground;
 521 }
 522
 523 bool
 524 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 525 {
 526   pt = reference_wgs84_point;
 527   rad = reference_vehicle_radius;
 528   ref_time = cache_ref_time;
 529   return found_ground;
 530 }
 531
 532 double
 533 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
 534                        SGVec3d end[2], SGVec3d vel[2])
 535 {
 536   // start with a distance of 1e10 meters...
 537   double dist = 1e10;
 538
 539   // Time difference to the reference time.
 540   t -= cache_ref_time;
 541
 542   size_t sz = catapults.size();
 543   for (size_t i = 0; i < sz; ++i) {
 544     SGVec3d pivotoff, rvel[2];
 545     pivotoff = catapults[i].start - catapults[i].rotation_pivot;
 546     rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 547     pivotoff = catapults[i].end - catapults[i].rotation_pivot;
 548     rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 549
 550     SGVec3d thisEnd[2];
 551     thisEnd[0] = catapults[i].start + t*rvel[0];
 552     thisEnd[1] = catapults[i].end + t*rvel[1];
 553
 554     double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
 555     if (this_dist < dist) {
 556       SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
 557              << this_dist << " meters away");
 558       dist = this_dist;
 559
 560       end[0] = thisEnd[0];
 561       end[1] = thisEnd[1];
 562       vel[0] = rvel[0];
 563       vel[1] = rvel[1];
 564     }
 565   }
 566
 567   // At the end take the root, we only computed squared distances ...
 568   return sqrt(dist);
 569 }
 570
 571 bool
 572 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
 573                        SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
 574                        int *type, const SGMaterial** material, double *agl)
 575 {
 576   bool ret = false;
 577
 578   *type = FGInterface::Unknown;
 579 //   *agl = 0.0;
 580   if (material)
 581     *material = 0;
 582   vel = SGVec3d(0, 0, 0);
 583   contact = SGVec3d(0, 0, 0);
 584   normal = SGVec3d(0, 0, 0);
 585
 586   // Time difference to th reference time.
 587   t -= cache_ref_time;
 588
 589   // The double valued point we start to search for intersection.
 590   SGVec3d pt = dpt;
 591   // shift the start of our ray by maxaltoff upwards
 592   SGRayd ray(pt - max_altoff*down, down);
 593
 594   // Initialize to something sensible
 595   double current_radius = 0.0;
 596
 597   size_t sz = triangles.size();
 598   for (size_t i = 0; i < sz; ++i) {
 599     SGSphered sphere;
 600     SGTriangled triangle;
 601     velocityTransformTriangle(t, triangle, sphere, triangles[i]);
 602     if (!intersectsInf(ray, sphere))
 603       continue;
 604
 605     // Check for intersection.
 606     SGVec3d isecpoint;
 607     if (intersects(isecpoint, triangle, ray, 1e-4)) {
 608       // Compute the vector from pt to the intersection point ...
 609       SGVec3d off = isecpoint - pt;
 610       // ... and check if it is too high or not
 611
 612       // compute the radius, good enough approximation to take the geocentric radius
 613       double radius = dot(isecpoint, isecpoint);
 614       if (current_radius < radius) {
 615         current_radius = radius;
 616         ret = true;
 617         // Save the new potential intersection point.
 618         contact = isecpoint;
 619         // The first three values in the vector are the plane normal.
 620         normal = triangle.getNormal();
 621         // The velocity wrt earth.
 622         SGVec3d pivotoff = pt - triangles[i].rotation_pivot;
 623         vel = triangles[i].velocity + cross(triangles[i].rotation, pivotoff);
 624         // Save the ground type.
 625         *type = triangles[i].type;
 626         *agl = dot(down, contact - dpt);
 627         if (material)
 628           *material = triangles[i].material;
 629       }
 630     }
 631   }
 632
 633   if (ret)
 634     return true;
 635
 636   // Whenever we did not have a ground triangle for the requested point,
 637   // take the ground level we found during the current cache build.
 638   // This is as good as what we had before for agl.
 639   double r = length(dpt);
 640   contact = dpt;
 641   contact *= ground_radius/r;
 642   normal = -down;
 643   vel = SGVec3d(0, 0, 0);
 644
 645   // The altitude is the distance of the requested point from the
 646   // contact point.
 647   *agl = dot(down, contact - dpt);
 648   *type = _type;
 649   if (material)
 650     *material = _material;
 651
 652   return ret;
 653 }
 654
 655 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 656 {
 657   size_t sz = wires.size();
 658   if (sz == 0)
 659     return false;
 660
 661   // Time difference to the reference time.
 662   t -= cache_ref_time;
 663
 664   // Build the two triangles spanning the area where the hook has moved
 665   // during the past step.
 666   SGTriangled triangle[2];
 667   triangle[0].set(pt[0], pt[1], pt[2]);
 668   triangle[1].set(pt[0], pt[2], pt[3]);
 669
 670   // Intersect the wire lines with each of these triangles.
 671   // You have caught a wire if they intersect.
 672   for (size_t i = 0; i < sz; ++i) {
 673     SGVec3d le[2];
 674     for (int k = 0; k < 2; ++k) {
 675       le[k] = wires[i].ends[k];
 676       SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
 677       SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 678       le[k] += t*vel;
 679     }
 680     SGLineSegmentd lineSegment(le[0], le[1]);
 681
 682     for (int k=0; k<2; ++k) {
 683       if (intersects(triangle[k], lineSegment)) {
 684         SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
 685         // Store the wire id.
 686         wire_id = wires[i].wire_id;
 687         return true;
 688       }
 689     }
 690   }
 691
 692   return false;
 693 }
 694
 695 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 696 {
 697   // Fast return if we do not have an active wire.
 698   if (wire_id < 0)
 699     return false;
 700
 701   // Time difference to the reference time.
 702   t -= cache_ref_time;
 703
 704   // Search for the wire with the matching wire id.
 705   size_t sz = wires.size();
 706   for (size_t i = 0; i < sz; ++i) {
 707     if (wires[i].wire_id == wire_id) {
 708       for (size_t k = 0; k < 2; ++k) {
 709         SGVec3d pivotoff = wires[i].ends[k] - wires[i].rotation_pivot;
 710         vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 711         end[k] = wires[i].ends[k] + t*vel[k];
 712       }
 713       return true;
 714     }
 715   }
 716
 717   return false;
 718 }
 719
 720 void FGGroundCache::release_wire(void)
 721 {
 722   wire_id = -1;
 723 }