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