1 // groundcache.cxx -- carries a small subset of the scenegraph near the vehicle
3 // Written by Mathias Froehlich, started Nov 2004.
5 // Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de
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.
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.
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.
30 #include <osg/CullFace>
31 #include <osg/Drawable>
33 #include <osg/Geometry>
34 #include <osg/TriangleFunctor>
36 #include <simgear/sg_inlines.h>
37 #include <simgear/constants.h>
38 #include <simgear/debug/logstream.hxx>
39 #include <simgear/math/sg_geodesy.hxx>
40 #include <simgear/scene/material/mat.hxx>
41 #include <simgear/scene/material/matlib.hxx>
42 #include <simgear/scene/util/SGNodeMasks.hxx>
44 #include <Main/globals.hxx>
45 #include <Scenery/scenery.hxx>
46 #include <Scenery/tilemgr.hxx>
47 #include <AIModel/AICarrier.hxx>
50 #include "groundcache.hxx"
53 fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] )
57 // Some tolerance in meters we accept a point to be outside of the triangle
58 // and still return that it is inside.
61 // punt if outside bouding cube
62 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
63 if( (point[0] < min - eps) || (point[0] > max + eps) )
67 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
68 if( (point[1] < min - eps) || (point[1] > max + eps) )
72 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
73 if( (point[2] < min - eps) || (point[2] > max + eps) )
77 // drop the smallest dimension so we only have to work in 2d.
78 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
79 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
80 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
81 // x is the smallest dimension
90 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
91 // y is the smallest dimension
100 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
101 // z is the smallest dimension
111 // all dimensions are really small so lets call it close
112 // enough and return a successful match
116 // check if intersection point is on the same side of p1 <-> p2 as p3
117 SGDfloat tmp = (y2 - y3);
118 SGDfloat tmpn = (x2 - x3);
119 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
120 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
121 + side1 * eps * fabs(tmpn));
122 if ( side1 != side2 ) {
123 // printf("failed side 1 check\n");
127 // check if intersection point is on correct side of p2 <-> p3 as p1
130 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
131 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
132 + side1 * eps * fabs(tmpn));
133 if ( side1 != side2 ) {
134 // printf("failed side 2 check\n");
138 // check if intersection point is on correct side of p1 <-> p3 as p2
141 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
142 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
143 + side1 * eps * fabs(tmpn));
144 if ( side1 != side2 ) {
145 // printf("failed side 3 check\n");
152 // Test if the line given by the point on the line pt_on_line and the
153 // line direction dir intersects the sphere sp.
154 // Adapted from plib.
156 fgdIsectSphereInfLine(const SGVec3d& sphereCenter, double radius,
157 const SGVec3d& pt_on_line, const SGVec3d& dir)
159 SGVec3d r = sphereCenter - pt_on_line;
160 double projectedDistance = dot(r, dir);
161 double dist = dot(r, r) - projectedDistance * projectedDistance;
162 return dist < radius*radius;
166 class SGExtendedTriangleFunctor : public osg::TriangleFunctor<T> {
168 // Ok, to be complete we should also implement the indexed variants
169 // For now this one appears to be enough ...
170 void drawArrays(GLenum mode, GLint first, GLsizei count)
172 if (_vertexArrayPtr==0 || count==0) return;
174 const osg::Vec3* vlast;
175 const osg::Vec3* vptr;
178 vlast = &_vertexArrayPtr[first+count];
179 for(vptr=&_vertexArrayPtr[first];vptr<vlast;vptr+=2)
180 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
183 vlast = &_vertexArrayPtr[first+count-1];
184 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
185 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
188 vlast = &_vertexArrayPtr[first+count-1];
189 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
190 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
191 this->operator()(_vertexArrayPtr[first+count-1],
192 _vertexArrayPtr[first],_treatVertexDataAsTemporary);
195 osg::TriangleFunctor<T>::drawArrays(mode, first, count);
200 using osg::TriangleFunctor<T>::_vertexArrayPtr;
201 using osg::TriangleFunctor<T>::_treatVertexDataAsTemporary;
204 class GroundCacheFillVisitor : public osg::NodeVisitor {
207 /// class to just redirect triangles to the GroundCacheFillVisitor
208 class GroundCacheFill {
210 void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv)
211 { mGroundCacheFillVisitor = gcfv; }
213 void operator () (const osg::Vec3& v1, const osg::Vec3& v2,
214 const osg::Vec3& v3, bool)
215 { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); }
217 void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool)
218 { mGroundCacheFillVisitor->addLine(v1, v2); }
221 GroundCacheFillVisitor* mGroundCacheFillVisitor;
225 GroundCacheFillVisitor(FGGroundCache* groundCache,
227 const SGVec3d& cacheReference,
229 double wireCacheRadius) :
230 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
231 mGroundCache(groundCache)
233 setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
236 mBackfaceCulling = false;
237 mCacheReference = cacheReference;
238 mCacheRadius = cacheRadius;
239 mWireCacheRadius = wireCacheRadius;
241 mTriangleFunctor.setGroundCacheFillVisitor(this);
243 mGroundProperty.wire_id = -1;
244 mGroundProperty.vel = SGVec3d(0, 0, 0);
245 mGroundProperty.rot = SGVec3d(0, 0, 0);
246 mGroundProperty.pivot = SGVec3d(0, 0, 0);
249 void updateCullMode(osg::StateSet* stateSet)
254 osg::StateAttribute* stateAttribute;
255 stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE);
258 osg::CullFace* cullFace = static_cast<osg::CullFace*>(stateAttribute);
259 mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK;
262 bool enterBoundingSphere(const osg::BoundingSphere& bs)
267 SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal);
268 double rc = bs.radius() + mCacheRadius;
269 // Ok, this node might intersect the cache. Visit it in depth.
270 double centerDist2 = distSqr(mCacheReference, cntr);
271 if (centerDist2 < rc*rc) {
274 // Check if the down direction touches the bounding sphere of the node
275 // if so, do at least croase agl computations.
276 // Ther other thing is that we must check if we are in range of
278 double rw = bs.radius() + mWireCacheRadius;
279 if (rw*rw < centerDist2 &&
280 !fgdIsectSphereInfLine(cntr, bs.radius(), mCacheReference, mDown))
288 bool enterNode(osg::Node& node)
290 if (!enterBoundingSphere(node.getBound()))
293 updateCullMode(node.getStateSet());
295 FGGroundCache::GroundProperty& gp = mGroundProperty;
296 // get some material information for use in the gear model
297 gp.material = globals->get_matlib()->findMaterial(&node);
299 gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
302 osg::Referenced* base = node.getUserData();
305 FGAICarrierHardware *ud =
306 dynamic_cast<FGAICarrierHardware*>(base);
311 case FGAICarrierHardware::Wire:
312 gp.type = FGInterface::Wire;
315 case FGAICarrierHardware::Catapult:
316 gp.type = FGInterface::Catapult;
319 gp.type = FGInterface::Solid;
322 // Copy the velocity from the carrier class.
323 ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
328 void fillWith(osg::Drawable* drawable)
330 bool oldSphIsec = sphIsec;
331 if (!enterBoundingSphere(drawable->getBound()))
334 bool oldBackfaceCulling = mBackfaceCulling;
335 updateCullMode(drawable->getStateSet());
337 drawable->accept(mTriangleFunctor);
339 mBackfaceCulling = oldBackfaceCulling;
340 sphIsec = oldSphIsec;
343 virtual void apply(osg::Geode& geode)
345 bool oldBackfaceCulling = mBackfaceCulling;
346 bool oldSphIsec = sphIsec;
347 FGGroundCache::GroundProperty oldGp = mGroundProperty;
348 if (!enterNode(geode))
351 for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
352 fillWith(geode.getDrawable(i));
353 sphIsec = oldSphIsec;
354 mGroundProperty = oldGp;
355 mBackfaceCulling = oldBackfaceCulling;
358 virtual void apply(osg::Group& group)
360 bool oldBackfaceCulling = mBackfaceCulling;
361 bool oldSphIsec = sphIsec;
362 FGGroundCache::GroundProperty oldGp = mGroundProperty;
363 if (!enterNode(group))
366 sphIsec = oldSphIsec;
367 mBackfaceCulling = oldBackfaceCulling;
368 mGroundProperty = oldGp;
371 virtual void apply(osg::Transform& transform)
373 if (!enterNode(transform))
375 bool oldBackfaceCulling = mBackfaceCulling;
376 bool oldSphIsec = sphIsec;
377 FGGroundCache::GroundProperty oldGp = mGroundProperty;
378 /// transform the caches center to local coords
379 osg::Matrix oldLocalToGlobal = mLocalToGlobal;
380 transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
386 mLocalToGlobal = oldLocalToGlobal;
387 sphIsec = oldSphIsec;
388 mBackfaceCulling = oldBackfaceCulling;
389 mGroundProperty = oldGp;
392 void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
395 FGGroundCache::Triangle t;
396 osg::Vec3d gv1 = osg::Vec3d(v1)*mLocalToGlobal;
397 osg::Vec3d gv2 = osg::Vec3d(v2)*mLocalToGlobal;
398 osg::Vec3d gv3 = osg::Vec3d(v3)*mLocalToGlobal;
399 for (unsigned i = 0; i < 3; ++i) {
400 t.vertices[0][i] = gv1[i];
401 t.vertices[1][i] = gv2[i];
402 t.vertices[2][i] = gv3[i];
404 // FIXME: can do better ...
405 t.boundCenter = (1.0/3)*(t.vertices[0] + t.vertices[1] + t.vertices[2]);
406 t.boundRadius = std::max(length(t.vertices[0] - t.boundCenter),
407 length(t.vertices[1] - t.boundCenter));
408 t.boundRadius = std::max(t.boundRadius,
409 length(t.vertices[2] - t.boundCenter));
411 sgdMakePlane(t.plane.sg(), t.vertices[0].sg(), t.vertices[1].sg(),
413 double d = sgdScalarProductVec3(mDown.sg(), t.plane.sg());
415 if (mBackfaceCulling) {
416 // Surface points downwards, ignore for altitude computations.
422 // Check if the sphere around the vehicle intersects the sphere
423 // around that triangle. If so, put that triangle into the cache.
425 distSqr(t.boundCenter, mCacheReference)
426 < (t.boundRadius + mCacheRadius)*(t.boundRadius + mCacheRadius) ) {
427 t.velocity = mGroundProperty.vel;
428 t.rotation = mGroundProperty.rot;
429 t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
430 t.type = mGroundProperty.type;
431 mGroundCache->triangles.push_back(t);
434 // In case the cache is empty, we still provide agl computations.
435 // But then we use the old way of having a fixed elevation value for
436 // the whole lifetime of this cache.
437 if ( fgdIsectSphereInfLine(t.boundCenter, t.boundRadius,
438 mCacheReference, mDown) ) {
440 if ( sgdIsectInfLinePlane( isectpoint.sg(), mCacheReference.sg(),
441 mDown.sg(), t.plane.sg() ) &&
442 fgdPointInTriangle( isectpoint, t.vertices ) ) {
443 // Only accept the altitude if the intersection point is below the
444 // ground cache midpoint
445 if (0 < dot(isectpoint - mCacheReference, mDown)) {
446 mGroundCache->found_ground = true;
447 isectpoint += mGroundCache->cache_center;
448 double this_radius = length(isectpoint);
449 if (mGroundCache->ground_radius < this_radius)
450 mGroundCache->ground_radius = this_radius;
456 void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
458 SGVec3d gv1 = SGVec3d(osg::Vec3d(v1)*mLocalToGlobal);
459 SGVec3d gv2 = SGVec3d(osg::Vec3d(v2)*mLocalToGlobal);
461 SGVec3d boundCenter = 0.5*(gv1 + gv2);
462 double boundRadius = length(gv1 - boundCenter);
464 if (distSqr(boundCenter, mCacheReference)
465 < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
466 if (mGroundProperty.type == FGInterface::Wire) {
467 FGGroundCache::Wire wire;
470 wire.velocity = mGroundProperty.vel;
471 wire.rotation = mGroundProperty.rot;
472 wire.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
473 wire.wire_id = mGroundProperty.wire_id;
475 mGroundCache->wires.push_back(wire);
477 if (mGroundProperty.type == FGInterface::Catapult) {
478 FGGroundCache::Catapult cat;
479 // Trick to get the ends in the right order.
480 // Use the x axis in the original coordinate system. Choose the
481 // most negative x-axis as the one pointing forward
489 cat.velocity = mGroundProperty.vel;
490 cat.rotation = mGroundProperty.rot;
491 cat.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
493 mGroundCache->catapults.push_back(cat);
498 SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
499 FGGroundCache* mGroundCache;
500 SGVec3d mCacheReference;
502 double mWireCacheRadius;
503 osg::Matrix mLocalToGlobal;
506 bool mBackfaceCulling;
507 FGGroundCache::GroundProperty mGroundProperty;
510 FGGroundCache::FGGroundCache()
512 cache_center = SGVec3d(0, 0, 0);
514 cache_ref_time = 0.0;
516 reference_wgs84_point = SGVec3d(0, 0, 0);
517 reference_vehicle_radius = 0.0;
518 found_ground = false;
521 FGGroundCache::~FGGroundCache()
526 FGGroundCache::velocityTransformTriangle(double dt,
527 FGGroundCache::Triangle& dst,
528 const FGGroundCache::Triangle& src)
532 if (fabs(dt*dot(src.velocity, src.velocity)) < SGLimitsd::epsilon())
535 for (int i = 0; i < 3; ++i) {
536 SGVec3d pivotoff = src.vertices[i] - src.rotation_pivot;
537 dst.vertices[i] += dt*(src.velocity + cross(src.rotation, pivotoff));
540 // Transform the plane equation
541 SGVec3d pivotoff, vel;
542 sgdSubVec3(pivotoff.sg(), dst.plane.sg(), src.rotation_pivot.sg());
543 vel = src.velocity + cross(src.rotation, pivotoff);
544 dst.plane[3] += dt*sgdScalarProductVec3(dst.plane.sg(), vel.sg());
546 dst.boundCenter += dt*src.velocity;
550 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
555 found_ground = false;
560 // Store the parameters we used to build up that cache.
561 reference_wgs84_point = pt;
562 reference_vehicle_radius = rad;
563 // Store the time reference used to compute movements of moving triangles.
564 cache_ref_time = ref_time;
566 // Get a normalized down vector valid for the whole cache
567 SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
568 down = hlToEc.rotate(SGVec3d(0, 0, 1));
570 // Decide where we put the scenery center.
571 SGVec3d old_cntr = globals->get_scenery()->get_center();
573 // Only move the cache center if it is unacceptable far away.
574 if (40*40 < distSqr(old_cntr, cntr))
575 globals->get_scenery()->set_center(cntr);
579 // The center of the cache.
582 // Prepare sphere around the aircraft.
583 SGVec3d ptoff = pt - cache_center;
584 double cacheRadius = rad;
586 // Prepare bigger sphere around the aircraft.
587 // This one is required for reliably finding wires we have caught but
588 // have already left the hopefully smaller sphere for the ground reactions.
589 const double max_wire_dist = 300.0;
590 double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
592 // Walk the scene graph and extract solid ground triangles and carrier data.
593 GroundCacheFillVisitor gcfv(this, down, ptoff, cacheRadius, wireCacheRadius);
594 globals->get_scenery()->get_scene_graph()->accept(gcfv);
597 SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
598 << ", # triangles = " << triangles.size()
599 << ", # wires = " << wires.size()
600 << ", # catapults = " << catapults.size()
601 << ", ground_radius = " << ground_radius );
603 // If the ground radius is still below 5e6 meters, then we do not yet have
605 found_ground = found_ground && 5e6 < ground_radius;
607 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
608 "without any scenery below the aircraft" );
610 if (cntr != old_cntr)
611 globals->get_scenery()->set_center(old_cntr);
617 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
619 pt = reference_wgs84_point;
620 rad = reference_vehicle_radius;
621 ref_time = cache_ref_time;
626 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
627 SGVec3d end[2], SGVec3d vel[2])
629 // start with a distance of 1e10 meters...
632 // Time difference to the reference time.
635 size_t sz = catapults.size();
636 for (size_t i = 0; i < sz; ++i) {
637 SGVec3d pivotoff, rvel[2];
638 pivotoff = catapults[i].start - catapults[i].rotation_pivot;
639 rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
640 pivotoff = catapults[i].end - catapults[i].rotation_pivot;
641 rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
644 thisEnd[0] = cache_center + catapults[i].start + t*rvel[0];
645 thisEnd[1] = cache_center + catapults[i].end + t*rvel[1];
648 sgdCopyVec3(ls.a, thisEnd[0].sg());
649 sgdCopyVec3(ls.b, thisEnd[1].sg());
650 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt.sg() );
652 if (this_dist < dist) {
653 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
654 << this_dist << " meters away");
664 // At the end take the root, we only computed squared distances ...
669 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
670 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
671 int *type, const SGMaterial** material, double *agl)
675 *type = FGInterface::Unknown;
679 vel = SGVec3d(0, 0, 0);
680 contact = SGVec3d(0, 0, 0);
681 normal = SGVec3d(0, 0, 0);
683 // Time difference to th reference time.
686 // The double valued point we start to search for intersection.
687 SGVec3d pt = dpt - cache_center;
689 // Initialize to something sensible
690 double current_radius = 0.0;
692 size_t sz = triangles.size();
693 for (size_t i = 0; i < sz; ++i) {
695 velocityTransformTriangle(t, triangle, triangles[i]);
696 if (!fgdIsectSphereInfLine(triangle.boundCenter, triangle.boundRadius, pt, down))
699 // Check for intersection.
701 if ( sgdIsectInfLinePlane( isecpoint.sg(), pt.sg(), down.sg(), triangle.plane.sg() ) &&
702 fgdPointInTriangle( isecpoint, triangle.vertices ) ) {
703 // Compute the vector from pt to the intersection point ...
704 SGVec3d off = isecpoint - pt;
705 // ... and check if it is too high or not
706 if (-max_altoff < dot(off, down)) {
707 // Transform to the wgs system
708 isecpoint += cache_center;
709 // compute the radius, good enough approximation to take the geocentric radius
710 double radius = dot(isecpoint, isecpoint);
711 if (current_radius < radius) {
712 current_radius = radius;
714 // Save the new potential intersection point.
716 // The first three values in the vector are the plane normal.
717 sgdCopyVec3( normal.sg(), triangle.plane.sg() );
718 // The velocity wrt earth.
719 SGVec3d pivotoff = pt - triangle.rotation_pivot;
720 vel = triangle.velocity + cross(triangle.rotation, pivotoff);
721 // Save the ground type.
722 *type = triangle.type;
723 *agl = dot(down, contact - dpt);
725 *material = triangle.material;
734 // Whenever we did not have a ground triangle for the requested point,
735 // take the ground level we found during the current cache build.
736 // This is as good as what we had before for agl.
737 double r = length(dpt);
739 contact *= ground_radius/r;
741 vel = SGVec3d(0, 0, 0);
743 // The altitude is the distance of the requested point from the
745 *agl = dot(down, contact - dpt);
746 *type = FGInterface::Unknown;
751 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
753 size_t sz = wires.size();
757 // Time difference to the reference time.
760 // Build the two triangles spanning the area where the hook has moved
761 // during the past step.
764 sgdMakePlane( plane[0].sg(), pt[0].sg(), pt[1].sg(), pt[2].sg() );
768 sgdMakePlane( plane[1].sg(), pt[0].sg(), pt[2].sg(), pt[3].sg() );
773 // Intersect the wire lines with each of these triangles.
774 // You have caught a wire if they intersect.
775 for (size_t i = 0; i < sz; ++i) {
777 for (int k = 0; k < 2; ++k) {
778 le[k] = wires[i].ends[k];
779 SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
780 SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
781 le[k] += t*vel + cache_center;
784 for (int k=0; k<2; ++k) {
786 double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(),
787 le[1].sg(), plane[k].sg());
788 if ( 0.0 <= isecval && isecval <= 1.0 &&
789 fgdPointInTriangle( isecpoint, tri[k] ) ) {
790 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
791 // Store the wire id.
792 wire_id = wires[i].wire_id;
801 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
803 // Fast return if we do not have an active wire.
807 // Time difference to the reference time.
810 // Search for the wire with the matching wire id.
811 size_t sz = wires.size();
812 for (size_t i = 0; i < sz; ++i) {
813 if (wires[i].wire_id == wire_id) {
814 for (size_t k = 0; k < 2; ++k) {
815 SGVec3d pivotoff = end[k] - wires[i].rotation_pivot;
816 vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
817 end[k] = cache_center + wires[i].ends[k] + t*vel[k];
826 void FGGroundCache::release_wire(void)