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 = toSG(inverseMatrix.preMult(toOsg(_center)));
155 _down = toSG(osg::Matrix::transform3x3(toOsg(_down), 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 = toSG(matrix.preMult(toOsg(_sceneryHit)));
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 SGVec3d boundCenter(toVec3d(toSG(bound._center)));
267 return distSqr(downSeg, boundCenter) <= maxDist*maxDist;
270 SGSharedPtr<simgear::BVHNode> getBVHNode() const
271 { return mSubTreeCollector.getNode(); }
273 bool getHaveElevationBelowCache() const
275 double getElevationBelowCache() const
276 { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
277 const SGMaterial* getMaterialBelowCache() const
278 { return _material; }
287 simgear::BVHSubTreeCollector mSubTreeCollector;
290 const SGMaterial* _material;
294 FGGroundCache::FGGroundCache() :
298 cache_time_offset(0),
300 reference_wgs84_point(SGVec3d(0, 0, 0)),
301 reference_vehicle_radius(0),
305 #ifdef GROUNDCACHE_DEBUG
306 _lookupTime = SGTimeStamp::fromSec(0.0);
308 _buildTime = SGTimeStamp::fromSec(0.0);
313 FGGroundCache::~FGGroundCache()
318 FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
319 const SGVec3d& pt, double rad)
321 #ifdef GROUNDCACHE_DEBUG
322 SGTimeStamp t0 = SGTimeStamp::now();
326 found_ground = false;
328 SGGeod geodPt = SGGeod::fromCart(pt);
329 // Don't blow away the cache ground_radius and stuff if there's no
331 if (!globals->get_tile_mgr()->scenery_available(geodPt, rad)) {
332 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
333 "returns false at " << geodPt << " " << pt << " " << rad);
338 // If we have an active wire, get some more area into the groundcache
340 rad = SGMiscd::max(200, rad);
342 // Store the parameters we used to build up that cache.
343 reference_wgs84_point = pt;
344 reference_vehicle_radius = rad;
345 // Store the time reference used to compute movements of moving triangles.
346 cache_ref_time = startSimTime;
348 // Get a normalized down vector valid for the whole cache
349 SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
350 down = hlToEc.rotate(SGVec3d(0, 0, 1));
352 // Get the ground cache, that is a local collision tree of the environment
353 startSimTime += cache_time_offset;
354 endSimTime += cache_time_offset;
355 CacheFill subtreeCollector(pt, down, rad, startSimTime, endSimTime);
356 globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
357 _localBvhTree = subtreeCollector.getBVHNode();
359 if (subtreeCollector.getHaveElevationBelowCache()) {
360 // Use the altitude value below the cache that we gathered during
362 _altitude = subtreeCollector.getElevationBelowCache();
363 _material = subtreeCollector.getMaterialBelowCache();
365 } else if (_localBvhTree) {
366 // We have nothing below us, so try starting with the lowest point
367 // upwards for a croase altitude value
368 SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down);
369 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
370 _localBvhTree->accept(lineSegmentVisitor);
372 if (!lineSegmentVisitor.empty()) {
373 SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
374 _altitude = geodPt.getElevationM();
375 _material = lineSegmentVisitor.getMaterial();
381 // Ok, still nothing here?? Last resort ...
383 found_ground = globals->get_scenery()->
384 get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
389 // Still not sucessful??
391 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
392 "cache without any scenery below the aircraft");
394 #ifdef GROUNDCACHE_DEBUG
395 t0 = SGTimeStamp::now() - t0;
399 if (_buildCount > 60) {
400 double buildTime = 0;
402 buildTime = _buildTime.toSecs()/_buildCount;
403 double lookupTime = 0;
405 lookupTime = _lookupTime.toSecs()/_lookupCount;
406 _buildTime = SGTimeStamp::fromSec(0.0);
408 _lookupTime = SGTimeStamp::fromSec(0.0);
410 SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
411 << ", lookup Time = " << lookupTime);
414 if (!_group.valid()) {
415 _group = new osg::Group;
416 globals->get_scenery()->get_scene_graph()->addChild(_group);
417 fgSetInt("/fdm/groundcache-debug-level", -3);
419 _group->removeChildren(0, _group->getNumChildren());
421 int level = fgGetInt("/fdm/groundcache-debug-level");
423 simgear::BVHDebugCollectVisitor debug(endSimTime, level);
424 _localBvhTree->accept(debug);
425 _group->addChild(debug.getNode());
434 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
436 pt = reference_wgs84_point;
437 rad = reference_vehicle_radius;
438 ref_time = cache_ref_time;
442 class FGGroundCache::BodyFinder : public BVHVisitor {
444 BodyFinder(BVHNode::Id id, const double& t) :
446 _bodyToWorld(SGMatrixd::unit()),
447 _linearVelocity(0, 0, 0),
448 _angularVelocity(0, 0, 0),
452 virtual void apply(BVHGroup& leaf)
456 leaf.traverse(*this);
458 virtual void apply(BVHTransform& transform)
463 transform.traverse(*this);
466 _linearVelocity = transform.vecToWorld(_linearVelocity);
467 _angularVelocity = transform.vecToWorld(_angularVelocity);
468 _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
471 virtual void apply(BVHMotionTransform& transform)
476 if (_id == transform.getId()) {
479 transform.traverse(*this);
483 SGMatrixd toWorld = transform.getToWorldTransform(_time);
484 SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
485 _linearVelocity += transform.getLinearVelocityAt(referencePoint);
486 _angularVelocity += transform.getAngularVelocity();
487 _linearVelocity = toWorld.xformVec(_linearVelocity);
488 _angularVelocity = toWorld.xformVec(_angularVelocity);
489 _bodyToWorld = toWorld*_bodyToWorld;
492 virtual void apply(BVHLineGeometry& node) { }
493 virtual void apply(BVHStaticGeometry& node) { }
495 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
496 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
498 const SGMatrixd& getBodyToWorld() const
499 { return _bodyToWorld; }
500 const SGVec3d& getLinearVelocity() const
501 { return _linearVelocity; }
502 const SGVec3d& getAngularVelocity() const
503 { return _angularVelocity; }
506 { return !_foundId; }
509 simgear::BVHNode::Id _id;
511 SGMatrixd _bodyToWorld;
513 SGVec3d _linearVelocity;
514 SGVec3d _angularVelocity;
522 FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
523 SGVec3d& angularVel, simgear::BVHNode::Id id)
525 // Get the transform matrix and velocities of a moving body with id at t.
528 t += cache_time_offset;
529 BodyFinder bodyFinder(id, t);
530 _localBvhTree->accept(bodyFinder);
531 if (bodyFinder.empty())
534 bodyToWorld = bodyFinder.getBodyToWorld();
535 linearVel = bodyFinder.getLinearVelocity();
536 angularVel = bodyFinder.getAngularVelocity();
541 class FGGroundCache::CatapultFinder : public BVHVisitor {
543 CatapultFinder(const SGSphered& sphere, const double& t) :
544 _haveLineSegment(false),
549 virtual void apply(BVHGroup& leaf)
551 if (!intersects(_sphere, leaf.getBoundingSphere()))
553 leaf.traverse(*this);
555 virtual void apply(BVHTransform& transform)
557 if (!intersects(_sphere, transform.getBoundingSphere()))
560 SGSphered sphere = _sphere;
561 _sphere = transform.sphereToLocal(sphere);
562 bool haveLineSegment = _haveLineSegment;
563 _haveLineSegment = false;
565 transform.traverse(*this);
567 if (_haveLineSegment) {
568 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
569 _linearVelocity = transform.vecToWorld(_linearVelocity);
570 _angularVelocity = transform.vecToWorld(_angularVelocity);
572 _haveLineSegment |= haveLineSegment;
573 _sphere.setCenter(sphere.getCenter());
575 virtual void apply(BVHMotionTransform& transform)
577 if (!intersects(_sphere, transform.getBoundingSphere()))
580 SGSphered sphere = _sphere;
581 _sphere = transform.sphereToLocal(sphere, _time);
582 bool haveLineSegment = _haveLineSegment;
583 _haveLineSegment = false;
585 transform.traverse(*this);
587 if (_haveLineSegment) {
588 SGMatrixd toWorld = transform.getToWorldTransform(_time);
590 += transform.getLinearVelocityAt(_lineSegment.getStart());
591 _angularVelocity += transform.getAngularVelocity();
592 _linearVelocity = toWorld.xformVec(_linearVelocity);
593 _angularVelocity = toWorld.xformVec(_angularVelocity);
594 _lineSegment = _lineSegment.transform(toWorld);
596 _haveLineSegment |= haveLineSegment;
597 _sphere.setCenter(sphere.getCenter());
599 virtual void apply(BVHLineGeometry& node)
601 if (node.getType() != BVHLineGeometry::CarrierCatapult)
604 SGLineSegmentd lineSegment(node.getLineSegment());
605 if (!intersects(_sphere, lineSegment))
608 _lineSegment = lineSegment;
609 double dist = distSqr(lineSegment, getSphere().getCenter());
610 _sphere.setRadius(sqrt(dist));
611 _linearVelocity = SGVec3d::zeros();
612 _angularVelocity = SGVec3d::zeros();
613 _haveLineSegment = true;
615 virtual void apply(BVHStaticGeometry& node)
618 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
619 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
621 void setSphere(const SGSphered& sphere)
622 { _sphere = sphere; }
623 const SGSphered& getSphere() const
626 const SGLineSegmentd& getLineSegment() const
627 { return _lineSegment; }
628 const SGVec3d& getLinearVelocity() const
629 { return _linearVelocity; }
630 const SGVec3d& getAngularVelocity() const
631 { return _angularVelocity; }
633 bool getHaveLineSegment() const
634 { return _haveLineSegment; }
637 SGLineSegmentd _lineSegment;
638 SGVec3d _linearVelocity;
639 SGVec3d _angularVelocity;
641 bool _haveLineSegment;
648 FGGroundCache::get_cat(double t, const SGVec3d& pt,
649 SGVec3d end[2], SGVec3d vel[2])
651 double maxDistance = 1000;
653 // Get the wire in question
654 t += cache_time_offset;
655 CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
657 _localBvhTree->accept(catapultFinder);
659 if (!catapultFinder.getHaveLineSegment())
662 // prepare the returns
663 end[0] = catapultFinder.getLineSegment().getStart();
664 end[1] = catapultFinder.getLineSegment().getEnd();
666 // The linear velocity is the one at the start of the line segment ...
667 vel[0] = catapultFinder.getLinearVelocity();
668 // ... so the end point has the additional cross product.
669 vel[1] = catapultFinder.getLinearVelocity();
670 vel[1] += cross(catapultFinder.getAngularVelocity(),
671 catapultFinder.getLineSegment().getDirection());
673 // Return the distance to the cat
674 return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
678 FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
679 SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
680 simgear::BVHNode::Id& id, const SGMaterial*& material)
682 #ifdef GROUNDCACHE_DEBUG
683 SGTimeStamp t0 = SGTimeStamp::now();
686 // Just set up a ground intersection query for the given point
687 SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
688 t += cache_time_offset;
689 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
691 _localBvhTree->accept(lineSegmentVisitor);
693 #ifdef GROUNDCACHE_DEBUG
694 t0 = SGTimeStamp::now() - t0;
699 if (!lineSegmentVisitor.empty()) {
700 // Have an intersection
701 contact = lineSegmentVisitor.getPoint();
702 normal = lineSegmentVisitor.getNormal();
703 if (0 < dot(normal, down))
705 linearVel = lineSegmentVisitor.getLinearVelocity();
706 angularVel = lineSegmentVisitor.getAngularVelocity();
707 material = lineSegmentVisitor.getMaterial();
708 id = lineSegmentVisitor.getId();
712 // Whenever we did not have a ground triangle for the requested point,
713 // take the ground level we found during the current cache build.
714 // This is as good as what we had before for agl.
715 SGGeod geodPt = SGGeod::fromCart(pt);
716 geodPt.setElevationM(_altitude);
717 contact = SGVec3d::fromGeod(geodPt);
719 linearVel = SGVec3d(0, 0, 0);
720 angularVel = SGVec3d(0, 0, 0);
721 material = _material;
730 FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
731 SGVec3d& contact, SGVec3d& linearVel,
732 SGVec3d& angularVel, simgear::BVHNode::Id& id,
733 const SGMaterial*& material)
738 #ifdef GROUNDCACHE_DEBUG
739 SGTimeStamp t0 = SGTimeStamp::now();
742 // Just set up a ground intersection query for the given point
743 SGSphered sphere(pt, maxDist);
744 t += cache_time_offset;
745 simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
746 _localBvhTree->accept(nearestPointVisitor);
748 #ifdef GROUNDCACHE_DEBUG
749 t0 = SGTimeStamp::now() - t0;
754 if (nearestPointVisitor.empty())
757 // Have geometry in the range of maxDist
758 contact = nearestPointVisitor.getPoint();
759 linearVel = nearestPointVisitor.getLinearVelocity();
760 angularVel = nearestPointVisitor.getAngularVelocity();
761 material = nearestPointVisitor.getMaterial();
762 id = nearestPointVisitor.getId();
768 class FGGroundCache::WireIntersector : public BVHVisitor {
770 WireIntersector(const SGVec3d pt[4], const double& t) :
771 _linearVelocity(SGVec3d::zeros()),
772 _angularVelocity(SGVec3d::zeros()),
776 // Build the two triangles spanning the area where the hook has moved
777 // during the past step.
778 _triangles[0].set(pt[0], pt[1], pt[2]);
779 _triangles[1].set(pt[0], pt[2], pt[3]);
782 virtual void apply(BVHGroup& leaf)
784 if (!_intersects(leaf.getBoundingSphere()))
787 leaf.traverse(*this);
789 virtual void apply(BVHTransform& transform)
791 if (!_intersects(transform.getBoundingSphere()))
794 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
795 _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
796 _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
798 transform.traverse(*this);
801 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
802 _linearVelocity = transform.vecToWorld(_linearVelocity);
803 _angularVelocity = transform.vecToWorld(_angularVelocity);
805 _triangles[0] = triangles[0];
806 _triangles[1] = triangles[1];
808 virtual void apply(BVHMotionTransform& transform)
810 if (!_intersects(transform.getBoundingSphere()))
813 SGMatrixd toLocal = transform.getToLocalTransform(_time);
815 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
816 _triangles[0] = triangles[0].transform(toLocal);
817 _triangles[1] = triangles[1].transform(toLocal);
819 transform.traverse(*this);
822 SGMatrixd toWorld = transform.getToWorldTransform(_time);
824 += transform.getLinearVelocityAt(_lineSegment.getStart());
825 _angularVelocity += transform.getAngularVelocity();
826 _linearVelocity = toWorld.xformVec(_linearVelocity);
827 _angularVelocity = toWorld.xformVec(_angularVelocity);
828 _lineSegment = _lineSegment.transform(toWorld);
830 _triangles[0] = triangles[0];
831 _triangles[1] = triangles[1];
833 virtual void apply(BVHLineGeometry& node)
835 if (node.getType() != BVHLineGeometry::CarrierWire)
837 SGLineSegmentd lineSegment(node.getLineSegment());
838 if (!_intersects(lineSegment))
841 _lineSegment = lineSegment;
842 _linearVelocity = SGVec3d::zeros();
843 _angularVelocity = SGVec3d::zeros();
846 virtual void apply(BVHStaticGeometry& node)
849 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
850 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
852 bool _intersects(const SGSphered& sphere) const
856 if (intersects(_triangles[0], sphere))
858 if (intersects(_triangles[1], sphere))
862 bool _intersects(const SGLineSegmentd& lineSegment) const
866 if (intersects(_triangles[0], lineSegment))
868 if (intersects(_triangles[1], lineSegment))
873 const SGLineSegmentd& getLineSegment() const
874 { return _lineSegment; }
875 const SGVec3d& getLinearVelocity() const
876 { return _linearVelocity; }
877 const SGVec3d& getAngularVelocity() const
878 { return _angularVelocity; }
880 const BVHLineGeometry* getWire() const
884 SGLineSegmentd _lineSegment;
885 SGVec3d _linearVelocity;
886 SGVec3d _angularVelocity;
887 const BVHLineGeometry* _wire;
889 SGTriangled _triangles[2];
893 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
895 // Get the wire in question
896 t += cache_time_offset;
897 WireIntersector wireIntersector(pt, t);
899 _localBvhTree->accept(wireIntersector);
901 _wire = wireIntersector.getWire();
905 class FGGroundCache::WireFinder : public BVHVisitor {
907 WireFinder(const BVHLineGeometry* wire, const double& t) :
910 _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
911 _linearVelocity(SGVec3d::zeros()),
912 _angularVelocity(SGVec3d::zeros()),
913 _haveLineSegment(false)
916 virtual void apply(BVHGroup& leaf)
918 if (_haveLineSegment)
920 leaf.traverse(*this);
922 virtual void apply(BVHTransform& transform)
924 if (_haveLineSegment)
927 transform.traverse(*this);
929 if (_haveLineSegment) {
930 _linearVelocity = transform.vecToWorld(_linearVelocity);
931 _angularVelocity = transform.vecToWorld(_angularVelocity);
932 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
935 virtual void apply(BVHMotionTransform& transform)
937 if (_haveLineSegment)
940 transform.traverse(*this);
942 if (_haveLineSegment) {
943 SGMatrixd toWorld = transform.getToWorldTransform(_time);
945 += transform.getLinearVelocityAt(_lineSegment.getStart());
946 _angularVelocity += transform.getAngularVelocity();
947 _linearVelocity = toWorld.xformVec(_linearVelocity);
948 _angularVelocity = toWorld.xformVec(_angularVelocity);
949 _lineSegment = _lineSegment.transform(toWorld);
952 virtual void apply(BVHLineGeometry& node)
954 if (_haveLineSegment)
958 if (node.getType() != BVHLineGeometry::CarrierWire)
960 _lineSegment = SGLineSegmentd(node.getLineSegment());
961 _linearVelocity = SGVec3d::zeros();
962 _angularVelocity = SGVec3d::zeros();
963 _haveLineSegment = true;
965 virtual void apply(BVHStaticGeometry&) { }
967 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
968 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
970 const SGLineSegmentd& getLineSegment() const
971 { return _lineSegment; }
973 bool getHaveLineSegment() const
974 { return _haveLineSegment; }
976 const SGVec3d& getLinearVelocity() const
977 { return _linearVelocity; }
978 const SGVec3d& getAngularVelocity() const
979 { return _angularVelocity; }
982 const BVHLineGeometry* _wire;
985 SGLineSegmentd _lineSegment;
986 SGVec3d _linearVelocity;
987 SGVec3d _angularVelocity;
989 bool _haveLineSegment;
992 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
994 // Fast return if we do not have an active wire.
998 // Get the wire in question
999 t += cache_time_offset;
1000 WireFinder wireFinder(_wire, t);
1002 _localBvhTree->accept(wireFinder);
1004 if (!wireFinder.getHaveLineSegment())
1007 // prepare the returns
1008 end[0] = wireFinder.getLineSegment().getStart();
1009 end[1] = wireFinder.getLineSegment().getEnd();
1011 // The linear velocity is the one at the start of the line segment ...
1012 vel[0] = wireFinder.getLinearVelocity();
1013 // ... so the end point has the additional cross product.
1014 vel[1] = wireFinder.getLinearVelocity();
1015 vel[1] += cross(wireFinder.getAngularVelocity(),
1016 wireFinder.getLineSegment().getDirection());
1021 void FGGroundCache::release_wire(void)