Force 32 bits depth on .ico - only for Windows
[fg:hoorays-flightgear.git] / 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, 2009  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 "groundcache.hxx"
28
29 #include <utility>
30
31 #include <osg/Drawable>
32 #include <osg/Geode>
33 #include <osg/Geometry>
34 #include <osg/Camera>
35 #include <osg/Transform>
36 #include <osg/MatrixTransform>
37 #include <osg/PositionAttitudeTransform>
38 #include <osg/CameraView>
39
40 #include <simgear/sg_inlines.h>
41 #include <simgear/constants.h>
42 #include <simgear/debug/logstream.hxx>
43 #include <simgear/math/SGMisc.hxx>
44 #include <simgear/scene/material/mat.hxx>
45 #include <simgear/scene/util/SGNodeMasks.hxx>
46 #include <simgear/scene/util/SGSceneUserData.hxx>
47 #include <simgear/scene/util/OsgMath.hxx>
48
49 #include <simgear/bvh/BVHNode.hxx>
50 #include <simgear/bvh/BVHGroup.hxx>
51 #include <simgear/bvh/BVHTransform.hxx>
52 #include <simgear/bvh/BVHMotionTransform.hxx>
53 #include <simgear/bvh/BVHLineGeometry.hxx>
54 #include <simgear/bvh/BVHStaticGeometry.hxx>
55 #include <simgear/bvh/BVHStaticData.hxx>
56 #include <simgear/bvh/BVHStaticNode.hxx>
57 #include <simgear/bvh/BVHStaticTriangle.hxx>
58 #include <simgear/bvh/BVHStaticBinary.hxx>
59 #include <simgear/bvh/BVHSubTreeCollector.hxx>
60 #include <simgear/bvh/BVHLineSegmentVisitor.hxx>
61 #include <simgear/bvh/BVHNearestPointVisitor.hxx>
62
63 #ifdef GROUNDCACHE_DEBUG
64 #include <simgear/scene/model/BVHDebugCollectVisitor.hxx>
65 #include <Main/fg_props.hxx>
66 #endif
67
68 #include <Main/globals.hxx>
69 #include <Scenery/scenery.hxx>
70 #include <Scenery/tilemgr.hxx>
71
72 #include "flight.hxx"
73
74 using namespace simgear;
75
76 class FGGroundCache::CacheFill : public osg::NodeVisitor {
77 public:
78     CacheFill(const SGVec3d& center, const SGVec3d& down, const double& radius,
79               const double& startTime, const double& endTime) :
80         osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
81         _center(center),
82         _down(down),
83         _radius(radius),
84         _startTime(startTime),
85         _endTime(endTime),
86         _sceneryHit(0, 0, 0),
87         _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
88         _material(0),
89         _haveHit(false)
90     {
91         setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
92     }
93     virtual void apply(osg::Node& node)
94     {
95         if (!testBoundingSphere(node.getBound()))
96             return;
97
98         addBoundingVolume(node);
99     }
100     
101     virtual void apply(osg::Group& group)
102     {
103         if (!testBoundingSphere(group.getBound()))
104             return;
105
106         simgear::BVHSubTreeCollector::NodeList parentNodeList;
107         mSubTreeCollector.pushNodeList(parentNodeList);
108         
109         traverse(group);
110         addBoundingVolume(group);
111         
112         mSubTreeCollector.popNodeList(parentNodeList);
113     }
114     
115     virtual void apply(osg::Transform& transform)
116     { handleTransform(transform); }
117     virtual void apply(osg::Camera& camera)
118     {
119         if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
120             return;
121         handleTransform(camera);
122     }
123     virtual void apply(osg::CameraView& transform)
124     { handleTransform(transform); }
125     virtual void apply(osg::MatrixTransform& transform)
126     { handleTransform(transform); }
127     virtual void apply(osg::PositionAttitudeTransform& transform)
128     { handleTransform(transform); }
129         
130     void handleTransform(osg::Transform& transform)
131     {
132         // Hmm, may be this needs to be refined somehow ...
133         if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
134             return;
135
136         if (!testBoundingSphere(transform.getBound()))
137             return;
138
139         osg::Matrix inverseMatrix;
140         if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
141             return;
142         osg::Matrix matrix;
143         if (!transform.computeLocalToWorldMatrix(matrix, this))
144             return;
145
146         // Look for a velocity note
147         const SGSceneUserData::Velocity* velocity = getVelocity(transform);
148
149         SGVec3d center = _center;
150         SGVec3d down = _down;
151         double radius = _radius;
152         bool haveHit = _haveHit;
153         const simgear::BVHMaterial* material = _material;
154
155         _haveHit = false;
156         _center = toSG(inverseMatrix.preMult(toOsg(_center)));
157         _down = toSG(osg::Matrix::transform3x3(toOsg(_down), inverseMatrix));
158         if (velocity) {
159             SGVec3d staticCenter(_center);
160
161             double dtStart = velocity->referenceTime - _startTime;
162             SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
163             SGQuatd startOr(SGQuatd::fromAngleAxis(dtStart*velocity->angular));
164             startCenter = startOr.transform(startCenter);
165             
166             double dtEnd = velocity->referenceTime - _endTime;
167             SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
168             SGQuatd endOr(SGQuatd::fromAngleAxis(dtEnd*velocity->angular));
169             endCenter = endOr.transform(endCenter);
170
171             _center = 0.5*(startCenter + endCenter);
172             _down = startOr.transform(_down);
173             _radius += 0.5*dist(startCenter, endCenter);
174         }
175         
176         simgear::BVHSubTreeCollector::NodeList parentNodeList;
177         mSubTreeCollector.pushNodeList(parentNodeList);
178
179         addBoundingVolume(transform);
180         traverse(transform);
181
182         if (mSubTreeCollector.haveChildren()) {
183             if (velocity) {
184                 simgear::BVHMotionTransform* bvhTransform;
185                 bvhTransform = new simgear::BVHMotionTransform;
186                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
187                 bvhTransform->setLinearVelocity(velocity->linear);
188                 bvhTransform->setAngularVelocity(velocity->angular);
189                 bvhTransform->setReferenceTime(velocity->referenceTime);
190                 bvhTransform->setStartTime(_startTime);
191                 bvhTransform->setEndTime(_endTime);
192                 bvhTransform->setId(velocity->id);
193
194                 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
195             } else {
196                 simgear::BVHTransform* bvhTransform;
197                 bvhTransform = new simgear::BVHTransform;
198                 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
199
200                 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
201             }
202         } else {
203             mSubTreeCollector.popNodeList(parentNodeList);
204         }
205
206         if (_haveHit) {
207             if (velocity) {
208                 double dt = _startTime - velocity->referenceTime;
209                 SGQuatd ori(SGQuatd::fromAngleAxis(dt*velocity->angular));
210                 _sceneryHit = ori.transform(_sceneryHit);
211                 _sceneryHit += dt*velocity->linear;
212             }
213             _sceneryHit = toSG(matrix.preMult(toOsg(_sceneryHit)));
214         } else {
215             _material = material;
216             _haveHit = haveHit;
217         }
218
219         _center = center;
220         _down = down;
221         _radius = radius;
222     }
223
224     const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
225     {
226         SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
227         if (!userData)
228             return 0;
229         return userData->getVelocity();
230     }
231     simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
232     {
233         SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
234         if (!userData)
235             return 0;
236         return userData->getBVHNode();
237     }
238     void addBoundingVolume(osg::Node& node)
239     {
240         simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
241         if (!bvNode)
242             return;
243
244         // Find a croase ground intersection 
245         SGLineSegmentd line(_center + _radius*_down, _center + _maxDown*_down);
246         simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, _startTime);
247         bvNode->accept(lineSegmentVisitor);
248         if (!lineSegmentVisitor.empty()) {
249             _sceneryHit = lineSegmentVisitor.getPoint();
250             _material = lineSegmentVisitor.getMaterial();
251             _maxDown = SGMiscd::max(_radius, dot(_down, _sceneryHit - _center));
252             _haveHit = true;
253         }
254
255         // Get that part of the local bv tree that intersects our sphere
256         // of interrest.
257         mSubTreeCollector.setSphere(SGSphered(_center, _radius));
258         bvNode->accept(mSubTreeCollector);
259     }
260     
261     bool testBoundingSphere(const osg::BoundingSphere& bound) const
262     {
263         if (!bound.valid())
264             return false;
265
266         SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
267         double maxDist = bound._radius + _radius;
268         SGVec3d boundCenter(toVec3d(toSG(bound._center)));
269         return distSqr(downSeg, boundCenter) <= maxDist*maxDist;
270     }
271     
272     SGSharedPtr<simgear::BVHNode> getBVHNode() const
273     { return mSubTreeCollector.getNode(); }
274
275     bool getHaveElevationBelowCache() const
276     { return _haveHit; }
277     double getElevationBelowCache() const
278     { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
279     const simgear::BVHMaterial* getMaterialBelowCache() const
280     { return _material; }
281     
282 private:
283     SGVec3d _center;
284     SGVec3d _down;
285     double _radius;
286     double _startTime;
287     double _endTime;
288
289     simgear::BVHSubTreeCollector mSubTreeCollector;
290     SGVec3d _sceneryHit;
291     double _maxDown;
292     const simgear::BVHMaterial* _material;
293     bool _haveHit;
294 };
295
296 FGGroundCache::FGGroundCache() :
297     _altitude(0),
298     _material(0),
299     cache_ref_time(0),
300     cache_time_offset(0),
301     _wire(0),
302     reference_wgs84_point(SGVec3d(0, 0, 0)),
303     reference_vehicle_radius(0),
304     down(0.0, 0.0, 0.0),
305     found_ground(false)
306 {
307 #ifdef GROUNDCACHE_DEBUG
308     _lookupTime = SGTimeStamp::fromSec(0.0);
309     _lookupCount = 0;
310     _buildTime = SGTimeStamp::fromSec(0.0);
311     _buildCount = 0;
312 #endif
313 }
314
315 FGGroundCache::~FGGroundCache()
316 {
317 }
318
319 bool
320 FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
321                                     const SGVec3d& pt, double rad)
322 {
323 #ifdef GROUNDCACHE_DEBUG
324     SGTimeStamp t0 = SGTimeStamp::now();
325 #endif
326
327     // Empty cache.
328     found_ground = false;
329
330     SGGeod geodPt = SGGeod::fromCart(pt);
331     // Don't blow away the cache ground_radius and stuff if there's no
332     // scenery
333     if (!globals->get_tile_mgr()->schedule_scenery(geodPt, rad, 1.0)) {
334         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
335                "returns false at " << geodPt << " " << pt << " " << rad);
336         return false;
337     }
338     _material = 0;
339
340     // If we have an active wire, get some more area into the groundcache
341     if (_wire)
342         rad = SGMiscd::max(200, rad);
343     
344     // Store the parameters we used to build up that cache.
345     reference_wgs84_point = pt;
346     reference_vehicle_radius = rad;
347     // Store the time reference used to compute movements of moving triangles.
348     cache_ref_time = startSimTime;
349     
350     // Get a normalized down vector valid for the whole cache
351     SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
352     down = hlToEc.rotate(SGVec3d(0, 0, 1));
353     
354     // Get the ground cache, that is a local collision tree of the environment
355     startSimTime += cache_time_offset;
356     endSimTime += cache_time_offset;
357     CacheFill subtreeCollector(pt, down, rad, startSimTime, endSimTime);
358     globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
359     _localBvhTree = subtreeCollector.getBVHNode();
360
361     if (subtreeCollector.getHaveElevationBelowCache()) {
362         // Use the altitude value below the cache that we gathered during
363         // cache collection
364         _altitude = subtreeCollector.getElevationBelowCache();
365         _material = subtreeCollector.getMaterialBelowCache();
366         found_ground = true;
367     } else if (_localBvhTree) {
368         // We have nothing below us, so try starting with the lowest point
369         // upwards for a croase altitude value
370         SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down);
371         simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
372         _localBvhTree->accept(lineSegmentVisitor);
373
374         if (!lineSegmentVisitor.empty()) {
375             SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
376             _altitude = geodPt.getElevationM();
377             _material = lineSegmentVisitor.getMaterial();
378             found_ground = true;
379         }
380     }
381     
382     if (!found_ground) {
383         // Ok, still nothing here?? Last resort ...
384         double alt = 0;
385         _material = 0;
386         found_ground = globals->get_scenery()->
387             get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
388         if (found_ground)
389             _altitude = alt;
390     }
391     
392     // Still not sucessful??
393     if (!found_ground)
394         SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
395                "cache without any scenery below the aircraft");
396
397 #ifdef GROUNDCACHE_DEBUG
398     t0 = SGTimeStamp::now() - t0;
399     _buildTime += t0;
400     _buildCount++;
401
402     if (_buildCount > 60) {
403         double buildTime = 0;
404         if (_buildCount)
405             buildTime = _buildTime.toSecs()/_buildCount;
406         double lookupTime = 0;
407         if (_lookupCount)
408             lookupTime = _lookupTime.toSecs()/_lookupCount;
409         _buildTime = SGTimeStamp::fromSec(0.0);
410         _buildCount = 0;
411         _lookupTime = SGTimeStamp::fromSec(0.0);
412         _lookupCount = 0;
413         SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
414                << ", lookup Time = " << lookupTime);
415     }
416
417     if (!_group.valid()) {
418         _group = new osg::Group;
419         globals->get_scenery()->get_scene_graph()->addChild(_group);
420         fgSetInt("/fdm/groundcache-debug-level", -3);
421     }
422     _group->removeChildren(0, _group->getNumChildren());
423     if (_localBvhTree) {
424         int level = fgGetInt("/fdm/groundcache-debug-level");
425         if (-2 <= level) {
426             simgear::BVHDebugCollectVisitor debug(endSimTime, level);
427             _localBvhTree->accept(debug);
428             _group->addChild(debug.getNode());
429         }
430     }
431 #endif
432
433     return found_ground;
434 }
435
436 bool
437 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
438 {
439     pt = reference_wgs84_point;
440     rad = reference_vehicle_radius;
441     ref_time = cache_ref_time;
442     return found_ground;
443 }
444
445 class FGGroundCache::BodyFinder : public BVHVisitor {
446 public:
447     BodyFinder(BVHNode::Id id, const double& t) :
448         _id(id),
449         _bodyToWorld(SGMatrixd::unit()),
450         _linearVelocity(0, 0, 0),
451         _angularVelocity(0, 0, 0),
452         _time(t)
453     { }
454     
455     virtual void apply(BVHGroup& leaf)
456     {
457         if (_foundId)
458             return;
459         leaf.traverse(*this);
460     }
461     virtual void apply(BVHPageNode& leaf)
462     {
463         if (_foundId)
464             return;
465         leaf.traverse(*this);
466     }
467     virtual void apply(BVHTransform& transform)
468     {
469         if (_foundId)
470             return;
471
472         transform.traverse(*this);
473         
474         if (_foundId) {
475             _linearVelocity = transform.vecToWorld(_linearVelocity);
476             _angularVelocity = transform.vecToWorld(_angularVelocity);
477             _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
478         }
479     }
480     virtual void apply(BVHMotionTransform& transform)
481     {
482         if (_foundId)
483             return;
484
485         if (_id == transform.getId()) {
486             _foundId = true;
487         } else {
488             transform.traverse(*this);
489         }
490         
491         if (_foundId) {
492             SGMatrixd toWorld = transform.getToWorldTransform(_time);
493             SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
494             _linearVelocity += transform.getLinearVelocityAt(referencePoint);
495             _angularVelocity += transform.getAngularVelocity();
496             _linearVelocity = toWorld.xformVec(_linearVelocity);
497             _angularVelocity = toWorld.xformVec(_angularVelocity);
498             _bodyToWorld = toWorld*_bodyToWorld;
499         }
500     }
501     virtual void apply(BVHLineGeometry& node) { }
502     virtual void apply(BVHStaticGeometry& node) { }
503     
504     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
505     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
506     
507     const SGMatrixd& getBodyToWorld() const
508     { return _bodyToWorld; }
509     const SGVec3d& getLinearVelocity() const
510     { return _linearVelocity; }
511     const SGVec3d& getAngularVelocity() const
512     { return _angularVelocity; }
513     
514     bool empty() const
515     { return !_foundId; }
516     
517 protected:
518     simgear::BVHNode::Id _id;
519
520     SGMatrixd _bodyToWorld;
521
522     SGVec3d _linearVelocity;
523     SGVec3d _angularVelocity;
524     
525     bool _foundId;
526
527     double _time;
528 };
529
530 bool
531 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
532                         SGVec3d& angularVel, simgear::BVHNode::Id id)
533 {
534     // Get the transform matrix and velocities of a moving body with id at t.
535     if (!_localBvhTree)
536         return false;
537     t += cache_time_offset;
538     BodyFinder bodyFinder(id, t);
539     _localBvhTree->accept(bodyFinder);
540     if (bodyFinder.empty())
541         return false;
542
543     bodyToWorld = bodyFinder.getBodyToWorld();
544     linearVel = bodyFinder.getLinearVelocity();
545     angularVel = bodyFinder.getAngularVelocity();
546
547     return true;
548 }
549
550 class FGGroundCache::CatapultFinder : public BVHVisitor {
551 public:
552     CatapultFinder(const SGSphered& sphere, const double& t) :
553         _haveLineSegment(false),
554         _sphere(sphere),
555         _time(t)
556     { }
557     
558     virtual void apply(BVHGroup& leaf)
559     {
560         if (!intersects(_sphere, leaf.getBoundingSphere()))
561             return;
562         leaf.traverse(*this);
563     }
564     virtual void apply(BVHPageNode& leaf)
565     {
566         if (!intersects(_sphere, leaf.getBoundingSphere()))
567             return;
568         leaf.traverse(*this);
569     }
570     virtual void apply(BVHTransform& transform)
571     {
572         if (!intersects(_sphere, transform.getBoundingSphere()))
573             return;
574         
575         SGSphered sphere = _sphere;
576         _sphere = transform.sphereToLocal(sphere);
577         bool haveLineSegment = _haveLineSegment;
578         _haveLineSegment = false;
579         
580         transform.traverse(*this);
581         
582         if (_haveLineSegment) {
583             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
584             _linearVelocity = transform.vecToWorld(_linearVelocity);
585             _angularVelocity = transform.vecToWorld(_angularVelocity);
586         }
587         _haveLineSegment |= haveLineSegment;
588         _sphere.setCenter(sphere.getCenter());
589     }
590     virtual void apply(BVHMotionTransform& transform)
591     {
592         if (!intersects(_sphere, transform.getBoundingSphere()))
593             return;
594         
595         SGSphered sphere = _sphere;
596         _sphere = transform.sphereToLocal(sphere, _time);
597         bool haveLineSegment = _haveLineSegment;
598         _haveLineSegment = false;
599         
600         transform.traverse(*this);
601         
602         if (_haveLineSegment) {
603             SGMatrixd toWorld = transform.getToWorldTransform(_time);
604             _linearVelocity
605                 += transform.getLinearVelocityAt(_lineSegment.getStart());
606             _angularVelocity += transform.getAngularVelocity();
607             _linearVelocity = toWorld.xformVec(_linearVelocity);
608             _angularVelocity = toWorld.xformVec(_angularVelocity);
609             _lineSegment = _lineSegment.transform(toWorld);
610         }
611         _haveLineSegment |= haveLineSegment;
612         _sphere.setCenter(sphere.getCenter());
613     }
614     virtual void apply(BVHLineGeometry& node)
615     {
616         if (node.getType() != BVHLineGeometry::CarrierCatapult)
617             return;
618
619         SGLineSegmentd lineSegment(node.getLineSegment());
620         if (!intersects(_sphere, lineSegment))
621             return;
622
623         _lineSegment = lineSegment;
624         double dist = distSqr(lineSegment, getSphere().getCenter());
625         _sphere.setRadius(sqrt(dist));
626         _linearVelocity = SGVec3d::zeros();
627         _angularVelocity = SGVec3d::zeros();
628         _haveLineSegment = true;
629     }
630     virtual void apply(BVHStaticGeometry& node)
631     { }
632     
633     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
634     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
635     
636     void setSphere(const SGSphered& sphere)
637     { _sphere = sphere; }
638     const SGSphered& getSphere() const
639     { return _sphere; }
640     
641     const SGLineSegmentd& getLineSegment() const
642     { return _lineSegment; }
643     const SGVec3d& getLinearVelocity() const
644     { return _linearVelocity; }
645     const SGVec3d& getAngularVelocity() const
646     { return _angularVelocity; }
647     
648     bool getHaveLineSegment() const
649     { return _haveLineSegment; }
650     
651 protected:
652     SGLineSegmentd _lineSegment;
653     SGVec3d _linearVelocity;
654     SGVec3d _angularVelocity;
655     
656     bool _haveLineSegment;
657
658     SGSphered _sphere;
659     double _time;
660 };
661
662 double
663 FGGroundCache::get_cat(double t, const SGVec3d& pt,
664                        SGVec3d end[2], SGVec3d vel[2])
665 {
666     double maxDistance = 1000;
667
668     // Get the wire in question
669     t += cache_time_offset;
670     CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
671     if (_localBvhTree)
672         _localBvhTree->accept(catapultFinder);
673     
674     if (!catapultFinder.getHaveLineSegment())
675         return maxDistance;
676
677     // prepare the returns
678     end[0] = catapultFinder.getLineSegment().getStart();
679     end[1] = catapultFinder.getLineSegment().getEnd();
680
681     // The linear velocity is the one at the start of the line segment ...
682     vel[0] = catapultFinder.getLinearVelocity();
683     // ... so the end point has the additional cross product.
684     vel[1] = catapultFinder.getLinearVelocity();
685     vel[1] += cross(catapultFinder.getAngularVelocity(),
686                     catapultFinder.getLineSegment().getDirection());
687
688     // Return the distance to the cat
689     return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
690 }
691
692 bool
693 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
694                        SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
695                        simgear::BVHNode::Id& id, const simgear::BVHMaterial*& material)
696 {
697 #ifdef GROUNDCACHE_DEBUG
698     SGTimeStamp t0 = SGTimeStamp::now();
699 #endif
700
701     // Just set up a ground intersection query for the given point
702     SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
703     t += cache_time_offset;
704     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
705     if (_localBvhTree)
706         _localBvhTree->accept(lineSegmentVisitor);
707
708 #ifdef GROUNDCACHE_DEBUG
709     t0 = SGTimeStamp::now() - t0;
710     _lookupTime += t0;
711     _lookupCount++;
712 #endif
713
714     if (!lineSegmentVisitor.empty()) {
715         // Have an intersection
716         contact = lineSegmentVisitor.getPoint();
717         normal = lineSegmentVisitor.getNormal();
718         if (0 < dot(normal, down))
719             normal = -normal;
720         linearVel = lineSegmentVisitor.getLinearVelocity();
721         angularVel = lineSegmentVisitor.getAngularVelocity();
722         material = lineSegmentVisitor.getMaterial();
723         id = lineSegmentVisitor.getId();
724
725         return true;
726     } else {
727         // Whenever we did not have a ground triangle for the requested point,
728         // take the ground level we found during the current cache build.
729         // This is as good as what we had before for agl.
730         SGGeod geodPt = SGGeod::fromCart(pt);
731         geodPt.setElevationM(_altitude);
732         contact = SGVec3d::fromGeod(geodPt);
733         normal = -down;
734         linearVel = SGVec3d(0, 0, 0);
735         angularVel = SGVec3d(0, 0, 0);
736         material = _material;
737         id = 0;
738
739         return found_ground;
740     }
741 }
742
743
744 bool
745 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
746                            SGVec3d& contact, SGVec3d& linearVel,
747                            SGVec3d& angularVel, simgear::BVHNode::Id& id,
748                            const simgear::BVHMaterial*& material)
749 {
750     if (!_localBvhTree)
751         return false;
752
753 #ifdef GROUNDCACHE_DEBUG
754     SGTimeStamp t0 = SGTimeStamp::now();
755 #endif
756
757     // Just set up a ground intersection query for the given point
758     SGSphered sphere(pt, maxDist);
759     t += cache_time_offset;
760     simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
761     _localBvhTree->accept(nearestPointVisitor);
762
763 #ifdef GROUNDCACHE_DEBUG
764     t0 = SGTimeStamp::now() - t0;
765     _lookupTime += t0;
766     _lookupCount++;
767 #endif
768
769     if (nearestPointVisitor.empty())
770         return false;
771
772     // Have geometry in the range of maxDist
773     contact = nearestPointVisitor.getPoint();
774     linearVel = nearestPointVisitor.getLinearVelocity();
775     angularVel = nearestPointVisitor.getAngularVelocity();
776     material = nearestPointVisitor.getMaterial();
777     id = nearestPointVisitor.getId();
778     
779     return true;
780 }
781
782
783 class FGGroundCache::WireIntersector : public BVHVisitor {
784 public:
785     WireIntersector(const SGVec3d pt[4], const double& t) :
786         _linearVelocity(SGVec3d::zeros()),
787         _angularVelocity(SGVec3d::zeros()),
788         _wire(0),
789         _time(t)
790     {
791         // Build the two triangles spanning the area where the hook has moved
792         // during the past step.
793         _triangles[0].set(pt[0], pt[1], pt[2]);
794         _triangles[1].set(pt[0], pt[2], pt[3]);
795     }
796
797     virtual void apply(BVHGroup& leaf)
798     {
799         if (!_intersects(leaf.getBoundingSphere()))
800             return;
801
802         leaf.traverse(*this);
803     }
804     virtual void apply(BVHPageNode& leaf)
805     {
806         if (!_intersects(leaf.getBoundingSphere()))
807             return;
808
809         leaf.traverse(*this);
810     }
811     virtual void apply(BVHTransform& transform)
812     {
813         if (!_intersects(transform.getBoundingSphere()))
814             return;
815         
816         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
817         _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
818         _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
819         
820         transform.traverse(*this);
821         
822         if (_wire) {
823             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
824             _linearVelocity = transform.vecToWorld(_linearVelocity);
825             _angularVelocity = transform.vecToWorld(_angularVelocity);
826         }
827         _triangles[0] = triangles[0];
828         _triangles[1] = triangles[1];
829     }
830     virtual void apply(BVHMotionTransform& transform)
831     {
832         if (!_intersects(transform.getBoundingSphere()))
833             return;
834         
835         SGMatrixd toLocal = transform.getToLocalTransform(_time);
836
837         SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
838         _triangles[0] = triangles[0].transform(toLocal);
839         _triangles[1] = triangles[1].transform(toLocal);
840         
841         transform.traverse(*this);
842         
843         if (_wire) {
844             SGMatrixd toWorld = transform.getToWorldTransform(_time);
845             _linearVelocity
846                 += transform.getLinearVelocityAt(_lineSegment.getStart());
847             _angularVelocity += transform.getAngularVelocity();
848             _linearVelocity = toWorld.xformVec(_linearVelocity);
849             _angularVelocity = toWorld.xformVec(_angularVelocity);
850             _lineSegment = _lineSegment.transform(toWorld);
851         }
852         _triangles[0] = triangles[0];
853         _triangles[1] = triangles[1];
854     }
855     virtual void apply(BVHLineGeometry& node)
856     {
857         if (node.getType() != BVHLineGeometry::CarrierWire)
858             return;
859         SGLineSegmentd lineSegment(node.getLineSegment());
860         if (!_intersects(lineSegment))
861             return;
862
863         _lineSegment = lineSegment;
864         _linearVelocity = SGVec3d::zeros();
865         _angularVelocity = SGVec3d::zeros();
866         _wire = &node;
867     }
868     virtual void apply(BVHStaticGeometry& node)
869     { }
870     
871     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
872     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
873
874     bool _intersects(const SGSphered& sphere) const
875     {
876         if (_wire)
877             return false;
878         if (intersects(_triangles[0], sphere))
879             return true;
880         if (intersects(_triangles[1], sphere))
881             return true;
882         return false;
883     }
884     bool _intersects(const SGLineSegmentd& lineSegment) const
885     {
886         if (_wire)
887             return false;
888         if (intersects(_triangles[0], lineSegment))
889             return true;
890         if (intersects(_triangles[1], lineSegment))
891             return true;
892         return false;
893     }
894     
895     const SGLineSegmentd& getLineSegment() const
896     { return _lineSegment; }
897     const SGVec3d& getLinearVelocity() const
898     { return _linearVelocity; }
899     const SGVec3d& getAngularVelocity() const
900     { return _angularVelocity; }
901     
902     const BVHLineGeometry* getWire() const
903     { return _wire; }
904     
905 private:
906     SGLineSegmentd _lineSegment;
907     SGVec3d _linearVelocity;
908     SGVec3d _angularVelocity;
909     const BVHLineGeometry* _wire;
910
911     SGTriangled _triangles[2];
912     double _time;
913 };
914
915 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
916 {
917     // Get the wire in question
918     t += cache_time_offset;
919     WireIntersector wireIntersector(pt, t);
920     if (_localBvhTree)
921         _localBvhTree->accept(wireIntersector);
922     
923     _wire = wireIntersector.getWire();
924     return (_wire != NULL);
925 }
926
927 class FGGroundCache::WireFinder : public BVHVisitor {
928 public:
929     WireFinder(const BVHLineGeometry* wire, const double& t) :
930         _wire(wire),
931         _time(t),
932         _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
933         _linearVelocity(SGVec3d::zeros()),
934         _angularVelocity(SGVec3d::zeros()),
935         _haveLineSegment(false)
936     { }
937
938     virtual void apply(BVHGroup& leaf)
939     {
940         if (_haveLineSegment)
941             return;
942         leaf.traverse(*this);
943     }
944     virtual void apply(BVHPageNode& leaf)
945     {
946         if (_haveLineSegment)
947             return;
948         leaf.traverse(*this);
949     }
950     virtual void apply(BVHTransform& transform)
951     {
952         if (_haveLineSegment)
953             return;
954
955         transform.traverse(*this);
956         
957         if (_haveLineSegment) {
958             _linearVelocity = transform.vecToWorld(_linearVelocity);
959             _angularVelocity = transform.vecToWorld(_angularVelocity);
960             _lineSegment = transform.lineSegmentToWorld(_lineSegment);
961         }
962     }
963     virtual void apply(BVHMotionTransform& transform)
964     {
965         if (_haveLineSegment)
966             return;
967
968         transform.traverse(*this);
969         
970         if (_haveLineSegment) {
971             SGMatrixd toWorld = transform.getToWorldTransform(_time);
972             _linearVelocity
973                 += transform.getLinearVelocityAt(_lineSegment.getStart());
974             _angularVelocity += transform.getAngularVelocity();
975             _linearVelocity = toWorld.xformVec(_linearVelocity);
976             _angularVelocity = toWorld.xformVec(_angularVelocity);
977             _lineSegment = _lineSegment.transform(toWorld);
978         }
979     }
980     virtual void apply(BVHLineGeometry& node)
981     {
982         if (_haveLineSegment)
983             return;
984         if (_wire != &node)
985             return;
986         if (node.getType() != BVHLineGeometry::CarrierWire)
987             return;
988         _lineSegment = SGLineSegmentd(node.getLineSegment());
989         _linearVelocity = SGVec3d::zeros();
990         _angularVelocity = SGVec3d::zeros();
991         _haveLineSegment = true;
992     }
993     virtual void apply(BVHStaticGeometry&) { }
994     
995     virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
996     virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
997
998     const SGLineSegmentd& getLineSegment() const
999     { return _lineSegment; }
1000     
1001     bool getHaveLineSegment() const
1002     { return _haveLineSegment; }
1003
1004     const SGVec3d& getLinearVelocity() const
1005     { return _linearVelocity; }
1006     const SGVec3d& getAngularVelocity() const
1007     { return _angularVelocity; }
1008
1009 private:
1010     const BVHLineGeometry* _wire;
1011     double _time;
1012
1013     SGLineSegmentd _lineSegment;
1014     SGVec3d _linearVelocity;
1015     SGVec3d _angularVelocity;
1016
1017     bool _haveLineSegment;
1018 };
1019
1020 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
1021 {
1022     // Fast return if we do not have an active wire.
1023     if (!_wire)
1024         return false;
1025
1026     // Get the wire in question
1027     t += cache_time_offset;
1028     WireFinder wireFinder(_wire, t);
1029     if (_localBvhTree)
1030         _localBvhTree->accept(wireFinder);
1031     
1032     if (!wireFinder.getHaveLineSegment())
1033         return false;
1034
1035     // prepare the returns
1036     end[0] = wireFinder.getLineSegment().getStart();
1037     end[1] = wireFinder.getLineSegment().getEnd();
1038
1039     // The linear velocity is the one at the start of the line segment ...
1040     vel[0] = wireFinder.getLinearVelocity();
1041     // ... so the end point has the additional cross product.
1042     vel[1] = wireFinder.getLinearVelocity();
1043     vel[1] += cross(wireFinder.getAngularVelocity(),
1044                     wireFinder.getLineSegment().getDirection());
1045
1046     return true;
1047 }
1048
1049 void FGGroundCache::release_wire(void)
1050 {
1051     _wire = 0;
1052 }