1 // groundcache.cxx -- carries a small subset of the scenegraph near the vehicle
3 // Written by Mathias Froehlich, started Nov 2004.
5 // Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
29 #include <osg/Drawable>
31 #include <osg/Geometry>
33 #include <osg/Transform>
34 #include <osg/MatrixTransform>
35 #include <osg/PositionAttitudeTransform>
36 #include <osg/CameraView>
38 #include <simgear/sg_inlines.h>
39 #include <simgear/constants.h>
40 #include <simgear/debug/logstream.hxx>
41 #include <simgear/math/sg_geodesy.hxx>
42 #include <simgear/scene/material/mat.hxx>
43 #include <simgear/scene/util/SGNodeMasks.hxx>
44 #include <simgear/scene/util/SGSceneUserData.hxx>
45 #include <simgear/scene/model/placementtrans.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>
60 #include <Main/globals.hxx>
61 #include <Scenery/scenery.hxx>
62 #include <Scenery/tilemgr.hxx>
65 #include "groundcache.hxx"
67 using namespace simgear;
69 static FGInterface::GroundType
70 materialToGroundType(const SGMaterial* material)
73 return FGInterface::Solid;
74 if (material->get_solid())
75 return FGInterface::Solid;
76 return FGInterface::Water;
79 class FGGroundCache::CacheFill : public osg::NodeVisitor {
81 CacheFill(const SGVec3d& center, const double& radius,
82 const double& startTime, const double& endTime) :
83 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
86 _startTime(startTime),
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);
146 // ... no velocity of there is only zero velocity
147 if (velocity && velocity->linear == SGVec3d::zeros() &&
148 velocity->angular == SGVec3d::zeros())
151 SGVec3d center = _center;
152 _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
153 double radius = _radius;
155 _radius += (_endTime - _startTime)*norm(velocity->linear);
157 simgear::BVHSubTreeCollector::NodeList parentNodeList;
158 mSubTreeCollector.pushNodeList(parentNodeList);
160 addBoundingVolume(transform);
163 if (mSubTreeCollector.haveChildren()) {
165 simgear::BVHMotionTransform* bvhTransform;
166 bvhTransform = new simgear::BVHMotionTransform;
167 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
168 bvhTransform->setLinearVelocity(velocity->linear);
169 bvhTransform->setAngularVelocity(velocity->angular);
170 bvhTransform->setReferenceTime(_startTime);
171 bvhTransform->setEndTime(_endTime);
173 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
175 simgear::BVHTransform* bvhTransform;
176 bvhTransform = new simgear::BVHTransform;
177 bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
179 mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
182 mSubTreeCollector.popNodeList(parentNodeList);
188 const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
190 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
193 return userData->getVelocity();
195 simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
197 SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
200 return userData->getBVHNode();
202 void addBoundingVolume(osg::Node& node)
204 simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
208 // Get that part of the local bv tree that intersects our sphere
210 mSubTreeCollector.setSphere(SGSphered(_center, _radius));
211 bvNode->accept(mSubTreeCollector);
214 bool testBoundingSphere(const osg::BoundingSphere& bound) const
219 double maxDist = bound._radius + _radius;
220 return distSqr(SGVec3d(bound._center), _center) <= maxDist*maxDist;
223 SGSharedPtr<simgear::BVHNode> getBVHNode() const
224 { return mSubTreeCollector.getNode(); }
233 simgear::BVHSubTreeCollector mSubTreeCollector;
236 FGGroundCache::FGGroundCache() :
242 reference_wgs84_point(SGVec3d(0, 0, 0)),
243 reference_vehicle_radius(0),
249 FGGroundCache::~FGGroundCache()
254 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
258 found_ground = false;
260 SGGeod geodPt = SGGeod::fromCart(pt);
261 // Don't blow away the cache ground_radius and stuff if there's no
263 if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
264 geodPt.getLongitudeDeg(),
269 // If we have an active wire, get some more area into the groundcache
271 rad = SGMiscd::max(200, rad);
273 // Store the parameters we used to build up that cache.
274 reference_wgs84_point = pt;
275 reference_vehicle_radius = rad;
276 // Store the time reference used to compute movements of moving triangles.
277 cache_ref_time = ref_time;
279 // Get a normalized down vector valid for the whole cache
280 SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
281 down = hlToEc.rotate(SGVec3d(0, 0, 1));
283 // Get the ground cache, that is a local collision tree of the environment
284 double endTime = cache_ref_time + 1; //FIXME??
285 CacheFill subtreeCollector(pt, rad, cache_ref_time, endTime);
286 globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
287 _localBvhTree = subtreeCollector.getBVHNode();
289 // Try to get a croase altitude value for the ground cache
290 SGLineSegmentd line(pt, pt + 1e4*down);
291 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time);
293 _localBvhTree->accept(lineSegmentVisitor);
295 // If this is successful, store this altitude for croase altitude values
296 if (!lineSegmentVisitor.empty()) {
297 SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
298 _altitude = geodPt.getElevationM();
299 _material = lineSegmentVisitor.getMaterial();
300 _type = materialToGroundType(_material);
303 // Else do a crude scene query for the current point
304 found_ground = globals->get_scenery()->
305 get_cart_elevation_m(pt, rad, _altitude, &_material);
306 _type = materialToGroundType(_material);
309 // Still not sucessful??
311 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
312 "cache without any scenery below the aircraft");
318 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
320 pt = reference_wgs84_point;
321 rad = reference_vehicle_radius;
322 ref_time = cache_ref_time;
326 class FGGroundCache::CatapultFinder : public BVHVisitor {
328 CatapultFinder(const SGSphered& sphere, const double& t) :
331 _haveLineSegment(false)
334 virtual void apply(BVHGroup& leaf)
336 if (!intersects(_sphere, leaf.getBoundingSphere()))
338 leaf.traverse(*this);
340 virtual void apply(BVHTransform& transform)
342 if (!intersects(_sphere, transform.getBoundingSphere()))
345 SGSphered sphere = _sphere;
346 _sphere = transform.sphereToLocal(sphere);
347 bool haveLineSegment = _haveLineSegment;
348 _haveLineSegment = false;
350 transform.traverse(*this);
352 if (_haveLineSegment) {
353 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
354 _linearVelocity = transform.vecToWorld(_linearVelocity);
355 _angularVelocity = transform.vecToWorld(_angularVelocity);
357 _haveLineSegment |= haveLineSegment;
358 _sphere.setCenter(sphere.getCenter());
360 virtual void apply(BVHMotionTransform& transform)
362 if (!intersects(_sphere, transform.getBoundingSphere()))
365 SGSphered sphere = _sphere;
366 _sphere = transform.sphereToLocal(sphere, _time);
367 bool haveLineSegment = _haveLineSegment;
368 _haveLineSegment = false;
370 transform.traverse(*this);
372 if (_haveLineSegment) {
373 SGMatrixd toWorld = transform.getToWorldTransform(_time);
375 += transform.getLinearVelocityAt(_lineSegment.getStart());
376 _angularVelocity += transform.getAngularVelocity();
377 _linearVelocity = toWorld.xformVec(_linearVelocity);
378 _angularVelocity = toWorld.xformVec(_angularVelocity);
379 _lineSegment = _lineSegment.transform(toWorld);
381 _haveLineSegment |= haveLineSegment;
382 _sphere.setCenter(sphere.getCenter());
384 virtual void apply(BVHLineGeometry& node)
386 if (node.getType() != BVHLineGeometry::CarrierCatapult)
389 SGLineSegmentd lineSegment(node.getLineSegment());
390 if (!intersects(_sphere, lineSegment))
393 _lineSegment = lineSegment;
394 double dist = distSqr(lineSegment, getSphere().getCenter());
395 _sphere.setRadius(sqrt(dist));
396 _linearVelocity = SGVec3d::zeros();
397 _angularVelocity = SGVec3d::zeros();
398 _haveLineSegment = true;
400 virtual void apply(BVHStaticGeometry& node)
403 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
404 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
406 void setSphere(const SGSphered& sphere)
407 { _sphere = sphere; }
408 const SGSphered& getSphere() const
411 const SGLineSegmentd& getLineSegment() const
412 { return _lineSegment; }
413 const SGVec3d& getLinearVelocity() const
414 { return _linearVelocity; }
415 const SGVec3d& getAngularVelocity() const
416 { return _angularVelocity; }
418 bool getHaveLineSegment() const
419 { return _haveLineSegment; }
422 SGLineSegmentd _lineSegment;
423 SGVec3d _linearVelocity;
424 SGVec3d _angularVelocity;
426 bool _haveLineSegment;
433 FGGroundCache::get_cat(double t, const SGVec3d& pt,
434 SGVec3d end[2], SGVec3d vel[2])
436 double maxDistance = 1000;
438 // Get the wire in question
439 CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
441 _localBvhTree->accept(catapultFinder);
443 if (!catapultFinder.getHaveLineSegment())
446 // prepare the returns
447 end[0] = catapultFinder.getLineSegment().getStart();
448 end[1] = catapultFinder.getLineSegment().getEnd();
450 // The linear velocity is the one at the start of the line segment ...
451 vel[0] = catapultFinder.getLinearVelocity();
452 // ... so the end point has the additional cross product.
453 vel[1] = catapultFinder.getLinearVelocity();
454 vel[1] += cross(catapultFinder.getAngularVelocity(),
455 catapultFinder.getLineSegment().getDirection());
457 // Return the distance to the cat
458 return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
462 FGGroundCache::get_agl(double t, const SGVec3d& pt, double max_altoff,
463 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
464 int *type, const SGMaterial** material, double *agl)
466 // Just set up a ground intersection query for the given point
467 SGLineSegmentd line(pt - max_altoff*down, pt + 1e4*down);
468 simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
470 _localBvhTree->accept(lineSegmentVisitor);
472 if (!lineSegmentVisitor.empty()) {
473 // Have an intersection
474 contact = lineSegmentVisitor.getPoint();
475 normal = lineSegmentVisitor.getNormal();
476 if (0 < dot(normal, down))
478 *agl = dot(down, contact - pt);
479 vel = lineSegmentVisitor.getLinearVelocity();
480 // correct the linear velocity, since the line intersector delivers
481 // values for the start point and the get_agl function should
482 // traditionally deliver for the contact point
483 vel += cross(lineSegmentVisitor.getAngularVelocity(),
484 contact - line.getStart());
485 *type = materialToGroundType(lineSegmentVisitor.getMaterial());
487 *material = lineSegmentVisitor.getMaterial();
491 // Whenever we did not have a ground triangle for the requested point,
492 // take the ground level we found during the current cache build.
493 // This is as good as what we had before for agl.
494 SGGeod geodPt = SGGeod::fromCart(pt);
495 *agl = geodPt.getElevationM() - _altitude;
496 geodPt.setElevationM(_altitude);
497 contact = SGVec3d::fromGeod(geodPt);
499 vel = SGVec3d(0, 0, 0);
502 *material = _material;
508 class FGGroundCache::WireIntersector : public BVHVisitor {
510 WireIntersector(const SGVec3d pt[4], const double& t) :
511 _linearVelocity(SGVec3d::zeros()),
512 _angularVelocity(SGVec3d::zeros()),
516 // Build the two triangles spanning the area where the hook has moved
517 // during the past step.
518 _triangles[0].set(pt[0], pt[1], pt[2]);
519 _triangles[1].set(pt[0], pt[2], pt[3]);
522 virtual void apply(BVHGroup& leaf)
524 if (!_intersects(leaf.getBoundingSphere()))
527 leaf.traverse(*this);
529 virtual void apply(BVHTransform& transform)
531 if (!_intersects(transform.getBoundingSphere()))
534 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
535 _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
536 _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
538 transform.traverse(*this);
541 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
542 _linearVelocity = transform.vecToWorld(_linearVelocity);
543 _angularVelocity = transform.vecToWorld(_angularVelocity);
545 _triangles[0] = triangles[0];
546 _triangles[1] = triangles[1];
548 virtual void apply(BVHMotionTransform& transform)
550 if (!_intersects(transform.getBoundingSphere()))
553 SGMatrixd toLocal = transform.getToLocalTransform(_time);
555 SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
556 _triangles[0] = triangles[0].transform(toLocal);
557 _triangles[1] = triangles[1].transform(toLocal);
559 transform.traverse(*this);
562 SGMatrixd toWorld = transform.getToWorldTransform(_time);
564 += transform.getLinearVelocityAt(_lineSegment.getStart());
565 _angularVelocity += transform.getAngularVelocity();
566 _linearVelocity = toWorld.xformVec(_linearVelocity);
567 _angularVelocity = toWorld.xformVec(_angularVelocity);
568 _lineSegment = _lineSegment.transform(toWorld);
570 _triangles[0] = triangles[0];
571 _triangles[1] = triangles[1];
573 virtual void apply(BVHLineGeometry& node)
575 if (node.getType() != BVHLineGeometry::CarrierWire)
577 SGLineSegmentd lineSegment(node.getLineSegment());
578 if (!_intersects(lineSegment))
581 _lineSegment = lineSegment;
582 _linearVelocity = SGVec3d::zeros();
583 _angularVelocity = SGVec3d::zeros();
586 virtual void apply(BVHStaticGeometry& node)
589 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
590 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
592 bool _intersects(const SGSphered& sphere) const
596 if (intersects(_triangles[0], sphere))
598 if (intersects(_triangles[1], sphere))
602 bool _intersects(const SGLineSegmentd& lineSegment) const
606 if (intersects(_triangles[0], lineSegment))
608 if (intersects(_triangles[1], lineSegment))
613 const SGLineSegmentd& getLineSegment() const
614 { return _lineSegment; }
615 const SGVec3d& getLinearVelocity() const
616 { return _linearVelocity; }
617 const SGVec3d& getAngularVelocity() const
618 { return _angularVelocity; }
620 const BVHLineGeometry* getWire() const
624 SGLineSegmentd _lineSegment;
625 SGVec3d _linearVelocity;
626 SGVec3d _angularVelocity;
627 const BVHLineGeometry* _wire;
629 SGTriangled _triangles[2];
633 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
635 // Get the wire in question
636 WireIntersector wireIntersector(pt, t);
638 _localBvhTree->accept(wireIntersector);
640 _wire = wireIntersector.getWire();
644 class FGGroundCache::WireFinder : public BVHVisitor {
646 WireFinder(const BVHLineGeometry* wire, const double& t) :
649 _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
650 _linearVelocity(SGVec3d::zeros()),
651 _angularVelocity(SGVec3d::zeros()),
652 _haveLineSegment(false)
655 virtual void apply(BVHGroup& leaf)
657 if (_haveLineSegment)
659 leaf.traverse(*this);
661 virtual void apply(BVHTransform& transform)
663 if (_haveLineSegment)
666 transform.traverse(*this);
668 if (_haveLineSegment) {
669 _linearVelocity = transform.vecToWorld(_linearVelocity);
670 _angularVelocity = transform.vecToWorld(_angularVelocity);
671 _lineSegment = transform.lineSegmentToWorld(_lineSegment);
674 virtual void apply(BVHMotionTransform& transform)
676 if (_haveLineSegment)
679 transform.traverse(*this);
681 if (_haveLineSegment) {
682 SGMatrixd toWorld = transform.getToWorldTransform(_time);
684 += transform.getLinearVelocityAt(_lineSegment.getStart());
685 _angularVelocity += transform.getAngularVelocity();
686 _linearVelocity = toWorld.xformVec(_linearVelocity);
687 _angularVelocity = toWorld.xformVec(_angularVelocity);
688 _lineSegment = _lineSegment.transform(toWorld);
691 virtual void apply(BVHLineGeometry& node)
693 if (_haveLineSegment)
697 if (node.getType() != BVHLineGeometry::CarrierWire)
699 _lineSegment = SGLineSegmentd(node.getLineSegment());
700 _linearVelocity = SGVec3d::zeros();
701 _angularVelocity = SGVec3d::zeros();
702 _haveLineSegment = true;
704 virtual void apply(BVHStaticGeometry&) { }
706 virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
707 virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
709 const SGLineSegmentd& getLineSegment() const
710 { return _lineSegment; }
712 bool getHaveLineSegment() const
713 { return _haveLineSegment; }
715 const SGVec3d& getLinearVelocity() const
716 { return _linearVelocity; }
717 const SGVec3d& getAngularVelocity() const
718 { return _angularVelocity; }
721 const BVHLineGeometry* _wire;
724 SGLineSegmentd _lineSegment;
725 SGVec3d _linearVelocity;
726 SGVec3d _angularVelocity;
728 bool _haveLineSegment;
731 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
733 // Fast return if we do not have an active wire.
737 // Get the wire in question
738 WireFinder wireFinder(_wire, t);
740 _localBvhTree->accept(wireFinder);
742 if (!wireFinder.getHaveLineSegment())
745 // prepare the returns
746 end[0] = wireFinder.getLineSegment().getStart();
747 end[1] = wireFinder.getLineSegment().getEnd();
749 // The linear velocity is the one at the start of the line segment ...
750 vel[0] = wireFinder.getLinearVelocity();
751 // ... so the end point has the additional cross product.
752 vel[1] = wireFinder.getLinearVelocity();
753 vel[1] += cross(wireFinder.getAngularVelocity(),
754 wireFinder.getLineSegment().getDirection());
759 void FGGroundCache::release_wire(void)