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