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 fgdRayTriangle(SGVec3d& x, const SGVec3d& point, const SGVec3d& dir,
57 // Method based on the observation that we are looking for a
58 // point x that can be expressed in terms of the triangle points
59 // x = p_0 + \mu_1*(p_1 - p_0) + \mu_2*(p_2 - p_0)
60 // with 0 <= \mu_1, \mu_2 and \mu_1 + \mu_2 <= 1.
61 // OTOH it could be expressed in terms of the ray
62 // x = point + \lambda*dir
63 // Now we can compute \mu_i and \lambda.
65 SGVec3d d1 = v[1] - v[0];
66 SGVec3d d2 = v[2] - v[0];
67 SGVec3d b = point - v[0];
69 // the vector in normal direction, but not normalized
70 SGVec3d d1crossd2 = cross(d1, d2);
72 double denom = -dot(dir, d1crossd2);
73 double signDenom = copysign(1, denom);
74 // return if paralell ??? FIXME what if paralell and in plane?
75 // may be we are ok below than anyway??
76 // if (SGMiscd::abs(denom) <= SGLimitsd::min())
79 // Now \lambda would read
80 // lambda = 1/denom*dot(b, d1crossd2);
81 // To avoid an expensive division we multiply by |denom|
82 double lambdaDenom = signDenom*dot(b, d1crossd2);
85 // For line segment we would test against
88 // with the original lambda. The multiplied test would read
89 // if (absDenom < lambdaDenom)
92 double absDenom = fabs(denom);
93 double absDenomEps = absDenom*eps;
95 SGVec3d bcrossr = cross(b, dir);
96 // double mu1 = 1/denom*dot(d2, bcrossr);
97 double mu1 = signDenom*dot(d2, bcrossr);
98 if (mu1 < -absDenomEps)
100 // double mu2 = -1/denom*dot(d1, bcrossr);
103 double mmu2 = signDenom*dot(d1, bcrossr);
104 if (mmu2 > absDenomEps)
107 if (mu1 - mmu2 > absDenom + absDenomEps)
111 // if we have survived here it could only happen with denom == 0
112 // that the point is already in plane. Then return the origin ...
113 if (SGLimitsd::min() < absDenom)
114 x += (lambdaDenom/absDenom)*dir;
120 fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] )
124 // Some tolerance in meters we accept a point to be outside of the triangle
125 // and still return that it is inside.
127 // punt if outside bouding cube
128 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
129 if( (point[0] < min) || (point[0] > max) )
133 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
134 if( (point[1] < min) || (point[1] > max) )
138 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
139 if( (point[2] < min) || (point[2] > max) )
143 // drop the smallest dimension so we only have to work in 2d.
144 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
145 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
146 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
147 // x is the smallest dimension
156 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
157 // y is the smallest dimension
166 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
167 // z is the smallest dimension
177 // all dimensions are really small so lets call it close
178 // enough and return a successful match
182 // check if intersection point is on the same side of p1 <-> p2 as p3
183 SGDfloat tmp = (y2 - y3);
184 SGDfloat tmpn = (x2 - x3);
185 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
186 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn);
187 if ( side1 != side2 ) {
188 // printf("failed side 1 check\n");
192 // check if intersection point is on correct side of p2 <-> p3 as p1
195 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
196 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn);
197 if ( side1 != side2 ) {
198 // printf("failed side 2 check\n");
202 // check if intersection point is on correct side of p1 <-> p3 as p2
205 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
206 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn);
207 if ( side1 != side2 ) {
208 // printf("failed side 3 check\n");
215 // Test if the line given by the point on the line pt_on_line and the
216 // line direction dir intersects the sphere sp.
217 // Adapted from plib.
219 fgdIsectSphereInfLine(const SGVec3d& sphereCenter, double radius,
220 const SGVec3d& pt_on_line, const SGVec3d& dir)
222 SGVec3d r = sphereCenter - pt_on_line;
223 double projectedDistance = dot(r, dir);
224 double dist = dot(r, r) - projectedDistance * projectedDistance;
225 return dist < radius*radius;
229 class SGExtendedTriangleFunctor : public osg::TriangleFunctor<T> {
231 // Ok, to be complete we should also implement the indexed variants
232 // For now this one appears to be enough ...
233 void drawArrays(GLenum mode, GLint first, GLsizei count)
235 if (_vertexArrayPtr==0 || count==0) return;
237 const osg::Vec3* vlast;
238 const osg::Vec3* vptr;
241 vlast = &_vertexArrayPtr[first+count];
242 for(vptr=&_vertexArrayPtr[first];vptr<vlast;vptr+=2)
243 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
246 vlast = &_vertexArrayPtr[first+count-1];
247 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
248 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
251 vlast = &_vertexArrayPtr[first+count-1];
252 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
253 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
254 this->operator()(_vertexArrayPtr[first+count-1],
255 _vertexArrayPtr[first],_treatVertexDataAsTemporary);
258 osg::TriangleFunctor<T>::drawArrays(mode, first, count);
263 using osg::TriangleFunctor<T>::_vertexArrayPtr;
264 using osg::TriangleFunctor<T>::_treatVertexDataAsTemporary;
267 class GroundCacheFillVisitor : public osg::NodeVisitor {
270 /// class to just redirect triangles to the GroundCacheFillVisitor
271 class GroundCacheFill {
273 void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv)
274 { mGroundCacheFillVisitor = gcfv; }
276 void operator () (const osg::Vec3& v1, const osg::Vec3& v2,
277 const osg::Vec3& v3, bool)
278 { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); }
280 void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool)
281 { mGroundCacheFillVisitor->addLine(v1, v2); }
284 GroundCacheFillVisitor* mGroundCacheFillVisitor;
288 GroundCacheFillVisitor(FGGroundCache* groundCache,
290 const SGVec3d& cacheReference,
292 double wireCacheRadius) :
293 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
294 mGroundCache(groundCache)
296 setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
300 mBackfaceCulling = false;
301 mCacheReference = cacheReference;
302 mLocalCacheReference = cacheReference;
303 mCacheRadius = cacheRadius;
304 mWireCacheRadius = wireCacheRadius;
306 mTriangleFunctor.setGroundCacheFillVisitor(this);
308 mGroundProperty.wire_id = -1;
309 mGroundProperty.vel = SGVec3d(0, 0, 0);
310 mGroundProperty.rot = SGVec3d(0, 0, 0);
311 mGroundProperty.pivot = SGVec3d(0, 0, 0);
314 void updateCullMode(osg::StateSet* stateSet)
319 osg::StateAttribute* stateAttribute;
320 stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE);
323 osg::CullFace* cullFace = static_cast<osg::CullFace*>(stateAttribute);
324 mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK;
327 bool enterBoundingSphere(const osg::BoundingSphere& bs)
332 SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal);
333 double rc = bs.radius() + mCacheRadius;
334 // Ok, this node might intersect the cache. Visit it in depth.
335 double centerDist2 = distSqr(mCacheReference, cntr);
336 if (centerDist2 < rc*rc) {
339 // Check if the down direction touches the bounding sphere of the node
340 // if so, do at least croase agl computations.
341 // Ther other thing is that we must check if we are in range of
343 double rw = bs.radius() + mWireCacheRadius;
344 if (rw*rw < centerDist2 &&
345 !fgdIsectSphereInfLine(cntr, bs.radius(), mCacheReference, mDown))
353 bool enterNode(osg::Node& node)
355 if (!enterBoundingSphere(node.getBound()))
358 updateCullMode(node.getStateSet());
360 FGGroundCache::GroundProperty& gp = mGroundProperty;
361 // get some material information for use in the gear model
362 gp.material = globals->get_matlib()->findMaterial(&node);
364 gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
367 gp.type = FGInterface::Unknown;
368 osg::Referenced* base = node.getUserData();
371 FGAICarrierHardware *ud =
372 dynamic_cast<FGAICarrierHardware*>(base);
377 case FGAICarrierHardware::Wire:
378 gp.type = FGInterface::Wire;
381 case FGAICarrierHardware::Catapult:
382 gp.type = FGInterface::Catapult;
385 gp.type = FGInterface::Solid;
388 // Copy the velocity from the carrier class.
389 ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
394 void fillWith(osg::Drawable* drawable)
396 bool oldSphIsec = sphIsec;
397 if (!enterBoundingSphere(drawable->getBound()))
400 bool oldBackfaceCulling = mBackfaceCulling;
401 updateCullMode(drawable->getStateSet());
403 drawable->accept(mTriangleFunctor);
405 mBackfaceCulling = oldBackfaceCulling;
406 sphIsec = oldSphIsec;
409 virtual void apply(osg::Geode& geode)
411 bool oldBackfaceCulling = mBackfaceCulling;
412 bool oldSphIsec = sphIsec;
413 FGGroundCache::GroundProperty oldGp = mGroundProperty;
414 if (!enterNode(geode))
417 for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
418 fillWith(geode.getDrawable(i));
419 sphIsec = oldSphIsec;
420 mGroundProperty = oldGp;
421 mBackfaceCulling = oldBackfaceCulling;
424 virtual void apply(osg::Group& group)
426 bool oldBackfaceCulling = mBackfaceCulling;
427 bool oldSphIsec = sphIsec;
428 FGGroundCache::GroundProperty oldGp = mGroundProperty;
429 if (!enterNode(group))
432 sphIsec = oldSphIsec;
433 mBackfaceCulling = oldBackfaceCulling;
434 mGroundProperty = oldGp;
437 virtual void apply(osg::Transform& transform)
439 if (!enterNode(transform))
441 bool oldBackfaceCulling = mBackfaceCulling;
442 bool oldSphIsec = sphIsec;
443 FGGroundCache::GroundProperty oldGp = mGroundProperty;
444 /// transform the caches center to local coords
445 osg::Matrix oldLocalToGlobal = mLocalToGlobal;
446 osg::Matrix oldGlobalToLocal = mGlobalToLocal;
447 transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
448 transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
450 SGVec3d oldLocalCacheReference = mLocalCacheReference;
451 mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
452 SGVec3d oldLocalDown = mLocalDown;
453 mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
459 mLocalDown = oldLocalDown;
460 mLocalCacheReference = oldLocalCacheReference;
461 mLocalToGlobal = oldLocalToGlobal;
462 mGlobalToLocal = oldGlobalToLocal;
463 sphIsec = oldSphIsec;
464 mBackfaceCulling = oldBackfaceCulling;
465 mGroundProperty = oldGp;
468 void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
477 // a bounding sphere in the node local system
478 SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
480 double boundRadius = std::max(norm1(v[0] - boundCenter),
481 norm1(v[1] - boundCenter));
482 boundRadius = std::max(boundRadius, norm1(v[2] - boundCenter));
483 // Ok, we take the 1-norm instead of the expensive 2 norm.
484 // Therefore we need that scaling factor - roughly sqrt(3)
485 boundRadius = 1.733*boundRadius;
487 double boundRadius = std::max(distSqr(v[0], boundCenter),
488 distSqr(v[1], boundCenter));
489 boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
490 boundRadius = sqrt(boundRadius);
493 // if we are not in the downward cylinder bail out
494 if (!fgdIsectSphereInfLine(boundCenter, boundRadius + mCacheRadius,
495 mLocalCacheReference, mLocalDown))
499 // The normal and plane in the node local coordinate system
500 SGVec3d n = normalize(cross(v[1] - v[0], v[2] - v[0]));
501 if (0 < dot(mLocalDown, n)) {
502 if (mBackfaceCulling) {
503 // Surface points downwards, ignore for altitude computations.
507 std::swap(v[1], v[2]);
511 // Only check if the triangle is in the cache sphere if the plane
512 // containing the triangle is near enough
513 if (sphIsec && fabs(dot(n, v[0] - mLocalCacheReference)) < mCacheRadius) {
514 // Check if the sphere around the vehicle intersects the sphere
515 // around that triangle. If so, put that triangle into the cache.
516 double r2 = boundRadius + mCacheRadius;
517 if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
518 FGGroundCache::Triangle t;
519 for (unsigned i = 0; i < 3; ++i)
520 t.vertices[i].osg() = v[i].osg()*mLocalToGlobal;
521 t.boundCenter.osg() = boundCenter.osg()*mLocalToGlobal;
522 t.boundRadius = boundRadius;
525 tmp.osg() = osg::Matrixd::transform3x3(n.osg(), mLocalToGlobal);
526 t.plane = SGVec4d(tmp[0], tmp[1], tmp[2], -dot(tmp, t.vertices[0]));
527 t.velocity = mGroundProperty.vel;
528 t.rotation = mGroundProperty.rot;
529 t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
530 t.type = mGroundProperty.type;
531 t.material = mGroundProperty.material;
532 mGroundCache->triangles.push_back(t);
536 // In case the cache is empty, we still provide agl computations.
537 // But then we use the old way of having a fixed elevation value for
538 // the whole lifetime of this cache.
539 SGVec4d plane = SGVec4d(n[0], n[1], n[2], -dot(n, v[0]));
542 if (fgdRayTriangle(isectpoint, mLocalCacheReference, mLocalDown, v)) {
543 mGroundCache->found_ground = true;
544 isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
545 isectpoint += mGroundCache->cache_center;
546 double this_radius = length(isectpoint);
547 if (mGroundCache->ground_radius < this_radius) {
548 mGroundCache->ground_radius = this_radius;
549 mGroundCache->_type = mGroundProperty.type;
550 mGroundCache->_material = mGroundProperty.material;
555 void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
557 SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
558 SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
560 SGVec3d boundCenter = 0.5*(gv1 + gv2);
561 double boundRadius = length(gv1 - boundCenter);
563 if (distSqr(boundCenter, mCacheReference)
564 < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
565 if (mGroundProperty.type == FGInterface::Wire) {
566 FGGroundCache::Wire wire;
569 wire.velocity = mGroundProperty.vel;
570 wire.rotation = mGroundProperty.rot;
571 wire.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
572 wire.wire_id = mGroundProperty.wire_id;
574 mGroundCache->wires.push_back(wire);
576 if (mGroundProperty.type == FGInterface::Catapult) {
577 FGGroundCache::Catapult cat;
578 // Trick to get the ends in the right order.
579 // Use the x axis in the original coordinate system. Choose the
580 // most negative x-axis as the one pointing forward
588 cat.velocity = mGroundProperty.vel;
589 cat.rotation = mGroundProperty.rot;
590 cat.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
592 mGroundCache->catapults.push_back(cat);
597 SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
598 FGGroundCache* mGroundCache;
599 SGVec3d mCacheReference;
601 double mWireCacheRadius;
602 osg::Matrix mLocalToGlobal;
603 osg::Matrix mGlobalToLocal;
606 SGVec3d mLocalCacheReference;
608 bool mBackfaceCulling;
609 FGGroundCache::GroundProperty mGroundProperty;
612 FGGroundCache::FGGroundCache()
614 cache_center = SGVec3d(0, 0, 0);
616 cache_ref_time = 0.0;
618 reference_wgs84_point = SGVec3d(0, 0, 0);
619 reference_vehicle_radius = 0.0;
620 found_ground = false;
623 FGGroundCache::~FGGroundCache()
628 FGGroundCache::velocityTransformTriangle(double dt,
629 FGGroundCache::Triangle& dst,
630 const FGGroundCache::Triangle& src)
634 if (fabs(dt*dot(src.velocity, src.velocity)) < SGLimitsd::epsilon())
637 for (int i = 0; i < 3; ++i) {
638 SGVec3d pivotoff = src.vertices[i] - src.rotation_pivot;
639 dst.vertices[i] += dt*(src.velocity + cross(src.rotation, pivotoff));
642 // Transform the plane equation
643 SGVec3d pivotoff, vel;
644 sgdSubVec3(pivotoff.sg(), dst.plane.sg(), src.rotation_pivot.sg());
645 vel = src.velocity + cross(src.rotation, pivotoff);
646 dst.plane[3] += dt*sgdScalarProductVec3(dst.plane.sg(), vel.sg());
648 dst.boundCenter += dt*src.velocity;
652 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
657 found_ground = false;
662 // Store the parameters we used to build up that cache.
663 reference_wgs84_point = pt;
664 reference_vehicle_radius = rad;
665 // Store the time reference used to compute movements of moving triangles.
666 cache_ref_time = ref_time;
668 // Get a normalized down vector valid for the whole cache
669 SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
670 down = hlToEc.rotate(SGVec3d(0, 0, 1));
672 // Decide where we put the scenery center.
673 SGVec3d old_cntr = globals->get_scenery()->get_center();
675 // Only move the cache center if it is unacceptable far away.
676 if (40*40 < distSqr(old_cntr, cntr))
677 globals->get_scenery()->set_center(cntr);
681 // The center of the cache.
684 // Prepare sphere around the aircraft.
685 SGVec3d ptoff = pt - cache_center;
686 double cacheRadius = rad;
688 // Prepare bigger sphere around the aircraft.
689 // This one is required for reliably finding wires we have caught but
690 // have already left the hopefully smaller sphere for the ground reactions.
691 const double max_wire_dist = 300.0;
692 double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
694 // Walk the scene graph and extract solid ground triangles and carrier data.
695 GroundCacheFillVisitor gcfv(this, down, ptoff, cacheRadius, wireCacheRadius);
696 globals->get_scenery()->get_scene_graph()->accept(gcfv);
699 SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
700 << ", # triangles = " << triangles.size()
701 << ", # wires = " << wires.size()
702 << ", # catapults = " << catapults.size()
703 << ", ground_radius = " << ground_radius );
705 // If the ground radius is still below 5e6 meters, then we do not yet have
707 found_ground = found_ground && 5e6 < ground_radius;
709 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
710 "without any scenery below the aircraft" );
712 if (cntr != old_cntr)
713 globals->get_scenery()->set_center(old_cntr);
719 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
721 pt = reference_wgs84_point;
722 rad = reference_vehicle_radius;
723 ref_time = cache_ref_time;
728 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
729 SGVec3d end[2], SGVec3d vel[2])
731 // start with a distance of 1e10 meters...
734 // Time difference to the reference time.
737 size_t sz = catapults.size();
738 for (size_t i = 0; i < sz; ++i) {
739 SGVec3d pivotoff, rvel[2];
740 pivotoff = catapults[i].start - catapults[i].rotation_pivot;
741 rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
742 pivotoff = catapults[i].end - catapults[i].rotation_pivot;
743 rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
746 thisEnd[0] = cache_center + catapults[i].start + t*rvel[0];
747 thisEnd[1] = cache_center + catapults[i].end + t*rvel[1];
750 sgdCopyVec3(ls.a, thisEnd[0].sg());
751 sgdCopyVec3(ls.b, thisEnd[1].sg());
752 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt.sg() );
754 if (this_dist < dist) {
755 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
756 << this_dist << " meters away");
766 // At the end take the root, we only computed squared distances ...
771 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
772 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
773 int *type, const SGMaterial** material, double *agl)
777 *type = FGInterface::Unknown;
781 vel = SGVec3d(0, 0, 0);
782 contact = SGVec3d(0, 0, 0);
783 normal = SGVec3d(0, 0, 0);
785 // Time difference to th reference time.
788 // The double valued point we start to search for intersection.
789 SGVec3d pt = dpt - cache_center;
790 // shift the start of our ray by maxaltoff upwards
791 SGVec3d raystart = pt - max_altoff*down;
793 // Initialize to something sensible
794 double current_radius = 0.0;
796 size_t sz = triangles.size();
797 for (size_t i = 0; i < sz; ++i) {
799 velocityTransformTriangle(t, triangle, triangles[i]);
800 if (!fgdIsectSphereInfLine(triangle.boundCenter, triangle.boundRadius, pt, down))
803 // Check for intersection.
805 if (fgdRayTriangle(isecpoint, raystart, down, triangle.vertices)) {
806 // Compute the vector from pt to the intersection point ...
807 SGVec3d off = isecpoint - pt;
808 // ... and check if it is too high or not
809 // Transform to the wgs system
810 isecpoint += cache_center;
811 // compute the radius, good enough approximation to take the geocentric radius
812 double radius = dot(isecpoint, isecpoint);
813 if (current_radius < radius) {
814 current_radius = radius;
816 // Save the new potential intersection point.
818 // The first three values in the vector are the plane normal.
819 sgdCopyVec3( normal.sg(), triangle.plane.sg() );
820 // The velocity wrt earth.
821 SGVec3d pivotoff = pt - triangle.rotation_pivot;
822 vel = triangle.velocity + cross(triangle.rotation, pivotoff);
823 // Save the ground type.
824 *type = triangle.type;
825 *agl = dot(down, contact - dpt);
827 *material = triangle.material;
835 // Whenever we did not have a ground triangle for the requested point,
836 // take the ground level we found during the current cache build.
837 // This is as good as what we had before for agl.
838 double r = length(dpt);
840 contact *= ground_radius/r;
842 vel = SGVec3d(0, 0, 0);
844 // The altitude is the distance of the requested point from the
846 *agl = dot(down, contact - dpt);
849 *material = _material;
854 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
856 size_t sz = wires.size();
860 // Time difference to the reference time.
863 // Build the two triangles spanning the area where the hook has moved
864 // during the past step.
867 sgdMakePlane( plane[0].sg(), pt[0].sg(), pt[1].sg(), pt[2].sg() );
871 sgdMakePlane( plane[1].sg(), pt[0].sg(), pt[2].sg(), pt[3].sg() );
876 // Intersect the wire lines with each of these triangles.
877 // You have caught a wire if they intersect.
878 for (size_t i = 0; i < sz; ++i) {
880 for (int k = 0; k < 2; ++k) {
881 le[k] = wires[i].ends[k];
882 SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
883 SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
884 le[k] += t*vel + cache_center;
887 for (int k=0; k<2; ++k) {
889 double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(),
890 le[1].sg(), plane[k].sg());
891 if ( 0.0 <= isecval && isecval <= 1.0 &&
892 fgdPointInTriangle( isecpoint, tri[k] ) ) {
893 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
894 // Store the wire id.
895 wire_id = wires[i].wire_id;
904 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
906 // Fast return if we do not have an active wire.
910 // Time difference to the reference time.
913 // Search for the wire with the matching wire id.
914 size_t sz = wires.size();
915 for (size_t i = 0; i < sz; ++i) {
916 if (wires[i].wire_id == wire_id) {
917 for (size_t k = 0; k < 2; ++k) {
918 SGVec3d pivotoff = end[k] - wires[i].rotation_pivot;
919 vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
920 end[k] = cache_center + wires[i].ends[k] + t*vel[k];
929 void FGGroundCache::release_wire(void)