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