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