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