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., 675 Mass Ave, Cambridge, MA 02139, 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
  31 #include <simgear/sg_inlines.h>
  32 #include <simgear/constants.h>
  33 #include <simgear/debug/logstream.hxx>
  34 #include <simgear/math/sg_geodesy.hxx>
  35
  36 #include <Main/globals.hxx>
  37 #include <Scenery/scenery.hxx>
  38 #include <Scenery/tilemgr.hxx>
  39 #include <AIModel/AICarrier.hxx>
  40
  41 #include "flight.hxx"
  42 #include "groundcache.hxx"
  43
  44 // Specialized version of sgMultMat4 needed because of mixed matrix
  45 // types
  46 static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
  47     for ( int j = 0 ; j < 4 ; j++ ) {
  48         dst[0][j] = m2[0][0] * m1[0][j] +
  49                     m2[0][1] * m1[1][j] +
  50                     m2[0][2] * m1[2][j] +
  51                     m2[0][3] * m1[3][j] ;
  52
  53         dst[1][j] = m2[1][0] * m1[0][j] +
  54                     m2[1][1] * m1[1][j] +
  55                     m2[1][2] * m1[2][j] +
  56                     m2[1][3] * m1[3][j] ;
  57
  58         dst[2][j] = m2[2][0] * m1[0][j] +
  59                     m2[2][1] * m1[1][j] +
  60                     m2[2][2] * m1[2][j] +
  61                     m2[2][3] * m1[3][j] ;
  62
  63         dst[3][j] = m2[3][0] * m1[0][j] +
  64                     m2[3][1] * m1[1][j] +
  65                     m2[3][2] * m1[2][j] +
  66                     m2[3][3] * m1[3][j] ;
  67     }
  68 }
  69
  70 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
  71 {
  72     sgdVec3 dif;
  73
  74     // Some tolerance in meters we accept a point to be outside of the triangle
  75     // and still return that it is inside.
  76     SGDfloat eps = 1e-2;
  77     SGDfloat min, max;
  78     // punt if outside bouding cube
  79     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
  80     if( (point[0] < min - eps) || (point[0] > max + eps) )
  81         return false;
  82     dif[0] = max - min;
  83
  84     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
  85     if( (point[1] < min - eps) || (point[1] > max + eps) )
  86         return false;
  87     dif[1] = max - min;
  88
  89     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
  90     if( (point[2] < min - eps) || (point[2] > max + eps) )
  91         return false;
  92     dif[2] = max - min;
  93
  94     // drop the smallest dimension so we only have to work in 2d.
  95     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
  96     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
  97     if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
  98         // x is the smallest dimension
  99         x1 = point[1];
 100         y1 = point[2];
 101         x2 = tri[0][1];
 102         y2 = tri[0][2];
 103         x3 = tri[1][1];
 104         y3 = tri[1][2];
 105         rx = tri[2][1];
 106         ry = tri[2][2];
 107     } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
 108         // y is the smallest dimension
 109         x1 = point[0];
 110         y1 = point[2];
 111         x2 = tri[0][0];
 112         y2 = tri[0][2];
 113         x3 = tri[1][0];
 114         y3 = tri[1][2];
 115         rx = tri[2][0];
 116         ry = tri[2][2];
 117     } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
 118         // z is the smallest dimension
 119         x1 = point[0];
 120         y1 = point[1];
 121         x2 = tri[0][0];
 122         y2 = tri[0][1];
 123         x3 = tri[1][0];
 124         y3 = tri[1][1];
 125         rx = tri[2][0];
 126         ry = tri[2][1];
 127     } else {
 128         // all dimensions are really small so lets call it close
 129         // enough and return a successful match
 130         return true;
 131     }
 132
 133     // check if intersection point is on the same side of p1 <-> p2 as p3
 134     SGDfloat tmp = (y2 - y3);
 135     SGDfloat tmpn = (x2 - x3);
 136     int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
 137     int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
 138                          + side1 * eps * fabs(tmpn));
 139     if ( side1 != side2 ) {
 140         // printf("failed side 1 check\n");
 141         return false;
 142     }
 143
 144     // check if intersection point is on correct side of p2 <-> p3 as p1
 145     tmp = (y3 - ry);
 146     tmpn = (x3 - rx);
 147     side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
 148     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 149                      + side1 * eps * fabs(tmpn));
 150     if ( side1 != side2 ) {
 151         // printf("failed side 2 check\n");
 152         return false;
 153     }
 154
 155     // check if intersection point is on correct side of p1 <-> p3 as p2
 156     tmp = (y2 - ry);
 157     tmpn = (x2 - rx);
 158     side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
 159     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 160                      + side1 * eps * fabs(tmpn));
 161     if ( side1 != side2 ) {
 162         // printf("failed side 3  check\n");
 163         return false;
 164     }
 165
 166     return true;
 167 }
 168
 169 // Test if the line given by the point on the line pt_on_line and the
 170 // line direction dir intersects the sphere sp.
 171 // Adapted from plib.
 172 static inline bool
 173 fgdIsectSphereInfLine(const sgdSphere& sp,
 174                       const sgdVec3 pt_on_line, const sgdVec3 dir)
 175 {
 176   sgdVec3 r;
 177   sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
 178
 179   SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
 180
 181   SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
 182     projectedDistance * projectedDistance;
 183
 184   SGDfloat radius = sp.getRadius();
 185   return dist < radius*radius;
 186 }
 187
 188 FGGroundCache::FGGroundCache()
 189 {
 190   sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
 191   ground_radius = 0.0;
 192   cache_ref_time = 0.0;
 193   wire_id = 0;
 194   sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
 195   reference_vehicle_radius = 0.0;
 196   found_ground = false;
 197 }
 198
 199 FGGroundCache::~FGGroundCache()
 200 {
 201 }
 202
 203 FGGroundCache::GroundProperty
 204 FGGroundCache::extractGroundProperty( ssgLeaf* l )
 205 {
 206   // FIXME: Do more ...
 207   // Idea: have a get_globals() function which knows about that stuff.
 208   // Or most probably read that from a configuration file,
 209   // from property tree or whatever ...
 210
 211   // Get ground dependent data.
 212   GroundProperty gp;
 213   gp.wire_id = -1;
 214
 215   FGAICarrierHardware *ud =
 216     dynamic_cast<FGAICarrierHardware*>(l->getUserData());
 217   if (ud) {
 218     switch (ud->type) {
 219     case FGAICarrierHardware::Wire:
 220       gp.type = FGInterface::Wire;
 221       gp.wire_id = ud->id;
 222       break;
 223     case FGAICarrierHardware::Catapult:
 224       gp.type = FGInterface::Catapult;
 225       break;
 226     default:
 227       gp.type = FGInterface::Solid;
 228       break;
 229     }
 230
 231     // Copy the velocity from the carrier class.
 232     ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
 233   }
 234
 235   else {
 236
 237     // Initialize velocity field.
 238     sgdSetVec3( gp.vel, 0.0, 0.0, 0.0 );
 239     sgdSetVec3( gp.rot, 0.0, 0.0, 0.0 );
 240     sgdSetVec3( gp.pivot, 0.0, 0.0, 0.0 );
 241   }
 242
 243   // Get the texture name and decide what ground type we have.
 244   ssgState *st = l->getState();
 245   if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) {
 246     ssgSimpleState *ss = (ssgSimpleState*)st;
 247     SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" );
 248     string file = fullPath.file();
 249     SGPath dirPath(fullPath.dir());
 250     string category = dirPath.file();
 251
 252     if (category == "Runway")
 253       gp.type = FGInterface::Solid;
 254     else {
 255       if (file == "asphault.rgb" || file == "airport.rgb")
 256         gp.type = FGInterface::Solid;
 257       else if (file == "water.rgb" || file == "water-lake.rgb")
 258         gp.type = FGInterface::Water;
 259       else if (file == "forest.rgb" || file == "cropwood.rgb")
 260         gp.type = FGInterface::Forest;
 261     }
 262   }
 263
 264   return gp;
 265 }
 266
 267 void
 268 FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
 269                                     ssgLeaf *l)
 270 {
 271   GroundProperty gp = extractGroundProperty(l);
 272
 273   // Lines must have special meanings.
 274   // Wires and catapults are done with lines.
 275   int nl = l->getNumLines();
 276   for (int i = 0; i < nl; ++i) {
 277     sgdSphere sphere;
 278     sphere.empty();
 279     sgdVec3 ends[2];
 280     short v[2];
 281     l->getLine(i, v, v+1 );
 282     for (int k=0; k<2; ++k) {
 283       sgdSetVec3(ends[k], l->getVertex(v[k]));
 284       sgdXformPnt3(ends[k], xform);
 285       sphere.extend(ends[k]);
 286     }
 287
 288     if (wsp->intersects( &sphere )) {
 289       if (gp.type == FGInterface::Wire) {
 290         Wire wire;
 291         sgdCopyVec3(wire.ends[0], ends[0]);
 292         sgdCopyVec3(wire.ends[1], ends[1]);
 293         sgdCopyVec3(wire.velocity, gp.vel);
 294         sgdCopyVec3(wire.rotation, gp.rot);
 295         sgdSubVec3(wire.rotation_pivot, gp.pivot, cache_center);
 296         wire.wire_id = gp.wire_id;
 297
 298         wires.push_back(wire);
 299       }
 300       if (gp.type == FGInterface::Catapult) {
 301         Catapult cat;
 302         sgdCopyVec3(cat.start, ends[0]);
 303         sgdCopyVec3(cat.end, ends[1]);
 304         sgdCopyVec3(cat.velocity, gp.vel);
 305         sgdCopyVec3(cat.rotation, gp.rot);
 306         sgdSubVec3(cat.rotation_pivot, gp.pivot, cache_center);
 307
 308         catapults.push_back(cat);
 309       }
 310     }
 311   }
 312 }
 313
 314 void
 315 FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
 316                                        const sgdMat4 xform, bool sphIsec,
 317                                        sgdVec3 down, ssgLeaf *l)
 318 {
 319   GroundProperty gp = extractGroundProperty(l);
 320
 321   int nt = l->getNumTriangles();
 322   for (int i = 0; i < nt; ++i) {
 323     Triangle t;
 324     t.sphere.empty();
 325     short v[3];
 326     l->getTriangle(i, &v[0], &v[1], &v[2]);
 327     for (int k = 0; k < 3; ++k) {
 328       sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
 329       sgdXformPnt3(t.vertices[k], xform);
 330       t.sphere.extend(t.vertices[k]);
 331     }
 332
 333     sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
 334     SGDfloat dot = sgdScalarProductVec3(down, t.plane);
 335     if (dot > 0) {
 336       if (!l->getCullFace()) {
 337         // Surface points downwards, ignore for altitude computations.
 338         continue;
 339       } else
 340         sgdScaleVec4( t.plane, -1 );
 341     }
 342
 343     // Check if the sphere around the vehicle intersects the sphere
 344     // around that triangle. If so, put that triangle into the cache.
 345     if (sphIsec && sp->intersects(&t.sphere)) {
 346       sgdCopyVec3(t.velocity, gp.vel);
 347       sgdCopyVec3(t.rotation, gp.rot);
 348       sgdSubVec3(t.rotation_pivot, gp.pivot, cache_center);
 349       t.type = gp.type;
 350       triangles.push_back(t);
 351     }
 352
 353     // In case the cache is empty, we still provide agl computations.
 354     // But then we use the old way of having a fixed elevation value for
 355     // the whole lifetime of this cache.
 356     if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
 357       sgdVec3 tmp;
 358       sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
 359       sgdVec3 isectpoint;
 360       if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
 361            fgdPointInTriangle( isectpoint, t.vertices ) ) {
 362         // Compute the offset to the ground cache midpoint
 363         sgdVec3 off;
 364         sgdSubVec3(off, isectpoint, tmp);
 365         // Only accept the altitude if the intersection point is below the
 366         // ground cache midpoint
 367         if (0 < sgdScalarProductVec3( off, down )) {
 368           found_ground = true;
 369           sgdAddVec3(isectpoint, cache_center);
 370           double this_radius = sgdLengthVec3(isectpoint);
 371           if (ground_radius < this_radius)
 372             ground_radius = this_radius;
 373         }
 374       }
 375     }
 376   }
 377 }
 378
 379 inline void
 380 FGGroundCache::velocityTransformTriangle(double dt,
 381                                          FGGroundCache::Triangle& dst,
 382                                          const FGGroundCache::Triangle& src)
 383 {
 384   sgdCopyVec3(dst.vertices[0], src.vertices[0]);
 385   sgdCopyVec3(dst.vertices[1], src.vertices[1]);
 386   sgdCopyVec3(dst.vertices[2], src.vertices[2]);
 387
 388   sgdCopyVec4(dst.plane, src.plane);
 389
 390   sgdCopyVec3(dst.sphere.center, src.sphere.center);
 391   dst.sphere.radius = src.sphere.radius;
 392
 393   sgdCopyVec3(dst.velocity, src.velocity);
 394   sgdCopyVec3(dst.rotation, src.rotation);
 395   sgdCopyVec3(dst.rotation_pivot, src.rotation_pivot);
 396
 397   dst.type = src.type;
 398
 399   if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
 400     sgdVec3 pivotoff, vel;
 401     for (int i = 0; i < 3; ++i) {
 402       sgdSubVec3(pivotoff, src.vertices[i], src.rotation_pivot);
 403       sgdVectorProductVec3(vel, src.rotation, pivotoff);
 404       sgdAddVec3(vel, src.velocity);
 405       sgdAddScaledVec3(dst.vertices[i], vel, dt);
 406     }
 407
 408     // Transform the plane equation
 409     sgdSubVec3(pivotoff, dst.plane, src.rotation_pivot);
 410     sgdVectorProductVec3(vel, src.rotation, pivotoff);
 411     sgdAddVec3(vel, src.velocity);
 412     dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, vel);
 413
 414     sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
 415   }
 416 }
 417
 418 void
 419 FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
 420                           sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
 421 {
 422   // Travel through all kids.
 423   ssgEntity *e;
 424   for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
 425     if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
 426       continue;
 427     if ( e->getBSphere()->isEmpty() )
 428       continue;
 429
 430     // We need to check further if either the sphere around the branch
 431     // intersects the sphere around the aircraft or the line downwards from
 432     // the aircraft intersects the branchs sphere.
 433     sgdSphere esphere;
 434     sgdSetVec3(esphere.center, e->getBSphere()->center);
 435     esphere.radius = e->getBSphere()->radius;
 436     esphere.orthoXform(xform);
 437     bool wspIsec = wsp->intersects(&esphere);
 438     bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
 439     if (!wspIsec && !downIsec)
 440       continue;
 441
 442     // For branches collect up the transforms to reach that branch and
 443     // call cache_fill recursively.
 444     if ( e->isAKindOf( ssgTypeBranch() ) ) {
 445       ssgBranch *b = (ssgBranch *)e;
 446       if ( b->isAKindOf( ssgTypeTransform() ) ) {
 447         // Collect up the transforms required to reach that part of
 448         // the branch.
 449         sgMat4 xform2;
 450         sgMakeIdentMat4( xform2 );
 451         ssgTransform *t = (ssgTransform*)b;
 452         t->getTransform( xform2 );
 453         sgdMat4 xform3;
 454         fgMultMat4(xform3, xform, xform2);
 455         cache_fill( b, xform3, sp, down, wsp );
 456       } else
 457         cache_fill( b, xform, sp, down, wsp );
 458     }
 459
 460     // For leafs, check each triangle for intersection.
 461     // This will minimize the number of vertices/triangles in the cache.
 462     else if (e->isAKindOf(ssgTypeLeaf())) {
 463       // Since we reach that leaf if we have an intersection with the
 464       // most probably bigger wire/catapult cache sphere, we need to check
 465       // that here, if the smaller cache for the surface has a chance for hits.
 466       // Also, if the spheres do not intersect compute a coarse agl value
 467       // by following the line downwards originating at the aircraft.
 468       bool spIsec = sp->intersects(&esphere);
 469       putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
 470
 471       // If we are here, we need to put all special hardware here into
 472       // the cache.
 473       if (wspIsec)
 474         putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
 475     }
 476   }
 477 }
 478
 479 bool
 480 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
 481                                     double rad)
 482 {
 483   // Empty cache.
 484   ground_radius = 0.0;
 485   found_ground = false;
 486   triangles.resize(0);
 487   catapults.resize(0);
 488   wires.resize(0);
 489
 490   // Store the parameters we used to build up that cache.
 491   sgdCopyVec3(reference_wgs84_point, pt);
 492   reference_vehicle_radius = rad;
 493   // Store the time reference used to compute movements of moving triangles.
 494   cache_ref_time = ref_time;
 495
 496   // Decide where we put the scenery center.
 497   Point3D old_cntr = globals->get_scenery()->get_center();
 498   Point3D cntr(pt[0], pt[1], pt[2]);
 499   // Only move the cache center if it is unacceptable far away.
 500   if (40*40 < old_cntr.distance3Dsquared(cntr))
 501     globals->get_scenery()->set_center(cntr);
 502   else
 503     cntr = old_cntr;
 504
 505   // The center of the cache.
 506   sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
 507
 508   sgdVec3 ptoff;
 509   sgdSubVec3(ptoff, pt, cache_center);
 510   // Prepare sphere around the aircraft.
 511   sgdSphere acSphere;
 512   acSphere.setRadius(rad);
 513   acSphere.setCenter(ptoff);
 514
 515   // Prepare bigger sphere around the aircraft.
 516   // This one is required for reliably finding wires we have caught but
 517   // have already left the hopefully smaller sphere for the ground reactions.
 518   const double max_wire_dist = 300.0;
 519   sgdSphere wireSphere;
 520   wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
 521   wireSphere.setCenter(ptoff);
 522
 523   // Down vector. Is used for croase agl computations when we are far enough
 524   // from ground that we have an empty cache.
 525   sgdVec3 down;
 526   sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
 527   sgdNormalizeVec3(down);
 528
 529   // We collapse all transforms we need to reach a particular leaf.
 530   // The leafs itself will be then transformed later.
 531   // So our cache is just flat.
 532   // For leafs which are moving (carriers surface, etc ...)
 533   // we will later store a speed in the GroundType class. We can then apply
 534   // some translations to that nodes according to the time which has passed
 535   // compared to that snapshot.
 536   sgdMat4 xform;
 537   sgdMakeIdentMat4( xform );
 538
 539
 540   // Walk the scene graph and extract solid ground triangles and carrier data.
 541   ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
 542   cache_fill(terrain, xform, &acSphere, down, &wireSphere);
 543
 544   // some stats
 545   SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
 546          << ", # triangles = " << triangles.size()
 547          << ", # wires = " << wires.size()
 548          << ", # catapults = " << catapults.size()
 549          << ", ground_radius = " << ground_radius );
 550
 551   // If the ground radius is still below 5e6 meters, then we do not yet have
 552   // any scenery.
 553   found_ground = found_ground && 5e6 < ground_radius;
 554   if (!found_ground)
 555     SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
 556            "without any scenery below the aircraft" );
 557
 558   if (cntr != old_cntr)
 559     globals->get_scenery()->set_center(old_cntr);
 560
 561   return found_ground;
 562 }
 563
 564 bool
 565 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
 566 {
 567   sgdCopyVec3(pt, reference_wgs84_point);
 568   *rad = reference_vehicle_radius;
 569   *ref_time = cache_ref_time;
 570   return found_ground;
 571 }
 572
 573 double
 574 FGGroundCache::get_cat(double t, const double dpt[3],
 575                        double end[2][3], double vel[2][3])
 576 {
 577   // start with a distance of 1e10 meters...
 578   double dist = 1e10;
 579
 580   // Time difference to the reference time.
 581   t -= cache_ref_time;
 582
 583   size_t sz = catapults.size();
 584   for (size_t i = 0; i < sz; ++i) {
 585     sgdVec3 pivotoff, rvel[2];
 586     sgdLineSegment3 ls;
 587     sgdCopyVec3(ls.a, catapults[i].start);
 588     sgdCopyVec3(ls.b, catapults[i].end);
 589
 590     sgdSubVec3(pivotoff, ls.a, catapults[i].rotation_pivot);
 591     sgdVectorProductVec3(rvel[0], catapults[i].rotation, pivotoff);
 592     sgdAddVec3(rvel[0], catapults[i].velocity);
 593     sgdSubVec3(pivotoff, ls.b, catapults[i].rotation_pivot);
 594     sgdVectorProductVec3(rvel[1], catapults[i].rotation, pivotoff);
 595     sgdAddVec3(rvel[1], catapults[i].velocity);
 596
 597     sgdAddVec3(ls.a, cache_center);
 598     sgdAddVec3(ls.b, cache_center);
 599
 600     sgdAddScaledVec3(ls.a, rvel[0], t);
 601     sgdAddScaledVec3(ls.b, rvel[1], t);
 602
 603     double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
 604     if (this_dist < dist) {
 605       SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
 606              << this_dist << " meters away");
 607       dist = this_dist;
 608
 609       // The carrier code takes care of that ordering.
 610       sgdCopyVec3( end[0], ls.a );
 611       sgdCopyVec3( end[1], ls.b );
 612       sgdCopyVec3( vel[0], rvel[0] );
 613       sgdCopyVec3( vel[1], rvel[1] );
 614     }
 615   }
 616
 617   // At the end take the root, we only computed squared distances ...
 618   return sqrt(dist);
 619 }
 620
 621 bool
 622 FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff,
 623                        double contact[3], double normal[3], double vel[3],
 624                        int *type, double *loadCapacity,
 625                        double *frictionFactor, double *agl)
 626 {
 627   bool ret = false;
 628
 629   *type = FGInterface::Unknown;
 630 //   *agl = 0.0;
 631   *loadCapacity = DBL_MAX;
 632   *frictionFactor = 1.0;
 633   sgdSetVec3( vel, 0.0, 0.0, 0.0 );
 634   sgdSetVec3( contact, 0.0, 0.0, 0.0 );
 635   sgdSetVec3( normal, 0.0, 0.0, 0.0 );
 636
 637   // Time difference to th reference time.
 638   t -= cache_ref_time;
 639
 640   // The double valued point we start to search for intersection.
 641   sgdVec3 pt;
 642   sgdSubVec3( pt, dpt, cache_center );
 643
 644   // The search direction
 645   sgdVec3 dir;
 646   sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
 647   sgdNormaliseVec3( dir );
 648
 649   // Initialize to something sensible
 650   double current_radius = 0.0;
 651
 652   size_t sz = triangles.size();
 653   for (size_t i = 0; i < sz; ++i) {
 654     Triangle triangle;
 655     velocityTransformTriangle(t, triangle, triangles[i]);
 656     if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
 657       continue;
 658
 659     // Check for intersection.
 660     sgdVec3 isecpoint;
 661     if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
 662          sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
 663       // Compute the vector from pt to the intersection point ...
 664       sgdVec3 off;
 665       sgdSubVec3(off, isecpoint, pt);
 666       // ... and check if it is too high or not
 667       if (-max_altoff < sgdScalarProductVec3( off, dir )) {
 668         // Transform to the wgs system
 669         sgdAddVec3( isecpoint, cache_center );
 670         // compute the radius, good enough approximation to take the geocentric radius
 671         SGDfloat radius = sgdLengthSquaredVec3(isecpoint);
 672         if (current_radius < radius) {
 673           current_radius = radius;
 674           ret = true;
 675           // Save the new potential intersection point.
 676           sgdCopyVec3( contact, isecpoint );
 677           // The first three values in the vector are the plane normal.
 678           sgdCopyVec3( normal, triangle.plane );
 679           // The velocity wrt earth.
 680           sgdVec3 pivotoff;
 681           sgdSubVec3(pivotoff, pt, triangle.rotation_pivot);
 682           sgdVectorProductVec3(vel, triangle.rotation, pivotoff);
 683           sgdAddVec3(vel, triangle.velocity);
 684           // Save the ground type.
 685           *type = triangle.type;
 686           // FIXME: figure out how to get that sign ...
 687 //           *agl = sqrt(sqdist);
 688           *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
 689 //           *loadCapacity = DBL_MAX;
 690 //           *frictionFactor = 1.0;
 691         }
 692       }
 693     }
 694   }
 695
 696   if (ret)
 697     return true;
 698
 699   // Whenever we did not have a ground triangle for the requested point,
 700   // take the ground level we found during the current cache build.
 701   // This is as good as what we had before for agl.
 702   double r = sgdLengthVec3( dpt );
 703   sgdCopyVec3( contact, dpt );
 704   sgdScaleVec3( contact, ground_radius/r );
 705   sgdCopyVec3( normal, dpt );
 706   sgdNormaliseVec3( normal );
 707   sgdSetVec3( vel, 0.0, 0.0, 0.0 );
 708
 709   // The altitude is the distance of the requested point from the
 710   // contact point.
 711   *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
 712   *type = FGInterface::Unknown;
 713   *loadCapacity = DBL_MAX;
 714   *frictionFactor = 1.0;
 715
 716   return ret;
 717 }
 718
 719 bool FGGroundCache::caught_wire(double t, const double pt[4][3])
 720 {
 721   size_t sz = wires.size();
 722   if (sz == 0)
 723     return false;
 724
 725   // Time difference to the reference time.
 726   t -= cache_ref_time;
 727
 728   // Build the two triangles spanning the area where the hook has moved
 729   // during the past step.
 730   sgdVec4 plane[2];
 731   sgdVec3 tri[2][3];
 732   sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
 733   sgdCopyVec3( tri[0][0], pt[0] );
 734   sgdCopyVec3( tri[0][1], pt[1] );
 735   sgdCopyVec3( tri[0][2], pt[2] );
 736   sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
 737   sgdCopyVec3( tri[1][0], pt[0] );
 738   sgdCopyVec3( tri[1][1], pt[2] );
 739   sgdCopyVec3( tri[1][2], pt[3] );
 740
 741   // Intersect the wire lines with each of these triangles.
 742   // You have caught a wire if they intersect.
 743   for (size_t i = 0; i < sz; ++i) {
 744     sgdVec3 le[2];
 745     for (int k = 0; k < 2; ++k) {
 746       sgdVec3 pivotoff, vel;
 747       sgdCopyVec3(le[k], wires[i].ends[k]);
 748       sgdSubVec3(pivotoff, le[k], wires[i].rotation_pivot);
 749       sgdVectorProductVec3(vel, wires[i].rotation, pivotoff);
 750       sgdAddVec3(vel, wires[i].velocity);
 751       sgdAddScaledVec3(le[k], vel, t);
 752       sgdAddVec3(le[k], cache_center);
 753     }
 754
 755     for (int k=0; k<2; ++k) {
 756       sgdVec3 isecpoint;
 757       double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
 758       if ( 0.0 <= isecval && isecval <= 1.0 &&
 759            sgdPointInTriangle( isecpoint, tri[k] ) ) {
 760         SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
 761         // Store the wire id.
 762         wire_id = wires[i].wire_id;
 763         return true;
 764       }
 765     }
 766   }
 767
 768   return false;
 769 }
 770
 771 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
 772 {
 773   // Fast return if we do not have an active wire.
 774   if (wire_id < 0)
 775     return false;
 776
 777   // Time difference to the reference time.
 778   t -= cache_ref_time;
 779
 780   // Search for the wire with the matching wire id.
 781   size_t sz = wires.size();
 782   for (size_t i = 0; i < sz; ++i) {
 783     if (wires[i].wire_id == wire_id) {
 784       for (size_t k = 0; k < 2; ++k) {
 785         sgdVec3 pivotoff;
 786         sgdCopyVec3(end[k], wires[i].ends[k]);
 787         sgdSubVec3(pivotoff, end[k], wires[i].rotation_pivot);
 788         sgdVectorProductVec3(vel[k], wires[i].rotation, pivotoff);
 789         sgdAddVec3(vel[k], wires[i].velocity);
 790         sgdAddScaledVec3(end[k], vel[k], t);
 791         sgdAddVec3(end[k], cache_center);
 792       }
 793       return true;
 794     }
 795   }
 796
 797   return false;
 798 }
 799
 800 void FGGroundCache::release_wire(void)
 801 {
 802   wire_id = -1;
 803 }