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>
47 #include <simgear/scene/bvh/BVHNode.hxx>
48 #include <simgear/scene/bvh/BVHGroup.hxx>
49 #include <simgear/scene/bvh/BVHTransform.hxx>
50 #include <simgear/scene/bvh/BVHMotionTransform.hxx>
51 #include <simgear/scene/bvh/BVHLineGeometry.hxx>
52 #include <simgear/scene/bvh/BVHStaticGeometry.hxx>
53 #include <simgear/scene/bvh/BVHStaticData.hxx>
54 #include <simgear/scene/bvh/BVHStaticNode.hxx>
55 #include <simgear/scene/bvh/BVHStaticTriangle.hxx>
56 #include <simgear/scene/bvh/BVHStaticBinary.hxx>
57 #include <simgear/scene/bvh/BVHSubTreeCollector.hxx>
58 #include <simgear/scene/bvh/BVHLineSegmentVisitor.hxx>
59 #include <simgear/scene/bvh/BVHNearestPointVisitor.hxx>
61 #ifdef GROUNDCACHE_DEBUG
62 #include <simgear/scene/bvh/BVHDebugCollectVisitor.hxx>
63 #include <Main/fg_props.hxx>
66 #include <Main/globals.hxx>
67 #include <Scenery/scenery.hxx>
68 #include <Scenery/tilemgr.hxx>
72 using namespace simgear;
74 class FGGroundCache::CacheFill : public osg::NodeVisitor {
76 CacheFill(const SGVec3d& center, const SGVec3d& down, const double& radius,
77 const double& startTime, const double& endTime) :
78 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
82 _startTime(startTime),
85 _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
89 setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
91 virtual void apply(osg::Node& node)
93 if (!testBoundingSphere(node.getBound()))
96 addBoundingVolume(node);
99 virtual void apply(osg::Group& group)
101 if (!testBoundingSphere(group.getBound()))
104 simgear::BVHSubTreeCollector::NodeList parentNodeList;
105 mSubTreeCollector.pushNodeList(parentNodeList);
108 addBoundingVolume(group);
110 mSubTreeCollector.popNodeList(parentNodeList);
113 virtual void apply(osg::Transform& transform)
114 { handleTransform(transform); }
115 virtual void apply(osg::Camera& camera)
117 if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
119 handleTransform(camera);
121 virtual void apply(osg::CameraView& transform)
122 { handleTransform(transform); }
123 virtual void apply(osg::MatrixTransform& transform)
124 { handleTransform(transform); }
125 virtual void apply(osg::PositionAttitudeTransform& transform)
126 { handleTransform(transform); }
128 void handleTransform(osg::Transform& transform)
130 // Hmm, may be this needs to be refined somehow ...
131 if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
134 if (!testBoundingSphere(transform.getBound()))
137 osg::Matrix inverseMatrix;
138 if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
141 if (!transform.computeLocalToWorldMatrix(matrix, this))
144 // Look for a velocity note
145 const SGSceneUserData::Velocity* velocity = getVelocity(transform);
147 SGVec3d center = _center;
148 SGVec3d down = _down;
149 double radius = _radius;
150 bool haveHit = _haveHit;
151 const SGMaterial* material = _material;
154 _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
155 _down = SGVec3d(osg::Matrix::transform3x3(_down.osg(), inverseMatrix));
157 SGVec3d staticCenter(_center);
159 double dtStart = velocity->referenceTime - _startTime;
160 SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
161 SGQuatd startOr(SGQuatd::fromAngleAxis(dtStart*velocity->angular));
162 startCenter = startOr.transform(startCenter);
164 double dtEnd = velocity->referenceTime - _endTime;
165 SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
166 SGQuatd endOr(SGQuatd::fromAngleAxis(dtEnd*velocity->angular));
167 endCenter = endOr.transform(endCenter);
169 _center = 0.5*(startCenter + endCenter);
170 _down = startOr.transform(_down);
171 _radius += 0.5*dist(startCenter, endCenter);
174 simgear::BVHSubTreeCollector::NodeList parentNodeList;
175 mSubTreeCollector.pushNodeList(parentNodeList);
177 addBoundingVolume(transform);
180 if (mSubTreeCollector.haveChildren()) {
182 simgear::BVHMotionTransform* bvhTransform;
183 bvhTransform = new simgear::BVHMotionTransform;
184 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
185 bvhTransform->setLinearVelocity(velocity->linear);
186 bvhTransform->setAngularVelocity(velocity->angular);
187 bvhTransform->setReferenceTime(velocity->referenceTime);
188 bvhTransform->setStartTime(_startTime);
189 bvhTransform->setEndTime(_endTime);
190 bvhTransform->setId(velocity->id);
192 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
194 simgear::BVHTransform* bvhTransform;
195 bvhTransform = new simgear::BVHTransform;
196 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
198 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
201 mSubTreeCollector.popNodeList(parentNodeList);
206 double dt = _startTime - velocity->referenceTime;
207 SGQuatd ori(SGQuatd::fromAngleAxis(dt*velocity->angular));
208 _sceneryHit = ori.transform(_sceneryHit);
209 _sceneryHit += dt*velocity->linear;
211 _sceneryHit = SGVec3d(matrix.preMult(_sceneryHit.osg()));
213 _material = material;
222 const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
224 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
227 return userData->getVelocity();
229 simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
231 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
234 return userData->getBVHNode();
236 void addBoundingVolume(osg::Node& node)
238 simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
242 // Find a croase ground intersection
243 SGLineSegmentd line(_center + _radius*_down, _center + _maxDown*_down);
244 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, _startTime);
245 bvNode->accept(lineSegmentVisitor);
246 if (!lineSegmentVisitor.empty()) {
247 _sceneryHit = lineSegmentVisitor.getPoint();
248 _material = lineSegmentVisitor.getMaterial();
249 _maxDown = SGMiscd::max(_radius, dot(_down, _sceneryHit - _center));
253 // Get that part of the local bv tree that intersects our sphere
255 mSubTreeCollector.setSphere(SGSphered(_center, _radius));
256 bvNode->accept(mSubTreeCollector);
259 bool testBoundingSphere(const osg::BoundingSphere& bound) const
264 SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
265 double maxDist = bound._radius + _radius;
266 return distSqr(downSeg, SGVec3d(bound._center)) <= maxDist*maxDist;
269 SGSharedPtr<simgear::BVHNode> getBVHNode() const
270 { return mSubTreeCollector.getNode(); }
272 bool getHaveElevationBelowCache() const
274 double getElevationBelowCache() const
275 { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
276 const SGMaterial* getMaterialBelowCache() const
277 { return _material; }
286 simgear::BVHSubTreeCollector mSubTreeCollector;
289 const SGMaterial* _material;
293 FGGroundCache::FGGroundCache() :
297 cache_time_offset(0),
299 reference_wgs84_point(SGVec3d(0, 0, 0)),
300 reference_vehicle_radius(0),
304 #ifdef GROUNDCACHE_DEBUG
305 _lookupTime = SGTimeStamp::fromSec(0.0);
307 _buildTime = SGTimeStamp::fromSec(0.0);
312 FGGroundCache::~FGGroundCache()
317 FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
318 const SGVec3d& pt, double rad)
320 #ifdef GROUNDCACHE_DEBUG
321 SGTimeStamp t0 = SGTimeStamp::now();
325 found_ground = false;
327 SGGeod geodPt = SGGeod::fromCart(pt);
328 // Don't blow away the cache ground_radius and stuff if there's no
330 if (!globals->get_tile_mgr()->scenery_available(geodPt, rad)) {
331 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
332 "returns false at " << geodPt << " " << pt << " " << rad);
337 // If we have an active wire, get some more area into the groundcache
339 rad = SGMiscd::max(200, rad);
341 // Store the parameters we used to build up that cache.
342 reference_wgs84_point = pt;
343 reference_vehicle_radius = rad;
344 // Store the time reference used to compute movements of moving triangles.
345 cache_ref_time = startSimTime;
347 // Get a normalized down vector valid for the whole cache
348 SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
349 down = hlToEc.rotate(SGVec3d(0, 0, 1));
351 // Get the ground cache, that is a local collision tree of the environment
352 startSimTime += cache_time_offset;
353 endSimTime += cache_time_offset;
354 CacheFill subtreeCollector(pt, down, rad, startSimTime, endSimTime);
355 globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
356 _localBvhTree = subtreeCollector.getBVHNode();
358 if (subtreeCollector.getHaveElevationBelowCache()) {
359 // Use the altitude value below the cache that we gathered during
361 _altitude = subtreeCollector.getElevationBelowCache();
362 _material = subtreeCollector.getMaterialBelowCache();
364 } else if (_localBvhTree) {
365 // We have nothing below us, so try starting with the lowest point
366 // upwards for a croase altitude value
367 SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down);
368 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
369 _localBvhTree->accept(lineSegmentVisitor);
371 if (!lineSegmentVisitor.empty()) {
372 SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
373 _altitude = geodPt.getElevationM();
374 _material = lineSegmentVisitor.getMaterial();
380 // Ok, still nothing here?? Last resort ...
382 found_ground = globals->get_scenery()->
383 get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
388 // Still not sucessful??
390 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
391 "cache without any scenery below the aircraft");
393 #ifdef GROUNDCACHE_DEBUG
394 t0 = SGTimeStamp::now() - t0;
398 if (_buildCount > 60) {
399 double buildTime = 0;
401 buildTime = _buildTime.toSecs()/_buildCount;
402 double lookupTime = 0;
404 lookupTime = _lookupTime.toSecs()/_lookupCount;
405 _buildTime = SGTimeStamp::fromSec(0.0);
407 _lookupTime = SGTimeStamp::fromSec(0.0);
409 SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
410 << ", lookup Time = " << lookupTime);
413 if (!_group.valid()) {
414 _group = new osg::Group;
415 globals->get_scenery()->get_scene_graph()->addChild(_group);
416 fgSetInt("/fdm/groundcache-debug-level", -3);
418 _group->removeChildren(0, _group->getNumChildren());
420 int level = fgGetInt("/fdm/groundcache-debug-level");
422 simgear::BVHDebugCollectVisitor debug(endSimTime, level);
423 _localBvhTree->accept(debug);
424 _group->addChild(debug.getNode());
433 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
435 pt = reference_wgs84_point;
436 rad = reference_vehicle_radius;
437 ref_time = cache_ref_time;
441 class FGGroundCache::BodyFinder : public BVHVisitor {
443 BodyFinder(BVHNode::Id id, const double& t) :
445 _bodyToWorld(SGMatrixd::unit()),
446 _linearVelocity(0, 0, 0),
447 _angularVelocity(0, 0, 0),
451 virtual void apply(BVHGroup& leaf)
455 leaf.traverse(*this);
457 virtual void apply(BVHTransform& transform)
462 transform.traverse(*this);
465 _linearVelocity = transform.vecToWorld(_linearVelocity);
466 _angularVelocity = transform.vecToWorld(_angularVelocity);
467 _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
470 virtual void apply(BVHMotionTransform& transform)
475 if (_id == transform.getId()) {
478 transform.traverse(*this);
482 SGMatrixd toWorld = transform.getToWorldTransform(_time);
483 SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
484 _linearVelocity += transform.getLinearVelocityAt(referencePoint);
485 _angularVelocity += transform.getAngularVelocity();
486 _linearVelocity = toWorld.xformVec(_linearVelocity);
487 _angularVelocity = toWorld.xformVec(_angularVelocity);
488 _bodyToWorld = toWorld*_bodyToWorld;
491 virtual void apply(BVHLineGeometry& node) { }
492 virtual void apply(BVHStaticGeometry& node) { }
494 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
495 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
497 const SGMatrixd& getBodyToWorld() const
498 { return _bodyToWorld; }
499 const SGVec3d& getLinearVelocity() const
500 { return _linearVelocity; }
501 const SGVec3d& getAngularVelocity() const
502 { return _angularVelocity; }
505 { return !_foundId; }
508 simgear::BVHNode::Id _id;
510 SGMatrixd _bodyToWorld;
512 SGVec3d _linearVelocity;
513 SGVec3d _angularVelocity;
521 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
522 SGVec3d& angularVel, simgear::BVHNode::Id id)
524 // Get the transform matrix and velocities of a moving body with id at t.
527 t += cache_time_offset;
528 BodyFinder bodyFinder(id, t);
529 _localBvhTree->accept(bodyFinder);
530 if (bodyFinder.empty())
533 bodyToWorld = bodyFinder.getBodyToWorld();
534 linearVel = bodyFinder.getLinearVelocity();
535 angularVel = bodyFinder.getAngularVelocity();
540 class FGGroundCache::CatapultFinder : public BVHVisitor {
542 CatapultFinder(const SGSphered& sphere, const double& t) :
543 _haveLineSegment(false),
548 virtual void apply(BVHGroup& leaf)
550 if (!intersects(_sphere, leaf.getBoundingSphere()))
552 leaf.traverse(*this);
554 virtual void apply(BVHTransform& transform)
556 if (!intersects(_sphere, transform.getBoundingSphere()))
559 SGSphered sphere = _sphere;
560 _sphere = transform.sphereToLocal(sphere);
561 bool haveLineSegment = _haveLineSegment;
562 _haveLineSegment = false;
564 transform.traverse(*this);
566 if (_haveLineSegment) {
567 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
568 _linearVelocity = transform.vecToWorld(_linearVelocity);
569 _angularVelocity = transform.vecToWorld(_angularVelocity);
571 _haveLineSegment |= haveLineSegment;
572 _sphere.setCenter(sphere.getCenter());
574 virtual void apply(BVHMotionTransform& transform)
576 if (!intersects(_sphere, transform.getBoundingSphere()))
579 SGSphered sphere = _sphere;
580 _sphere = transform.sphereToLocal(sphere, _time);
581 bool haveLineSegment = _haveLineSegment;
582 _haveLineSegment = false;
584 transform.traverse(*this);
586 if (_haveLineSegment) {
587 SGMatrixd toWorld = transform.getToWorldTransform(_time);
589 += transform.getLinearVelocityAt(_lineSegment.getStart());
590 _angularVelocity += transform.getAngularVelocity();
591 _linearVelocity = toWorld.xformVec(_linearVelocity);
592 _angularVelocity = toWorld.xformVec(_angularVelocity);
593 _lineSegment = _lineSegment.transform(toWorld);
595 _haveLineSegment |= haveLineSegment;
596 _sphere.setCenter(sphere.getCenter());
598 virtual void apply(BVHLineGeometry& node)
600 if (node.getType() != BVHLineGeometry::CarrierCatapult)
603 SGLineSegmentd lineSegment(node.getLineSegment());
604 if (!intersects(_sphere, lineSegment))
607 _lineSegment = lineSegment;
608 double dist = distSqr(lineSegment, getSphere().getCenter());
609 _sphere.setRadius(sqrt(dist));
610 _linearVelocity = SGVec3d::zeros();
611 _angularVelocity = SGVec3d::zeros();
612 _haveLineSegment = true;
614 virtual void apply(BVHStaticGeometry& node)
617 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
618 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
620 void setSphere(const SGSphered& sphere)
621 { _sphere = sphere; }
622 const SGSphered& getSphere() const
625 const SGLineSegmentd& getLineSegment() const
626 { return _lineSegment; }
627 const SGVec3d& getLinearVelocity() const
628 { return _linearVelocity; }
629 const SGVec3d& getAngularVelocity() const
630 { return _angularVelocity; }
632 bool getHaveLineSegment() const
633 { return _haveLineSegment; }
636 SGLineSegmentd _lineSegment;
637 SGVec3d _linearVelocity;
638 SGVec3d _angularVelocity;
640 bool _haveLineSegment;
647 FGGroundCache::get_cat(double t, const SGVec3d& pt,
648 SGVec3d end[2], SGVec3d vel[2])
650 double maxDistance = 1000;
652 // Get the wire in question
653 t += cache_time_offset;
654 CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
656 _localBvhTree->accept(catapultFinder);
658 if (!catapultFinder.getHaveLineSegment())
661 // prepare the returns
662 end[0] = catapultFinder.getLineSegment().getStart();
663 end[1] = catapultFinder.getLineSegment().getEnd();
665 // The linear velocity is the one at the start of the line segment ...
666 vel[0] = catapultFinder.getLinearVelocity();
667 // ... so the end point has the additional cross product.
668 vel[1] = catapultFinder.getLinearVelocity();
669 vel[1] += cross(catapultFinder.getAngularVelocity(),
670 catapultFinder.getLineSegment().getDirection());
672 // Return the distance to the cat
673 return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
677 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
678 SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
679 simgear::BVHNode::Id& id, const SGMaterial*& material)
681 #ifdef GROUNDCACHE_DEBUG
682 SGTimeStamp t0 = SGTimeStamp::now();
685 // Just set up a ground intersection query for the given point
686 SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
687 t += cache_time_offset;
688 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
690 _localBvhTree->accept(lineSegmentVisitor);
692 #ifdef GROUNDCACHE_DEBUG
693 t0 = SGTimeStamp::now() - t0;
698 if (!lineSegmentVisitor.empty()) {
699 // Have an intersection
700 contact = lineSegmentVisitor.getPoint();
701 normal = lineSegmentVisitor.getNormal();
702 if (0 < dot(normal, down))
704 linearVel = lineSegmentVisitor.getLinearVelocity();
705 angularVel = lineSegmentVisitor.getAngularVelocity();
706 material = lineSegmentVisitor.getMaterial();
707 id = lineSegmentVisitor.getId();
711 // Whenever we did not have a ground triangle for the requested point,
712 // take the ground level we found during the current cache build.
713 // This is as good as what we had before for agl.
714 SGGeod geodPt = SGGeod::fromCart(pt);
715 geodPt.setElevationM(_altitude);
716 contact = SGVec3d::fromGeod(geodPt);
718 linearVel = SGVec3d(0, 0, 0);
719 angularVel = SGVec3d(0, 0, 0);
720 material = _material;
729 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
730 SGVec3d& contact, SGVec3d& linearVel,
731 SGVec3d& angularVel, simgear::BVHNode::Id& id,
732 const SGMaterial*& material)
737 #ifdef GROUNDCACHE_DEBUG
738 SGTimeStamp t0 = SGTimeStamp::now();
741 // Just set up a ground intersection query for the given point
742 SGSphered sphere(pt, maxDist);
743 t += cache_time_offset;
744 simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
745 _localBvhTree->accept(nearestPointVisitor);
747 #ifdef GROUNDCACHE_DEBUG
748 t0 = SGTimeStamp::now() - t0;
753 if (nearestPointVisitor.empty())
756 // Have geometry in the range of maxDist
757 contact = nearestPointVisitor.getPoint();
758 linearVel = nearestPointVisitor.getLinearVelocity();
759 angularVel = nearestPointVisitor.getAngularVelocity();
760 material = nearestPointVisitor.getMaterial();
761 id = nearestPointVisitor.getId();
767 class FGGroundCache::WireIntersector : public BVHVisitor {
769 WireIntersector(const SGVec3d pt[4], const double& t) :
770 _linearVelocity(SGVec3d::zeros()),
771 _angularVelocity(SGVec3d::zeros()),
775 // Build the two triangles spanning the area where the hook has moved
776 // during the past step.
777 _triangles[0].set(pt[0], pt[1], pt[2]);
778 _triangles[1].set(pt[0], pt[2], pt[3]);
781 virtual void apply(BVHGroup& leaf)
783 if (!_intersects(leaf.getBoundingSphere()))
786 leaf.traverse(*this);
788 virtual void apply(BVHTransform& transform)
790 if (!_intersects(transform.getBoundingSphere()))
793 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
794 _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
795 _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
797 transform.traverse(*this);
800 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
801 _linearVelocity = transform.vecToWorld(_linearVelocity);
802 _angularVelocity = transform.vecToWorld(_angularVelocity);
804 _triangles[0] = triangles[0];
805 _triangles[1] = triangles[1];
807 virtual void apply(BVHMotionTransform& transform)
809 if (!_intersects(transform.getBoundingSphere()))
812 SGMatrixd toLocal = transform.getToLocalTransform(_time);
814 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
815 _triangles[0] = triangles[0].transform(toLocal);
816 _triangles[1] = triangles[1].transform(toLocal);
818 transform.traverse(*this);
821 SGMatrixd toWorld = transform.getToWorldTransform(_time);
823 += transform.getLinearVelocityAt(_lineSegment.getStart());
824 _angularVelocity += transform.getAngularVelocity();
825 _linearVelocity = toWorld.xformVec(_linearVelocity);
826 _angularVelocity = toWorld.xformVec(_angularVelocity);
827 _lineSegment = _lineSegment.transform(toWorld);
829 _triangles[0] = triangles[0];
830 _triangles[1] = triangles[1];
832 virtual void apply(BVHLineGeometry& node)
834 if (node.getType() != BVHLineGeometry::CarrierWire)
836 SGLineSegmentd lineSegment(node.getLineSegment());
837 if (!_intersects(lineSegment))
840 _lineSegment = lineSegment;
841 _linearVelocity = SGVec3d::zeros();
842 _angularVelocity = SGVec3d::zeros();
845 virtual void apply(BVHStaticGeometry& node)
848 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
849 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
851 bool _intersects(const SGSphered& sphere) const
855 if (intersects(_triangles[0], sphere))
857 if (intersects(_triangles[1], sphere))
861 bool _intersects(const SGLineSegmentd& lineSegment) const
865 if (intersects(_triangles[0], lineSegment))
867 if (intersects(_triangles[1], lineSegment))
872 const SGLineSegmentd& getLineSegment() const
873 { return _lineSegment; }
874 const SGVec3d& getLinearVelocity() const
875 { return _linearVelocity; }
876 const SGVec3d& getAngularVelocity() const
877 { return _angularVelocity; }
879 const BVHLineGeometry* getWire() const
883 SGLineSegmentd _lineSegment;
884 SGVec3d _linearVelocity;
885 SGVec3d _angularVelocity;
886 const BVHLineGeometry* _wire;
888 SGTriangled _triangles[2];
892 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
894 // Get the wire in question
895 t += cache_time_offset;
896 WireIntersector wireIntersector(pt, t);
898 _localBvhTree->accept(wireIntersector);
900 _wire = wireIntersector.getWire();
904 class FGGroundCache::WireFinder : public BVHVisitor {
906 WireFinder(const BVHLineGeometry* wire, const double& t) :
909 _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
910 _linearVelocity(SGVec3d::zeros()),
911 _angularVelocity(SGVec3d::zeros()),
912 _haveLineSegment(false)
915 virtual void apply(BVHGroup& leaf)
917 if (_haveLineSegment)
919 leaf.traverse(*this);
921 virtual void apply(BVHTransform& transform)
923 if (_haveLineSegment)
926 transform.traverse(*this);
928 if (_haveLineSegment) {
929 _linearVelocity = transform.vecToWorld(_linearVelocity);
930 _angularVelocity = transform.vecToWorld(_angularVelocity);
931 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
934 virtual void apply(BVHMotionTransform& transform)
936 if (_haveLineSegment)
939 transform.traverse(*this);
941 if (_haveLineSegment) {
942 SGMatrixd toWorld = transform.getToWorldTransform(_time);
944 += transform.getLinearVelocityAt(_lineSegment.getStart());
945 _angularVelocity += transform.getAngularVelocity();
946 _linearVelocity = toWorld.xformVec(_linearVelocity);
947 _angularVelocity = toWorld.xformVec(_angularVelocity);
948 _lineSegment = _lineSegment.transform(toWorld);
951 virtual void apply(BVHLineGeometry& node)
953 if (_haveLineSegment)
957 if (node.getType() != BVHLineGeometry::CarrierWire)
959 _lineSegment = SGLineSegmentd(node.getLineSegment());
960 _linearVelocity = SGVec3d::zeros();
961 _angularVelocity = SGVec3d::zeros();
962 _haveLineSegment = true;
964 virtual void apply(BVHStaticGeometry&) { }
966 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
967 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
969 const SGLineSegmentd& getLineSegment() const
970 { return _lineSegment; }
972 bool getHaveLineSegment() const
973 { return _haveLineSegment; }
975 const SGVec3d& getLinearVelocity() const
976 { return _linearVelocity; }
977 const SGVec3d& getAngularVelocity() const
978 { return _angularVelocity; }
981 const BVHLineGeometry* _wire;
984 SGLineSegmentd _lineSegment;
985 SGVec3d _linearVelocity;
986 SGVec3d _angularVelocity;
988 bool _haveLineSegment;
991 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
993 // Fast return if we do not have an active wire.
997 // Get the wire in question
998 t += cache_time_offset;
999 WireFinder wireFinder(_wire, t);
1001 _localBvhTree->accept(wireFinder);
1003 if (!wireFinder.getHaveLineSegment())
1006 // prepare the returns
1007 end[0] = wireFinder.getLineSegment().getStart();
1008 end[1] = wireFinder.getLineSegment().getEnd();
1010 // The linear velocity is the one at the start of the line segment ...
1011 vel[0] = wireFinder.getLinearVelocity();
1012 // ... so the end point has the additional cross product.
1013 vel[1] = wireFinder.getLinearVelocity();
1014 vel[1] += cross(wireFinder.getAngularVelocity(),
1015 wireFinder.getLineSegment().getDirection());
1020 void FGGroundCache::release_wire(void)