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