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     unsigned int numDrawables = geode.getNumDrawables();
 253     for(unsigned i = 0; i < numDrawables; ++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   ground_radius(0.0),
 441   _type(0),
 442   _material(0),
 443   cache_ref_time(0.0),
 444   wire_id(0),
 445   reference_wgs84_point(SGVec3d(0, 0, 0)),
 446   reference_vehicle_radius(0.0),
 447   down(0.0, 0.0, 0.0),
 448   found_ground(false)
 449 {
 450 }
 451
 452 FGGroundCache::~FGGroundCache()
 453 {
 454 }
 455
 456 inline void
 457 FGGroundCache::velocityTransformTriangle(double dt,
 458                                          SGTriangled& dst, SGSphered& sdst,
 459                                          const FGGroundCache::Triangle& src)
 460 {
 461   dst = src.triangle;
 462   sdst = src.sphere;
 463
 464   if (dt*dt*dot(src.velocity, src.velocity) < SGLimitsd::epsilon())
 465     return;
 466
 467   SGVec3d baseVert = dst.getBaseVertex();
 468   SGVec3d pivotoff = baseVert - src.rotation_pivot;
 469   baseVert += dt*(src.velocity + cross(src.rotation, pivotoff));
 470   dst.setBaseVertex(baseVert);
 471   dst.setEdge(0, dst.getEdge(0) + dt*cross(src.rotation, dst.getEdge(0)));
 472   dst.setEdge(1, dst.getEdge(1) + dt*cross(src.rotation, dst.getEdge(1)));
 473 }
 474
 475
 476 bool
 477 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
 478                                     double rad)
 479 {
 480   // Empty cache.
 481   found_ground = false;
 482   SGGeod geodPt = SGGeod::fromCart(pt);
 483   // Don't blow away the cache ground_radius and stuff if there's no
 484   // scenery
 485   if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
 486                                                   geodPt.getLongitudeDeg(),
 487                                                   rad))
 488     return false;
 489   ground_radius = 0.0;
 490   triangles.resize(0);
 491   catapults.resize(0);
 492   wires.resize(0);
 493
 494   // Store the parameters we used to build up that cache.
 495   reference_wgs84_point = pt;
 496   reference_vehicle_radius = rad;
 497   // Store the time reference used to compute movements of moving triangles.
 498   cache_ref_time = ref_time;
 499
 500   // Get a normalized down vector valid for the whole cache
 501   SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
 502   down = hlToEc.rotate(SGVec3d(0, 0, 1));
 503
 504   // Prepare sphere around the aircraft.
 505   double cacheRadius = rad;
 506
 507   // Prepare bigger sphere around the aircraft.
 508   // This one is required for reliably finding wires we have caught but
 509   // have already left the hopefully smaller sphere for the ground reactions.
 510   const double max_wire_dist = 300.0;
 511   double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
 512
 513   // Walk the scene graph and extract solid ground triangles and carrier data.
 514   GroundCacheFillVisitor gcfv(this, down, pt, cacheRadius, wireCacheRadius);
 515   globals->get_scenery()->get_scene_graph()->accept(gcfv);
 516
 517   // some stats
 518   SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
 519          << ", # triangles = " << triangles.size()
 520          << ", # wires = " << wires.size()
 521          << ", # catapults = " << catapults.size()
 522          << ", ground_radius = " << ground_radius );
 523
 524   // If the ground radius is still below 5e6 meters, then we do not yet have
 525   // any scenery.
 526   found_ground = found_ground && 5e6 < ground_radius;
 527   if (!found_ground)
 528     SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
 529            "without any scenery below the aircraft" );
 530
 531   return found_ground;
 532 }
 533
 534 bool
 535 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 536 {
 537   pt = reference_wgs84_point;
 538   rad = reference_vehicle_radius;
 539   ref_time = cache_ref_time;
 540   return found_ground;
 541 }
 542
 543 double
 544 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
 545                        SGVec3d end[2], SGVec3d vel[2])
 546 {
 547   // start with a distance of 1e10 meters...
 548   double dist = 1e10;
 549
 550   // Time difference to the reference time.
 551   t -= cache_ref_time;
 552
 553   size_t sz = catapults.size();
 554   for (size_t i = 0; i < sz; ++i) {
 555     SGVec3d pivotoff, rvel[2];
 556     pivotoff = catapults[i].start - catapults[i].rotation_pivot;
 557     rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 558     pivotoff = catapults[i].end - catapults[i].rotation_pivot;
 559     rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
 560
 561     SGVec3d thisEnd[2];
 562     thisEnd[0] = catapults[i].start + t*rvel[0];
 563     thisEnd[1] = catapults[i].end + t*rvel[1];
 564
 565     double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
 566     if (this_dist < dist) {
 567       SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
 568              << this_dist << " meters away");
 569       dist = this_dist;
 570
 571       end[0] = thisEnd[0];
 572       end[1] = thisEnd[1];
 573       vel[0] = rvel[0];
 574       vel[1] = rvel[1];
 575     }
 576   }
 577
 578   // At the end take the root, we only computed squared distances ...
 579   return sqrt(dist);
 580 }
 581
 582 bool
 583 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
 584                        SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
 585                        int *type, const SGMaterial** material, double *agl)
 586 {
 587   bool ret = false;
 588
 589   *type = FGInterface::Unknown;
 590 //   *agl = 0.0;
 591   if (material)
 592     *material = 0;
 593   vel = SGVec3d(0, 0, 0);
 594   contact = SGVec3d(0, 0, 0);
 595   normal = SGVec3d(0, 0, 0);
 596
 597   // Time difference to th reference time.
 598   t -= cache_ref_time;
 599
 600   // The double valued point we start to search for intersection.
 601   SGVec3d pt = dpt;
 602   // shift the start of our ray by maxaltoff upwards
 603   SGRayd ray(pt - max_altoff*down, down);
 604
 605   // Initialize to something sensible
 606   double current_radius = 0.0;
 607
 608   size_t sz = triangles.size();
 609   for (size_t i = 0; i < sz; ++i) {
 610     SGSphered sphere;
 611     SGTriangled triangle;
 612     velocityTransformTriangle(t, triangle, sphere, triangles[i]);
 613     if (!intersectsInf(ray, sphere))
 614       continue;
 615
 616     // Check for intersection.
 617     SGVec3d isecpoint;
 618     if (intersects(isecpoint, triangle, ray, 1e-4)) {
 619       // Compute the vector from pt to the intersection point ...
 620       SGVec3d off = isecpoint - pt;
 621       // ... and check if it is too high or not
 622
 623       // compute the radius, good enough approximation to take the geocentric radius
 624       double radius = dot(isecpoint, isecpoint);
 625       if (current_radius < radius) {
 626         current_radius = radius;
 627         ret = true;
 628         // Save the new potential intersection point.
 629         contact = isecpoint;
 630         // The first three values in the vector are the plane normal.
 631         normal = triangle.getNormal();
 632         // The velocity wrt earth.
 633         SGVec3d pivotoff = pt - triangles[i].rotation_pivot;
 634         vel = triangles[i].velocity + cross(triangles[i].rotation, pivotoff);
 635         // Save the ground type.
 636         *type = triangles[i].type;
 637         *agl = dot(down, contact - dpt);
 638         if (material)
 639           *material = triangles[i].material;
 640       }
 641     }
 642   }
 643
 644   if (ret)
 645     return true;
 646
 647   // Whenever we did not have a ground triangle for the requested point,
 648   // take the ground level we found during the current cache build.
 649   // This is as good as what we had before for agl.
 650   double r = length(dpt);
 651   contact = dpt;
 652   contact *= ground_radius/r;
 653   normal = -down;
 654   vel = SGVec3d(0, 0, 0);
 655
 656   // The altitude is the distance of the requested point from the
 657   // contact point.
 658   *agl = dot(down, contact - dpt);
 659   *type = _type;
 660   if (material)
 661     *material = _material;
 662
 663   return ret;
 664 }
 665
 666 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 667 {
 668   size_t sz = wires.size();
 669   if (sz == 0)
 670     return false;
 671
 672   // Time difference to the reference time.
 673   t -= cache_ref_time;
 674
 675   // Build the two triangles spanning the area where the hook has moved
 676   // during the past step.
 677   SGTriangled triangle[2];
 678   triangle[0].set(pt[0], pt[1], pt[2]);
 679   triangle[1].set(pt[0], pt[2], pt[3]);
 680
 681   // Intersect the wire lines with each of these triangles.
 682   // You have caught a wire if they intersect.
 683   for (size_t i = 0; i < sz; ++i) {
 684     SGVec3d le[2];
 685     for (int k = 0; k < 2; ++k) {
 686       le[k] = wires[i].ends[k];
 687       SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
 688       SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 689       le[k] += t*vel;
 690     }
 691     SGLineSegmentd lineSegment(le[0], le[1]);
 692
 693     for (int k=0; k<2; ++k) {
 694       if (intersects(triangle[k], lineSegment)) {
 695         SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
 696         // Store the wire id.
 697         wire_id = wires[i].wire_id;
 698         return true;
 699       }
 700     }
 701   }
 702
 703   return false;
 704 }
 705
 706 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 707 {
 708   // Fast return if we do not have an active wire.
 709   if (wire_id < 0)
 710     return false;
 711
 712   // Time difference to the reference time.
 713   t -= cache_ref_time;
 714
 715   // Search for the wire with the matching wire id.
 716   size_t sz = wires.size();
 717   for (size_t i = 0; i < sz; ++i) {
 718     if (wires[i].wire_id == wire_id) {
 719       for (size_t k = 0; k < 2; ++k) {
 720         SGVec3d pivotoff = wires[i].ends[k] - wires[i].rotation_pivot;
 721         vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
 722         end[k] = wires[i].ends[k] + t*vel[k];
 723       }
 724       return true;
 725     }
 726   }
 727
 728   return false;
 729 }
 730
 731 void FGGroundCache::release_wire(void)
 732 {
 733   wire_id = -1;
 734 }