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 <utility>
  30
  31 #include <osg/CullFace>
  32 #include <osg/Drawable>
  33 #include <osg/Geode>
  34 #include <osg/Geometry>
  35 #include <osg/PrimitiveSet>
  36 #include <osg/TriangleFunctor>
  37
  38 #include <osgUtil/PolytopeIntersector>
  39
  40 #include <simgear/sg_inlines.h>
  41 #include <simgear/constants.h>
  42 #include <simgear/debug/logstream.hxx>
  43 #include <simgear/math/sg_geodesy.hxx>
  44 #include <simgear/scene/material/mat.hxx>
  45 #include <simgear/scene/material/matlib.hxx>
  46 #include <simgear/scene/util/PrimitiveUtils.hxx>
  47 #include <simgear/scene/util/SGNodeMasks.hxx>
  48
  49 #include <Main/globals.hxx>
  50 #include <Scenery/scenery.hxx>
  51 #include <Scenery/tilemgr.hxx>
  52 #include <AIModel/AICarrier.hxx>
  53
  54 #include "flight.hxx"
  55 #include "groundcache.hxx"
  56
  57 using namespace osg;
  58 using namespace osgUtil;
  59 using namespace simgear;
  60
  61 void makePolytopeShaft(Polytope& polyt, const Vec3d& refPoint,
  62                        const Vec3d& direction, double radius)
  63 {
  64     polyt.clear();
  65     // Choose best principal axis to start making orthogonal axis.
  66     Vec3d majorAxis;
  67     if (fabs(direction.x()) <= fabs(direction.y())) {
  68         if (fabs(direction.z()) <= fabs(direction.x()))
  69             majorAxis = Vec3d(0.0, 0.0, 1.0);
  70         else
  71             majorAxis = Vec3d(1.0, 0.0, 0.0);
  72     } else {
  73         if (fabs(direction.z()) <= fabs(direction.y()))
  74             majorAxis = Vec3d(0.0, 0.0, 1.0);
  75         else
  76             majorAxis = Vec3d(0.0, 1.0, 0.0);
  77     }
  78     Vec3d axis1 = majorAxis ^ direction;
  79     axis1.normalize();
  80     Vec3d axis2 = direction ^ axis1;
  81
  82     polyt.add(Plane(-axis1, refPoint + axis1 * radius));
  83     polyt.add(Plane(axis1, refPoint - axis1 * radius));
  84     polyt.add(Plane(-axis2, refPoint + axis2 * radius));
  85     polyt.add(Plane(axis2 , refPoint - axis2 * radius));
  86 }
  87
  88 void makePolytopeBox(Polytope& polyt, const Vec3d& center,
  89                      const Vec3d& direction, double radius)
  90 {
  91     makePolytopeShaft(polyt, center, direction, radius);
  92     polyt.add(Plane(-direction, center + direction * radius));
  93     polyt.add(Plane(direction, center - direction * radius));
  94 }
  95
  96 // Intersector for finding catapults and wires
  97
  98 class WireIntersector : public PolytopeIntersector
  99 {
 100 public:
 101     WireIntersector(const Polytope& polytope)
 102         : PolytopeIntersector(polytope), depth(0)
 103     {
 104         setDimensionMask(DimOne);
 105     }
 106
 107     bool enter(const osg::Node& node)
 108     {
 109         if (!PolytopeIntersector::enter(node))
 110             return false;
 111         const Referenced* base = node.getUserData();
 112         if (base) {
 113             const FGAICarrierHardware *ud
 114                 = dynamic_cast<const FGAICarrierHardware*>(base);
 115             if (ud)
 116                 carriers.push_back(depth);
 117         }
 118         depth++;
 119         return true;
 120     }
 121
 122     void leave()
 123     {
 124         depth--;
 125         if (!carriers.empty() && depth == carriers.back())
 126             carriers.pop_back();
 127     }
 128
 129     void intersect(IntersectionVisitor& iv, Drawable* drawable)
 130     {
 131         if (carriers.empty())
 132             return;
 133         PolytopeIntersector::intersect(iv, drawable);
 134     }
 135
 136     void reset()
 137     {
 138         carriers.clear();
 139     }
 140     std::vector<int> carriers;
 141     int depth;
 142 };
 143
 144 /// Ok, variant that uses a infinite line istead of the ray.
 145 /// also not that this only works if the ray direction is normalized.
 146 static inline bool
 147 intersectsInf(const SGRayd& ray, const SGSphered& sphere)
 148 {
 149   SGVec3d r = sphere.getCenter() - ray.getOrigin();
 150   double projectedDistance = dot(r, ray.getDirection());
 151   double dist = dot(r, r) - projectedDistance * projectedDistance;
 152   return dist < sphere.getRadius2();
 153 }
 154
 155 FGGroundCache::FGGroundCache() :
 156   ground_radius(0.0),
 157   _type(0),
 158   _material(0),
 159   cache_ref_time(0.0),
 160   wire_id(0),
 161   reference_wgs84_point(SGVec3d(0, 0, 0)),
 162   reference_vehicle_radius(0.0),
 163   down(0.0, 0.0, 0.0),
 164   found_ground(false)
 165 {
 166 }
 167
 168 FGGroundCache::~FGGroundCache()
 169 {
 170 }
 171
 172 inline void
 173 FGGroundCache::velocityTransformTriangle(double dt,
 174                                          SGTriangled& dst, SGSphered& sdst,
 175                                          const FGGroundCache::Triangle& src)
 176 {
 177   dst = src.triangle;
 178   sdst = src.sphere;
 179
 180   if (dt*dt*dot(src.gp.vel, src.gp.vel) < SGLimitsd::epsilon())
 181     return;
 182
 183   SGVec3d baseVert = dst.getBaseVertex();
 184   SGVec3d pivotoff = baseVert - src.gp.pivot;
 185   baseVert += dt*(src.gp.vel + cross(src.gp.rot, pivotoff));
 186   dst.setBaseVertex(baseVert);
 187   dst.setEdge(0, dst.getEdge(0) + dt*cross(src.gp.rot, dst.getEdge(0)));
 188   dst.setEdge(1, dst.getEdge(1) + dt*cross(src.gp.rot, dst.getEdge(1)));
 189 }
 190
 191 void FGGroundCache::getGroundProperty(Drawable* drawable,
 192                                       const NodePath& nodePath,
 193                                       FGGroundCache::GroundProperty& gp,
 194                                       bool& backfaceCulling)
 195 {
 196     gp.type = FGInterface::Unknown;
 197     gp.wire_id = 0;
 198     gp.vel = SGVec3d(0.0, 0.0, 0.0);
 199     gp.rot = SGVec3d(0.0, 0.0, 0.0);
 200     gp.pivot = SGVec3d(0.0, 0.0, 0.0);
 201     gp.material = 0;
 202     backfaceCulling = false;
 203     // XXX state set might be higher up in scene graph
 204     gp.material = SGMaterialLib::findMaterial(drawable->getStateSet());
 205     if (gp.material)
 206         gp.type = (gp.material->get_solid() ? FGInterface::Solid
 207                    : FGInterface::Water);
 208     for (NodePath::const_iterator iter = nodePath.begin(), e = nodePath.end();
 209          iter != e;
 210          ++iter) {
 211         Node* node = *iter;
 212         StateSet* stateSet = node->getStateSet();
 213         StateAttribute* stateAttribute = 0;
 214         if (stateSet && (stateAttribute
 215                          = stateSet->getAttribute(StateAttribute::CULLFACE))) {
 216             backfaceCulling
 217                 = (static_cast<osg::CullFace*>(stateAttribute)->getMode()
 218                    == CullFace::BACK);
 219         }
 220
 221         // get some material information for use in the gear model
 222         Referenced* base = node->getUserData();
 223         if (!base)
 224             continue;
 225         FGAICarrierHardware *ud = dynamic_cast<FGAICarrierHardware*>(base);
 226         if (!ud)
 227             continue;
 228         switch (ud->type) {
 229         case FGAICarrierHardware::Wire:
 230             gp.type = FGInterface::Wire;
 231             gp.wire_id = ud->id;
 232             break;
 233         case FGAICarrierHardware::Catapult:
 234             gp.type = FGInterface::Catapult;
 235             break;
 236         default:
 237             gp.type = FGInterface::Solid;
 238             break;
 239         }
 240         // Copy the velocity from the carrier class.
 241         ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
 242         break;
 243     }
 244 }
 245
 246 void FGGroundCache::getTriIntersectorResults(PolytopeIntersector* triInt)
 247 {
 248     const PolytopeIntersector::Intersections& intersections
 249         = triInt->getIntersections();
 250     Drawable* lastDrawable = 0;
 251     RefMatrix* lastMatrix = 0;
 252     Matrix worldToLocal;
 253     GroundProperty gp;
 254     bool backfaceCulling = false;
 255     for (PolytopeIntersector::Intersections::const_iterator
 256              itr = intersections.begin(), e = intersections.end();
 257          itr != e;
 258          ++itr) {
 259         const PolytopeIntersector::Intersection& intr = *itr;
 260         if (intr.drawable.get() != lastDrawable) {
 261             getGroundProperty(intr.drawable.get(), intr.nodePath, gp,
 262                               backfaceCulling);
 263             lastDrawable = intr.drawable.get();
 264         }
 265         Primitive triPrim = getPrimitive(intr.drawable, intr.primitiveIndex);
 266         if (triPrim.numVerts != 3)
 267             continue;
 268         SGVec3d v[3] = { SGVec3d(triPrim.vertices[0]),
 269                          SGVec3d(triPrim.vertices[1]),
 270                          SGVec3d(triPrim.vertices[2])
 271         };
 272         RefMatrix* mat = intr.matrix.get();
 273         // If the drawable is the same then the intersection model
 274         // matrix should be the same, because it is only set by nodes
 275         // in the scene graph. However, do an extra test in case
 276         // something funny is going on with the drawable.
 277         if (mat != lastMatrix) {
 278             lastMatrix = mat;
 279             worldToLocal = Matrix::inverse(*mat);
 280         }
 281         SGVec3d localCacheReference;
 282         localCacheReference.osg() = reference_wgs84_point.osg() * worldToLocal;
 283         SGVec3d localDown;
 284         localDown.osg() = Matrixd::transform3x3(down.osg(), worldToLocal);
 285         // a bounding sphere in the node local system
 286         SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
 287         double boundRadius = std::max(distSqr(v[0], boundCenter),
 288                                       distSqr(v[1], boundCenter));
 289         boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
 290         boundRadius = sqrt(boundRadius);
 291         SGRayd ray(localCacheReference, localDown);
 292         SGTriangled triangle(v);
 293         // The normal and plane in the node local coordinate system
 294         SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
 295         if (0 < dot(localDown, n)) {
 296             if (backfaceCulling) {
 297                 // Surface points downwards, ignore for altitude computations.
 298                 continue;
 299             } else {
 300                 triangle.flip();
 301             }
 302         }
 303
 304         // Only check if the triangle is in the cache sphere if the plane
 305         // containing the triangle is near enough
 306         double d = dot(n, v[0] - localCacheReference);
 307         if (d*d < reference_vehicle_radius*dot(n, n)) {
 308             // Check if the sphere around the vehicle intersects the sphere
 309             // around that triangle. If so, put that triangle into the cache.
 310             double r2 = boundRadius + reference_vehicle_radius;
 311             if (distSqr(boundCenter, localCacheReference) < r2*r2) {
 312                 FGGroundCache::Triangle t;
 313                 t.triangle.setBaseVertex(SGVec3d(v[0].osg() * *mat));
 314                 t.triangle.setEdge(0, SGVec3d(Matrixd::
 315                                               transform3x3(triangle
 316                                                            .getEdge(0).osg(),
 317                                                            *mat)));
 318                 t.triangle.setEdge(1, SGVec3d(Matrixd::
 319                                               transform3x3(triangle
 320                                                            .getEdge(1).osg(),
 321                                                            *mat)));
 322                 t.sphere.setCenter(SGVec3d(boundCenter.osg()* *mat));
 323                 t.sphere.setRadius(boundRadius);
 324                 t.gp = gp;
 325                 triangles.push_back(t);
 326             }
 327         }
 328         // In case the cache is empty, we still provide agl computations.
 329         // But then we use the old way of having a fixed elevation value for
 330         // the whole lifetime of this cache.
 331         SGVec3d isectpoint;
 332         if (intersects(isectpoint, triangle, ray, 1e-4)) {
 333             found_ground = true;
 334             isectpoint.osg() = isectpoint.osg() * *mat;
 335             double this_radius = length(isectpoint);
 336             if (ground_radius < this_radius) {
 337                 ground_radius = this_radius;
 338                 _type = gp.type;
 339                 _material = gp.material;
 340             }
 341         }
 342     }
 343 }
 344
 345 void FGGroundCache::getWireIntersectorResults(WireIntersector* wireInt,
 346                                               double wireCacheRadius)
 347 {
 348     const WireIntersector::Intersections& intersections
 349         = wireInt->getIntersections();
 350     Drawable* lastDrawable = 0;
 351     GroundProperty gp;
 352     bool backfaceCulling = false;
 353     for (PolytopeIntersector::Intersections::const_iterator
 354              itr = intersections.begin(), e = intersections.end();
 355          itr != e;
 356          ++itr) {
 357         if (itr->drawable.get() != lastDrawable) {
 358             getGroundProperty(itr->drawable.get(), itr->nodePath, gp,
 359                               backfaceCulling);
 360             lastDrawable = itr->drawable.get();
 361         }
 362         Primitive linePrim = getPrimitive(itr->drawable, itr->primitiveIndex);
 363         if (linePrim.numVerts != 2)
 364             continue;
 365         RefMatrix* mat = itr->matrix.get();
 366         SGVec3d gv1(osg::Vec3d(linePrim.vertices[0]) * *mat);
 367         SGVec3d gv2(osg::Vec3d(linePrim.vertices[1]) * *mat);
 368
 369         SGVec3d boundCenter = 0.5*(gv1 + gv2);
 370         double boundRadius = length(gv1 - boundCenter);
 371
 372         if (distSqr(boundCenter, reference_wgs84_point)
 373             < (boundRadius + wireCacheRadius)*(boundRadius + wireCacheRadius)) {
 374             if (gp.type == FGInterface::Wire) {
 375                 FGGroundCache::Wire wire;
 376                 wire.ends[0] = gv1;
 377                 wire.ends[1] = gv2;
 378                 wire.gp = gp;
 379                 wires.push_back(wire);
 380             } else if (gp.type == FGInterface::Catapult) {
 381                 FGGroundCache::Catapult cat;
 382                 // Trick to get the ends in the right order.
 383                 // Use the x axis in the original coordinate system. Choose the
 384                 // most negative x-axis as the one pointing forward
 385                 if (linePrim.vertices[0][0] > linePrim.vertices[1][0]) {
 386                     cat.start = gv1;
 387                     cat.end = gv2;
 388                 } else {
 389                     cat.start = gv2;
 390                     cat.end = gv1;
 391                 }
 392                 cat.gp = gp;
 393                 catapults.push_back(cat);
 394             }
 395         }
 396
 397     }
 398 }
 399
 400 bool
 401 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
 402                                     double rad)
 403 {
 404   // Empty cache.
 405   found_ground = false;
 406   SGGeod geodPt = SGGeod::fromCart(pt);
 407   // Don't blow away the cache ground_radius and stuff if there's no
 408   // scenery
 409   if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
 410                                                   geodPt.getLongitudeDeg(),
 411                                                   rad))
 412     return false;
 413   ground_radius = 0.0;
 414   triangles.resize(0);
 415   catapults.resize(0);
 416   wires.resize(0);
 417
 418   // Store the parameters we used to build up that cache.
 419   reference_wgs84_point = pt;
 420   reference_vehicle_radius = rad;
 421   // Store the time reference used to compute movements of moving triangles.
 422   cache_ref_time = ref_time;
 423
 424   // Get a normalized down vector valid for the whole cache
 425   SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
 426   down = hlToEc.rotate(SGVec3d(0, 0, 1));
 427
 428   // Prepare sphere around the aircraft.
 429   double cacheRadius = rad;
 430
 431   // Prepare bigger sphere around the aircraft.
 432   // This one is required for reliably finding wires we have caught but
 433   // have already left the hopefully smaller sphere for the ground reactions.
 434   const double max_wire_dist = 300.0;
 435   double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
 436
 437   Polytope triPolytope;
 438   makePolytopeShaft(triPolytope, pt.osg(), down.osg(), cacheRadius);
 439   ref_ptr<PolytopeIntersector> triIntersector
 440       = new PolytopeIntersector(triPolytope);
 441   triIntersector->setDimensionMask(PolytopeIntersector::DimTwo);
 442   Polytope wirePolytope;
 443   makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius);
 444   ref_ptr<WireIntersector> wireIntersector = new WireIntersector(wirePolytope);
 445   wireIntersector->setDimensionMask(PolytopeIntersector::DimOne);
 446   ref_ptr<IntersectorGroup> intersectors = new IntersectorGroup;
 447   intersectors->addIntersector(triIntersector.get());
 448   intersectors->addIntersector(wireIntersector.get());
 449
 450   // Walk the scene graph and extract solid ground triangles and
 451   // carrier data.
 452   IntersectionVisitor iv(intersectors);
 453   iv.setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
 454   globals->get_scenery()->get_scene_graph()->accept(iv);
 455   getTriIntersectorResults(triIntersector.get());
 456   getWireIntersectorResults(wireIntersector.get(), wireCacheRadius);
 457
 458   // some stats
 459   SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
 460          << ", # triangles = " << triangles.size()
 461          << ", # wires = " << wires.size()
 462          << ", # catapults = " << catapults.size()
 463          << ", ground_radius = " << ground_radius );
 464
 465   // If the ground radius is still below 5e6 meters, then we do not yet have
 466   // any scenery.
 467   found_ground = found_ground && 5e6 < ground_radius;
 468   if (!found_ground)
 469     SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
 470            "without any scenery below the aircraft" );
 471
 472   return found_ground;
 473 }
 474
 475 bool
 476 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
 477 {
 478   pt = reference_wgs84_point;
 479   rad = reference_vehicle_radius;
 480   ref_time = cache_ref_time;
 481   return found_ground;
 482 }
 483
 484 double
 485 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
 486                        SGVec3d end[2], SGVec3d vel[2])
 487 {
 488   // start with a distance of 1e10 meters...
 489   double dist = 1e10;
 490
 491   // Time difference to the reference time.
 492   t -= cache_ref_time;
 493
 494   size_t sz = catapults.size();
 495   for (size_t i = 0; i < sz; ++i) {
 496     SGVec3d pivotoff, rvel[2];
 497     pivotoff = catapults[i].start - catapults[i].gp.pivot;
 498     rvel[0] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
 499     pivotoff = catapults[i].end - catapults[i].gp.pivot;
 500     rvel[1] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
 501
 502     SGVec3d thisEnd[2];
 503     thisEnd[0] = catapults[i].start + t*rvel[0];
 504     thisEnd[1] = catapults[i].end + t*rvel[1];
 505
 506     double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
 507     if (this_dist < dist) {
 508       SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
 509              << this_dist << " meters away");
 510       dist = this_dist;
 511
 512       end[0] = thisEnd[0];
 513       end[1] = thisEnd[1];
 514       vel[0] = rvel[0];
 515       vel[1] = rvel[1];
 516     }
 517   }
 518
 519   // At the end take the root, we only computed squared distances ...
 520   return sqrt(dist);
 521 }
 522
 523 bool
 524 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
 525                        SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
 526                        int *type, const SGMaterial** material, double *agl)
 527 {
 528   bool ret = false;
 529
 530   *type = FGInterface::Unknown;
 531 //   *agl = 0.0;
 532   if (material)
 533     *material = 0;
 534   vel = SGVec3d(0, 0, 0);
 535   contact = SGVec3d(0, 0, 0);
 536   normal = SGVec3d(0, 0, 0);
 537
 538   // Time difference to th reference time.
 539   t -= cache_ref_time;
 540
 541   // The double valued point we start to search for intersection.
 542   SGVec3d pt = dpt;
 543   // shift the start of our ray by maxaltoff upwards
 544   SGRayd ray(pt - max_altoff*down, down);
 545
 546   // Initialize to something sensible
 547   double current_radius = 0.0;
 548
 549   size_t sz = triangles.size();
 550   for (size_t i = 0; i < sz; ++i) {
 551     SGSphered sphere;
 552     SGTriangled triangle;
 553     velocityTransformTriangle(t, triangle, sphere, triangles[i]);
 554     if (!intersectsInf(ray, sphere))
 555       continue;
 556
 557     // Check for intersection.
 558     SGVec3d isecpoint;
 559     if (intersects(isecpoint, triangle, ray, 1e-4)) {
 560       // Compute the vector from pt to the intersection point ...
 561       SGVec3d off = isecpoint - pt;
 562       // ... and check if it is too high or not
 563
 564       // compute the radius, good enough approximation to take the geocentric radius
 565       double radius = dot(isecpoint, isecpoint);
 566       if (current_radius < radius) {
 567         current_radius = radius;
 568         ret = true;
 569         // Save the new potential intersection point.
 570         contact = isecpoint;
 571         // The first three values in the vector are the plane normal.
 572         normal = triangle.getNormal();
 573         // The velocity wrt earth.
 574         SGVec3d pivotoff = pt - triangles[i].gp.pivot;
 575         vel = triangles[i].gp.vel + cross(triangles[i].gp.rot, pivotoff);
 576         // Save the ground type.
 577         *type = triangles[i].gp.type;
 578         *agl = dot(down, contact - dpt);
 579         if (material)
 580           *material = triangles[i].gp.material;
 581       }
 582     }
 583   }
 584
 585   if (ret)
 586     return true;
 587
 588   // Whenever we did not have a ground triangle for the requested point,
 589   // take the ground level we found during the current cache build.
 590   // This is as good as what we had before for agl.
 591   double r = length(dpt);
 592   contact = dpt;
 593   contact *= ground_radius/r;
 594   normal = -down;
 595   vel = SGVec3d(0, 0, 0);
 596
 597   // The altitude is the distance of the requested point from the
 598   // contact point.
 599   *agl = dot(down, contact - dpt);
 600   *type = _type;
 601   if (material)
 602     *material = _material;
 603
 604   return ret;
 605 }
 606
 607 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
 608 {
 609   size_t sz = wires.size();
 610   if (sz == 0)
 611     return false;
 612
 613   // Time difference to the reference time.
 614   t -= cache_ref_time;
 615
 616   // Build the two triangles spanning the area where the hook has moved
 617   // during the past step.
 618   SGTriangled triangle[2];
 619   triangle[0].set(pt[0], pt[1], pt[2]);
 620   triangle[1].set(pt[0], pt[2], pt[3]);
 621
 622   // Intersect the wire lines with each of these triangles.
 623   // You have caught a wire if they intersect.
 624   for (size_t i = 0; i < sz; ++i) {
 625     SGVec3d le[2];
 626     for (int k = 0; k < 2; ++k) {
 627       le[k] = wires[i].ends[k];
 628       SGVec3d pivotoff = le[k] - wires[i].gp.pivot;
 629       SGVec3d vel = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
 630       le[k] += t*vel;
 631     }
 632     SGLineSegmentd lineSegment(le[0], le[1]);
 633
 634     for (int k=0; k<2; ++k) {
 635       if (intersects(triangle[k], lineSegment)) {
 636         SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
 637         // Store the wire id.
 638         wire_id = wires[i].gp.wire_id;
 639         return true;
 640       }
 641     }
 642   }
 643
 644   return false;
 645 }
 646
 647 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
 648 {
 649   // Fast return if we do not have an active wire.
 650   if (wire_id < 0)
 651     return false;
 652
 653   // Time difference to the reference time.
 654   t -= cache_ref_time;
 655
 656   // Search for the wire with the matching wire id.
 657   size_t sz = wires.size();
 658   for (size_t i = 0; i < sz; ++i) {
 659     if (wires[i].gp.wire_id == wire_id) {
 660       for (size_t k = 0; k < 2; ++k) {
 661         SGVec3d pivotoff = wires[i].ends[k] - wires[i].gp.pivot;
 662         vel[k] = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
 663         end[k] = wires[i].ends[k] + t*vel[k];
 664       }
 665       return true;
 666     }
 667   }
 668
 669   return false;
 670 }
 671
 672 void FGGroundCache::release_wire(void)
 673 {
 674   wire_id = -1;
 675 }