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, 2009 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.
27 #include "groundcache.hxx"
31 #include <osg/Drawable>
33 #include <osg/Geometry>
35 #include <osg/Transform>
36 #include <osg/MatrixTransform>
37 #include <osg/PositionAttitudeTransform>
38 #include <osg/CameraView>
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/util/SGNodeMasks.hxx>
45 #include <simgear/scene/util/SGSceneUserData.hxx>
46 #include <simgear/scene/util/OsgMath.hxx>
48 #include <simgear/bvh/BVHNode.hxx>
49 #include <simgear/bvh/BVHGroup.hxx>
50 #include <simgear/bvh/BVHTransform.hxx>
51 #include <simgear/bvh/BVHMotionTransform.hxx>
52 #include <simgear/bvh/BVHLineGeometry.hxx>
53 #include <simgear/bvh/BVHStaticGeometry.hxx>
54 #include <simgear/bvh/BVHStaticData.hxx>
55 #include <simgear/bvh/BVHStaticNode.hxx>
56 #include <simgear/bvh/BVHStaticTriangle.hxx>
57 #include <simgear/bvh/BVHStaticBinary.hxx>
58 #include <simgear/bvh/BVHSubTreeCollector.hxx>
59 #include <simgear/bvh/BVHLineSegmentVisitor.hxx>
60 #include <simgear/bvh/BVHNearestPointVisitor.hxx>
62 #ifdef GROUNDCACHE_DEBUG
63 #include <simgear/scene/model/BVHDebugCollectVisitor.hxx>
64 #include <Main/fg_props.hxx>
67 #include <Main/globals.hxx>
68 #include <Scenery/scenery.hxx>
69 #include <Scenery/tilemgr.hxx>
73 using namespace simgear;
75 class FGGroundCache::CacheFill : public osg::NodeVisitor {
77 CacheFill(const SGVec3d& center, const SGVec3d& down, const double& radius,
78 const double& startTime, const double& endTime) :
79 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
83 _startTime(startTime),
86 _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
90 setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
92 virtual void apply(osg::Node& node)
94 if (!testBoundingSphere(node.getBound()))
97 addBoundingVolume(node);
100 virtual void apply(osg::Group& group)
102 if (!testBoundingSphere(group.getBound()))
105 simgear::BVHSubTreeCollector::NodeList parentNodeList;
106 mSubTreeCollector.pushNodeList(parentNodeList);
109 addBoundingVolume(group);
111 mSubTreeCollector.popNodeList(parentNodeList);
114 virtual void apply(osg::Transform& transform)
115 { handleTransform(transform); }
116 virtual void apply(osg::Camera& camera)
118 if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
120 handleTransform(camera);
122 virtual void apply(osg::CameraView& transform)
123 { handleTransform(transform); }
124 virtual void apply(osg::MatrixTransform& transform)
125 { handleTransform(transform); }
126 virtual void apply(osg::PositionAttitudeTransform& transform)
127 { handleTransform(transform); }
129 void handleTransform(osg::Transform& transform)
131 // Hmm, may be this needs to be refined somehow ...
132 if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
135 if (!testBoundingSphere(transform.getBound()))
138 osg::Matrix inverseMatrix;
139 if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
142 if (!transform.computeLocalToWorldMatrix(matrix, this))
145 // Look for a velocity note
146 const SGSceneUserData::Velocity* velocity = getVelocity(transform);
148 SGVec3d center = _center;
149 SGVec3d down = _down;
150 double radius = _radius;
151 bool haveHit = _haveHit;
152 const simgear::BVHMaterial* material = _material;
155 _center = toSG(inverseMatrix.preMult(toOsg(_center)));
156 _down = toSG(osg::Matrix::transform3x3(toOsg(_down), inverseMatrix));
158 SGVec3d staticCenter(_center);
160 double dtStart = velocity->referenceTime - _startTime;
161 SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
162 SGQuatd startOr(SGQuatd::fromAngleAxis(dtStart*velocity->angular));
163 startCenter = startOr.transform(startCenter);
165 double dtEnd = velocity->referenceTime - _endTime;
166 SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
167 SGQuatd endOr(SGQuatd::fromAngleAxis(dtEnd*velocity->angular));
168 endCenter = endOr.transform(endCenter);
170 _center = 0.5*(startCenter + endCenter);
171 _down = startOr.transform(_down);
172 _radius += 0.5*dist(startCenter, endCenter);
175 simgear::BVHSubTreeCollector::NodeList parentNodeList;
176 mSubTreeCollector.pushNodeList(parentNodeList);
178 addBoundingVolume(transform);
181 if (mSubTreeCollector.haveChildren()) {
183 simgear::BVHMotionTransform* bvhTransform;
184 bvhTransform = new simgear::BVHMotionTransform;
185 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
186 bvhTransform->setLinearVelocity(velocity->linear);
187 bvhTransform->setAngularVelocity(velocity->angular);
188 bvhTransform->setReferenceTime(velocity->referenceTime);
189 bvhTransform->setStartTime(_startTime);
190 bvhTransform->setEndTime(_endTime);
191 bvhTransform->setId(velocity->id);
193 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
195 simgear::BVHTransform* bvhTransform;
196 bvhTransform = new simgear::BVHTransform;
197 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
199 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
202 mSubTreeCollector.popNodeList(parentNodeList);
207 double dt = _startTime - velocity->referenceTime;
208 SGQuatd ori(SGQuatd::fromAngleAxis(dt*velocity->angular));
209 _sceneryHit = ori.transform(_sceneryHit);
210 _sceneryHit += dt*velocity->linear;
212 _sceneryHit = toSG(matrix.preMult(toOsg(_sceneryHit)));
214 _material = material;
223 const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
225 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
228 return userData->getVelocity();
230 simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
232 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
235 return userData->getBVHNode();
237 void addBoundingVolume(osg::Node& node)
239 simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
243 // Find a croase ground intersection
244 SGLineSegmentd line(_center + _radius*_down, _center + _maxDown*_down);
245 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, _startTime);
246 bvNode->accept(lineSegmentVisitor);
247 if (!lineSegmentVisitor.empty()) {
248 _sceneryHit = lineSegmentVisitor.getPoint();
249 _material = lineSegmentVisitor.getMaterial();
250 _maxDown = SGMiscd::max(_radius, dot(_down, _sceneryHit - _center));
254 // Get that part of the local bv tree that intersects our sphere
256 mSubTreeCollector.setSphere(SGSphered(_center, _radius));
257 bvNode->accept(mSubTreeCollector);
260 bool testBoundingSphere(const osg::BoundingSphere& bound) const
265 SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
266 double maxDist = bound._radius + _radius;
267 SGVec3d boundCenter(toVec3d(toSG(bound._center)));
268 return distSqr(downSeg, boundCenter) <= maxDist*maxDist;
271 SGSharedPtr<simgear::BVHNode> getBVHNode() const
272 { return mSubTreeCollector.getNode(); }
274 bool getHaveElevationBelowCache() const
276 double getElevationBelowCache() const
277 { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
278 const simgear::BVHMaterial* getMaterialBelowCache() const
279 { return _material; }
288 simgear::BVHSubTreeCollector mSubTreeCollector;
291 const simgear::BVHMaterial* _material;
295 FGGroundCache::FGGroundCache() :
299 cache_time_offset(0),
301 reference_wgs84_point(SGVec3d(0, 0, 0)),
302 reference_vehicle_radius(0),
306 #ifdef GROUNDCACHE_DEBUG
307 _lookupTime = SGTimeStamp::fromSec(0.0);
309 _buildTime = SGTimeStamp::fromSec(0.0);
314 FGGroundCache::~FGGroundCache()
319 FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
320 const SGVec3d& pt, double rad)
322 #ifdef GROUNDCACHE_DEBUG
323 SGTimeStamp t0 = SGTimeStamp::now();
327 found_ground = false;
329 SGGeod geodPt = SGGeod::fromCart(pt);
330 // Don't blow away the cache ground_radius and stuff if there's no
332 if (!globals->get_tile_mgr()->schedule_scenery(geodPt, rad, 1.0)) {
333 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
334 "returns false at " << geodPt << " " << pt << " " << rad);
339 // If we have an active wire, get some more area into the groundcache
341 rad = SGMiscd::max(200, rad);
343 // Store the parameters we used to build up that cache.
344 reference_wgs84_point = pt;
345 reference_vehicle_radius = rad;
346 // Store the time reference used to compute movements of moving triangles.
347 cache_ref_time = startSimTime;
349 // Get a normalized down vector valid for the whole cache
350 SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
351 down = hlToEc.rotate(SGVec3d(0, 0, 1));
353 // Get the ground cache, that is a local collision tree of the environment
354 startSimTime += cache_time_offset;
355 endSimTime += cache_time_offset;
356 CacheFill subtreeCollector(pt, down, rad, startSimTime, endSimTime);
357 globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
358 _localBvhTree = subtreeCollector.getBVHNode();
360 if (subtreeCollector.getHaveElevationBelowCache()) {
361 // Use the altitude value below the cache that we gathered during
363 _altitude = subtreeCollector.getElevationBelowCache();
364 _material = subtreeCollector.getMaterialBelowCache();
366 } else if (_localBvhTree) {
367 // We have nothing below us, so try starting with the lowest point
368 // upwards for a croase altitude value
369 SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down);
370 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
371 _localBvhTree->accept(lineSegmentVisitor);
373 if (!lineSegmentVisitor.empty()) {
374 SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
375 _altitude = geodPt.getElevationM();
376 _material = lineSegmentVisitor.getMaterial();
382 // Ok, still nothing here?? Last resort ...
384 const SGMaterial* m = NULL;
385 found_ground = globals->get_scenery()->
386 get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &m);
392 // Still not sucessful??
394 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
395 "cache without any scenery below the aircraft");
397 #ifdef GROUNDCACHE_DEBUG
398 t0 = SGTimeStamp::now() - t0;
402 if (_buildCount > 60) {
403 double buildTime = 0;
405 buildTime = _buildTime.toSecs()/_buildCount;
406 double lookupTime = 0;
408 lookupTime = _lookupTime.toSecs()/_lookupCount;
409 _buildTime = SGTimeStamp::fromSec(0.0);
411 _lookupTime = SGTimeStamp::fromSec(0.0);
413 SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
414 << ", lookup Time = " << lookupTime);
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);
422 _group->removeChildren(0, _group->getNumChildren());
424 int level = fgGetInt("/fdm/groundcache-debug-level");
426 simgear::BVHDebugCollectVisitor debug(endSimTime, level);
427 _localBvhTree->accept(debug);
428 _group->addChild(debug.getNode());
437 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
439 pt = reference_wgs84_point;
440 rad = reference_vehicle_radius;
441 ref_time = cache_ref_time;
445 class FGGroundCache::BodyFinder : public BVHVisitor {
447 BodyFinder(BVHNode::Id id, const double& t) :
449 _bodyToWorld(SGMatrixd::unit()),
450 _linearVelocity(0, 0, 0),
451 _angularVelocity(0, 0, 0),
455 virtual void apply(BVHGroup& leaf)
459 leaf.traverse(*this);
461 virtual void apply(BVHPageNode& leaf)
465 leaf.traverse(*this);
467 virtual void apply(BVHTransform& transform)
472 transform.traverse(*this);
475 _linearVelocity = transform.vecToWorld(_linearVelocity);
476 _angularVelocity = transform.vecToWorld(_angularVelocity);
477 _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
480 virtual void apply(BVHMotionTransform& transform)
485 if (_id == transform.getId()) {
488 transform.traverse(*this);
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;
501 virtual void apply(BVHLineGeometry& node) { }
502 virtual void apply(BVHStaticGeometry& node) { }
504 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
505 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
507 const SGMatrixd& getBodyToWorld() const
508 { return _bodyToWorld; }
509 const SGVec3d& getLinearVelocity() const
510 { return _linearVelocity; }
511 const SGVec3d& getAngularVelocity() const
512 { return _angularVelocity; }
515 { return !_foundId; }
518 simgear::BVHNode::Id _id;
520 SGMatrixd _bodyToWorld;
522 SGVec3d _linearVelocity;
523 SGVec3d _angularVelocity;
531 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
532 SGVec3d& angularVel, simgear::BVHNode::Id id)
534 // Get the transform matrix and velocities of a moving body with id at t.
537 t += cache_time_offset;
538 BodyFinder bodyFinder(id, t);
539 _localBvhTree->accept(bodyFinder);
540 if (bodyFinder.empty())
543 bodyToWorld = bodyFinder.getBodyToWorld();
544 linearVel = bodyFinder.getLinearVelocity();
545 angularVel = bodyFinder.getAngularVelocity();
550 class FGGroundCache::CatapultFinder : public BVHVisitor {
552 CatapultFinder(const SGSphered& sphere, const double& t) :
553 _haveLineSegment(false),
558 virtual void apply(BVHGroup& leaf)
560 if (!intersects(_sphere, leaf.getBoundingSphere()))
562 leaf.traverse(*this);
564 virtual void apply(BVHPageNode& leaf)
566 if (!intersects(_sphere, leaf.getBoundingSphere()))
568 leaf.traverse(*this);
570 virtual void apply(BVHTransform& transform)
572 if (!intersects(_sphere, transform.getBoundingSphere()))
575 SGSphered sphere = _sphere;
576 _sphere = transform.sphereToLocal(sphere);
577 bool haveLineSegment = _haveLineSegment;
578 _haveLineSegment = false;
580 transform.traverse(*this);
582 if (_haveLineSegment) {
583 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
584 _linearVelocity = transform.vecToWorld(_linearVelocity);
585 _angularVelocity = transform.vecToWorld(_angularVelocity);
587 _haveLineSegment |= haveLineSegment;
588 _sphere.setCenter(sphere.getCenter());
590 virtual void apply(BVHMotionTransform& transform)
592 if (!intersects(_sphere, transform.getBoundingSphere()))
595 SGSphered sphere = _sphere;
596 _sphere = transform.sphereToLocal(sphere, _time);
597 bool haveLineSegment = _haveLineSegment;
598 _haveLineSegment = false;
600 transform.traverse(*this);
602 if (_haveLineSegment) {
603 SGMatrixd toWorld = transform.getToWorldTransform(_time);
605 += transform.getLinearVelocityAt(_lineSegment.getStart());
606 _angularVelocity += transform.getAngularVelocity();
607 _linearVelocity = toWorld.xformVec(_linearVelocity);
608 _angularVelocity = toWorld.xformVec(_angularVelocity);
609 _lineSegment = _lineSegment.transform(toWorld);
611 _haveLineSegment |= haveLineSegment;
612 _sphere.setCenter(sphere.getCenter());
614 virtual void apply(BVHLineGeometry& node)
616 if (node.getType() != BVHLineGeometry::CarrierCatapult)
619 SGLineSegmentd lineSegment(node.getLineSegment());
620 if (!intersects(_sphere, lineSegment))
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;
630 virtual void apply(BVHStaticGeometry& node)
633 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
634 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
636 void setSphere(const SGSphered& sphere)
637 { _sphere = sphere; }
638 const SGSphered& getSphere() const
641 const SGLineSegmentd& getLineSegment() const
642 { return _lineSegment; }
643 const SGVec3d& getLinearVelocity() const
644 { return _linearVelocity; }
645 const SGVec3d& getAngularVelocity() const
646 { return _angularVelocity; }
648 bool getHaveLineSegment() const
649 { return _haveLineSegment; }
652 SGLineSegmentd _lineSegment;
653 SGVec3d _linearVelocity;
654 SGVec3d _angularVelocity;
656 bool _haveLineSegment;
663 FGGroundCache::get_cat(double t, const SGVec3d& pt,
664 SGVec3d end[2], SGVec3d vel[2])
666 double maxDistance = 1000;
668 // Get the wire in question
669 t += cache_time_offset;
670 CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
672 _localBvhTree->accept(catapultFinder);
674 if (!catapultFinder.getHaveLineSegment())
677 // prepare the returns
678 end[0] = catapultFinder.getLineSegment().getStart();
679 end[1] = catapultFinder.getLineSegment().getEnd();
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());
688 // Return the distance to the cat
689 return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
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)
697 #ifdef GROUNDCACHE_DEBUG
698 SGTimeStamp t0 = SGTimeStamp::now();
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);
706 _localBvhTree->accept(lineSegmentVisitor);
708 #ifdef GROUNDCACHE_DEBUG
709 t0 = SGTimeStamp::now() - t0;
714 if (!lineSegmentVisitor.empty()) {
715 // Have an intersection
716 contact = lineSegmentVisitor.getPoint();
717 normal = lineSegmentVisitor.getNormal();
718 if (0 < dot(normal, down))
720 linearVel = lineSegmentVisitor.getLinearVelocity();
721 angularVel = lineSegmentVisitor.getAngularVelocity();
722 material = lineSegmentVisitor.getMaterial();
723 id = lineSegmentVisitor.getId();
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);
734 linearVel = SGVec3d(0, 0, 0);
735 angularVel = SGVec3d(0, 0, 0);
736 material = _material;
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)
753 #ifdef GROUNDCACHE_DEBUG
754 SGTimeStamp t0 = SGTimeStamp::now();
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);
763 #ifdef GROUNDCACHE_DEBUG
764 t0 = SGTimeStamp::now() - t0;
769 if (nearestPointVisitor.empty())
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();
783 class FGGroundCache::WireIntersector : public BVHVisitor {
785 WireIntersector(const SGVec3d pt[4], const double& t) :
786 _linearVelocity(SGVec3d::zeros()),
787 _angularVelocity(SGVec3d::zeros()),
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]);
797 virtual void apply(BVHGroup& leaf)
799 if (!_intersects(leaf.getBoundingSphere()))
802 leaf.traverse(*this);
804 virtual void apply(BVHPageNode& leaf)
806 if (!_intersects(leaf.getBoundingSphere()))
809 leaf.traverse(*this);
811 virtual void apply(BVHTransform& transform)
813 if (!_intersects(transform.getBoundingSphere()))
816 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
817 _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
818 _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
820 transform.traverse(*this);
823 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
824 _linearVelocity = transform.vecToWorld(_linearVelocity);
825 _angularVelocity = transform.vecToWorld(_angularVelocity);
827 _triangles[0] = triangles[0];
828 _triangles[1] = triangles[1];
830 virtual void apply(BVHMotionTransform& transform)
832 if (!_intersects(transform.getBoundingSphere()))
835 SGMatrixd toLocal = transform.getToLocalTransform(_time);
837 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
838 _triangles[0] = triangles[0].transform(toLocal);
839 _triangles[1] = triangles[1].transform(toLocal);
841 transform.traverse(*this);
844 SGMatrixd toWorld = transform.getToWorldTransform(_time);
846 += transform.getLinearVelocityAt(_lineSegment.getStart());
847 _angularVelocity += transform.getAngularVelocity();
848 _linearVelocity = toWorld.xformVec(_linearVelocity);
849 _angularVelocity = toWorld.xformVec(_angularVelocity);
850 _lineSegment = _lineSegment.transform(toWorld);
852 _triangles[0] = triangles[0];
853 _triangles[1] = triangles[1];
855 virtual void apply(BVHLineGeometry& node)
857 if (node.getType() != BVHLineGeometry::CarrierWire)
859 SGLineSegmentd lineSegment(node.getLineSegment());
860 if (!_intersects(lineSegment))
863 _lineSegment = lineSegment;
864 _linearVelocity = SGVec3d::zeros();
865 _angularVelocity = SGVec3d::zeros();
868 virtual void apply(BVHStaticGeometry& node)
871 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
872 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
874 bool _intersects(const SGSphered& sphere) const
878 if (intersects(_triangles[0], sphere))
880 if (intersects(_triangles[1], sphere))
884 bool _intersects(const SGLineSegmentd& lineSegment) const
888 if (intersects(_triangles[0], lineSegment))
890 if (intersects(_triangles[1], lineSegment))
895 const SGLineSegmentd& getLineSegment() const
896 { return _lineSegment; }
897 const SGVec3d& getLinearVelocity() const
898 { return _linearVelocity; }
899 const SGVec3d& getAngularVelocity() const
900 { return _angularVelocity; }
902 const BVHLineGeometry* getWire() const
906 SGLineSegmentd _lineSegment;
907 SGVec3d _linearVelocity;
908 SGVec3d _angularVelocity;
909 const BVHLineGeometry* _wire;
911 SGTriangled _triangles[2];
915 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
917 // Get the wire in question
918 t += cache_time_offset;
919 WireIntersector wireIntersector(pt, t);
921 _localBvhTree->accept(wireIntersector);
923 _wire = wireIntersector.getWire();
927 class FGGroundCache::WireFinder : public BVHVisitor {
929 WireFinder(const BVHLineGeometry* wire, const double& t) :
932 _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
933 _linearVelocity(SGVec3d::zeros()),
934 _angularVelocity(SGVec3d::zeros()),
935 _haveLineSegment(false)
938 virtual void apply(BVHGroup& leaf)
940 if (_haveLineSegment)
942 leaf.traverse(*this);
944 virtual void apply(BVHPageNode& leaf)
946 if (_haveLineSegment)
948 leaf.traverse(*this);
950 virtual void apply(BVHTransform& transform)
952 if (_haveLineSegment)
955 transform.traverse(*this);
957 if (_haveLineSegment) {
958 _linearVelocity = transform.vecToWorld(_linearVelocity);
959 _angularVelocity = transform.vecToWorld(_angularVelocity);
960 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
963 virtual void apply(BVHMotionTransform& transform)
965 if (_haveLineSegment)
968 transform.traverse(*this);
970 if (_haveLineSegment) {
971 SGMatrixd toWorld = transform.getToWorldTransform(_time);
973 += transform.getLinearVelocityAt(_lineSegment.getStart());
974 _angularVelocity += transform.getAngularVelocity();
975 _linearVelocity = toWorld.xformVec(_linearVelocity);
976 _angularVelocity = toWorld.xformVec(_angularVelocity);
977 _lineSegment = _lineSegment.transform(toWorld);
980 virtual void apply(BVHLineGeometry& node)
982 if (_haveLineSegment)
986 if (node.getType() != BVHLineGeometry::CarrierWire)
988 _lineSegment = SGLineSegmentd(node.getLineSegment());
989 _linearVelocity = SGVec3d::zeros();
990 _angularVelocity = SGVec3d::zeros();
991 _haveLineSegment = true;
993 virtual void apply(BVHStaticGeometry&) { }
995 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
996 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
998 const SGLineSegmentd& getLineSegment() const
999 { return _lineSegment; }
1001 bool getHaveLineSegment() const
1002 { return _haveLineSegment; }
1004 const SGVec3d& getLinearVelocity() const
1005 { return _linearVelocity; }
1006 const SGVec3d& getAngularVelocity() const
1007 { return _angularVelocity; }
1010 const BVHLineGeometry* _wire;
1013 SGLineSegmentd _lineSegment;
1014 SGVec3d _linearVelocity;
1015 SGVec3d _angularVelocity;
1017 bool _haveLineSegment;
1020 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
1022 // Fast return if we do not have an active wire.
1026 // Get the wire in question
1027 t += cache_time_offset;
1028 WireFinder wireFinder(_wire, t);
1030 _localBvhTree->accept(wireFinder);
1032 if (!wireFinder.getHaveLineSegment())
1035 // prepare the returns
1036 end[0] = wireFinder.getLineSegment().getStart();
1037 end[1] = wireFinder.getLineSegment().getEnd();
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());
1049 void FGGroundCache::release_wire(void)