# include "config.h"
#endif
+#include "groundcache.hxx"
+
#include <utility>
#include <osg/Drawable>
#include <simgear/sg_inlines.h>
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
-#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/SGMisc.hxx>
+#include <simgear/scene/material/mat.hxx>
#include <simgear/scene/util/SGNodeMasks.hxx>
#include <simgear/scene/util/SGSceneUserData.hxx>
-
-#include <simgear/scene/bvh/BVHNode.hxx>
-#include <simgear/scene/bvh/BVHGroup.hxx>
-#include <simgear/scene/bvh/BVHTransform.hxx>
-#include <simgear/scene/bvh/BVHMotionTransform.hxx>
-#include <simgear/scene/bvh/BVHLineGeometry.hxx>
-#include <simgear/scene/bvh/BVHStaticGeometry.hxx>
-#include <simgear/scene/bvh/BVHStaticData.hxx>
-#include <simgear/scene/bvh/BVHStaticNode.hxx>
-#include <simgear/scene/bvh/BVHStaticTriangle.hxx>
-#include <simgear/scene/bvh/BVHStaticBinary.hxx>
-#include <simgear/scene/bvh/BVHSubTreeCollector.hxx>
-#include <simgear/scene/bvh/BVHLineSegmentVisitor.hxx>
-#include <simgear/scene/bvh/BVHNearestPointVisitor.hxx>
+#include <simgear/scene/util/OsgMath.hxx>
+
+#include <simgear/bvh/BVHNode.hxx>
+#include <simgear/bvh/BVHGroup.hxx>
+#include <simgear/bvh/BVHTransform.hxx>
+#include <simgear/bvh/BVHMotionTransform.hxx>
+#include <simgear/bvh/BVHLineGeometry.hxx>
+#include <simgear/bvh/BVHStaticGeometry.hxx>
+#include <simgear/bvh/BVHStaticData.hxx>
+#include <simgear/bvh/BVHStaticNode.hxx>
+#include <simgear/bvh/BVHStaticTriangle.hxx>
+#include <simgear/bvh/BVHStaticBinary.hxx>
+#include <simgear/bvh/BVHSubTreeCollector.hxx>
+#include <simgear/bvh/BVHLineSegmentVisitor.hxx>
+#include <simgear/bvh/BVHNearestPointVisitor.hxx>
+
+#ifdef GROUNDCACHE_DEBUG
+#include <simgear/scene/model/BVHDebugCollectVisitor.hxx>
+#include <Main/fg_props.hxx>
+#endif
#include <Main/globals.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include "flight.hxx"
-#include "groundcache.hxx"
using namespace simgear;
class FGGroundCache::CacheFill : public osg::NodeVisitor {
public:
- CacheFill(const SGVec3d& center, const double& radius,
+ CacheFill(const SGVec3d& center, const SGVec3d& down, const double& radius,
const double& startTime, const double& endTime) :
osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
_center(center),
+ _down(down),
_radius(radius),
_startTime(startTime),
- _endTime(endTime)
+ _endTime(endTime),
+ _sceneryHit(0, 0, 0),
+ _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
+ _material(0),
+ _haveHit(false)
{
setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
}
const SGSceneUserData::Velocity* velocity = getVelocity(transform);
SGVec3d center = _center;
- _center = SGVec3d(inverseMatrix.preMult(_center.osg()));
+ SGVec3d down = _down;
double radius = _radius;
- if (velocity)
- _radius += (_endTime - _startTime)*norm(velocity->linear);
+ bool haveHit = _haveHit;
+ const simgear::BVHMaterial* material = _material;
+
+ _haveHit = false;
+ _center = toSG(inverseMatrix.preMult(toOsg(_center)));
+ _down = toSG(osg::Matrix::transform3x3(toOsg(_down), inverseMatrix));
+ if (velocity) {
+ SGVec3d staticCenter(_center);
+
+ double dtStart = velocity->referenceTime - _startTime;
+ SGVec3d startCenter = staticCenter + dtStart*velocity->linear;
+ SGQuatd startOr(SGQuatd::fromAngleAxis(dtStart*velocity->angular));
+ startCenter = startOr.transform(startCenter);
+
+ double dtEnd = velocity->referenceTime - _endTime;
+ SGVec3d endCenter = staticCenter + dtEnd*velocity->linear;
+ SGQuatd endOr(SGQuatd::fromAngleAxis(dtEnd*velocity->angular));
+ endCenter = endOr.transform(endCenter);
+
+ _center = 0.5*(startCenter + endCenter);
+ _down = startOr.transform(_down);
+ _radius += 0.5*dist(startCenter, endCenter);
+ }
simgear::BVHSubTreeCollector::NodeList parentNodeList;
mSubTreeCollector.pushNodeList(parentNodeList);
bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
bvhTransform->setLinearVelocity(velocity->linear);
bvhTransform->setAngularVelocity(velocity->angular);
- bvhTransform->setReferenceTime(_startTime);
+ bvhTransform->setReferenceTime(velocity->referenceTime);
+ bvhTransform->setStartTime(_startTime);
bvhTransform->setEndTime(_endTime);
bvhTransform->setId(velocity->id);
} else {
mSubTreeCollector.popNodeList(parentNodeList);
}
+
+ if (_haveHit) {
+ if (velocity) {
+ double dt = _startTime - velocity->referenceTime;
+ SGQuatd ori(SGQuatd::fromAngleAxis(dt*velocity->angular));
+ _sceneryHit = ori.transform(_sceneryHit);
+ _sceneryHit += dt*velocity->linear;
+ }
+ _sceneryHit = toSG(matrix.preMult(toOsg(_sceneryHit)));
+ } else {
+ _material = material;
+ _haveHit = haveHit;
+ }
+
_center = center;
+ _down = down;
_radius = radius;
}
if (!bvNode)
return;
+ // Find a croase ground intersection
+ SGLineSegmentd line(_center + _radius*_down, _center + _maxDown*_down);
+ simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, _startTime);
+ bvNode->accept(lineSegmentVisitor);
+ if (!lineSegmentVisitor.empty()) {
+ _sceneryHit = lineSegmentVisitor.getPoint();
+ _material = lineSegmentVisitor.getMaterial();
+ _maxDown = SGMiscd::max(_radius, dot(_down, _sceneryHit - _center));
+ _haveHit = true;
+ }
+
// Get that part of the local bv tree that intersects our sphere
// of interrest.
mSubTreeCollector.setSphere(SGSphered(_center, _radius));
if (!bound.valid())
return false;
+ SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
double maxDist = bound._radius + _radius;
- return distSqr(SGVec3d(bound._center), _center) <= maxDist*maxDist;
+ SGVec3d boundCenter(toVec3d(toSG(bound._center)));
+ return distSqr(downSeg, boundCenter) <= maxDist*maxDist;
}
SGSharedPtr<simgear::BVHNode> getBVHNode() const
{ return mSubTreeCollector.getNode(); }
+
+ bool getHaveElevationBelowCache() const
+ { return _haveHit; }
+ double getElevationBelowCache() const
+ { return SGGeod::fromCart(_sceneryHit).getElevationM(); }
+ const simgear::BVHMaterial* getMaterialBelowCache() const
+ { return _material; }
private:
-
SGVec3d _center;
+ SGVec3d _down;
double _radius;
double _startTime;
double _endTime;
simgear::BVHSubTreeCollector mSubTreeCollector;
+ SGVec3d _sceneryHit;
+ double _maxDown;
+ const simgear::BVHMaterial* _material;
+ bool _haveHit;
};
FGGroundCache::FGGroundCache() :
_altitude(0),
_material(0),
cache_ref_time(0),
+ cache_time_offset(0),
_wire(0),
reference_wgs84_point(SGVec3d(0, 0, 0)),
reference_vehicle_radius(0),
down(0.0, 0.0, 0.0),
found_ground(false)
{
+#ifdef GROUNDCACHE_DEBUG
+ _lookupTime = SGTimeStamp::fromSec(0.0);
+ _lookupCount = 0;
+ _buildTime = SGTimeStamp::fromSec(0.0);
+ _buildCount = 0;
+#endif
}
FGGroundCache::~FGGroundCache()
}
bool
-FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
- double rad)
+FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
+ const SGVec3d& pt, double rad)
{
+#ifdef GROUNDCACHE_DEBUG
+ SGTimeStamp t0 = SGTimeStamp::now();
+#endif
+
// Empty cache.
found_ground = false;
SGGeod geodPt = SGGeod::fromCart(pt);
// Don't blow away the cache ground_radius and stuff if there's no
// scenery
- if (!globals->get_tile_mgr()->scenery_available(geodPt, rad)) {
+ if (!globals->get_tile_mgr()->schedule_scenery(geodPt, rad, 1.0)) {
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): scenery_available "
"returns false at " << geodPt << " " << pt << " " << rad);
return false;
reference_wgs84_point = pt;
reference_vehicle_radius = rad;
// Store the time reference used to compute movements of moving triangles.
- cache_ref_time = ref_time;
+ cache_ref_time = startSimTime;
// Get a normalized down vector valid for the whole cache
SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
down = hlToEc.rotate(SGVec3d(0, 0, 1));
// Get the ground cache, that is a local collision tree of the environment
- double endTime = cache_ref_time + 1; //FIXME??
- CacheFill subtreeCollector(pt, rad, cache_ref_time, endTime);
+ startSimTime += cache_time_offset;
+ endSimTime += cache_time_offset;
+ CacheFill subtreeCollector(pt, down, rad, startSimTime, endSimTime);
globals->get_scenery()->get_scene_graph()->accept(subtreeCollector);
_localBvhTree = subtreeCollector.getBVHNode();
- // Try to get a croase altitude value for the ground cache
- SGLineSegmentd line(pt, pt + 2*reference_vehicle_radius*down);
- simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time);
- if (_localBvhTree)
+ if (subtreeCollector.getHaveElevationBelowCache()) {
+ // Use the altitude value below the cache that we gathered during
+ // cache collection
+ _altitude = subtreeCollector.getElevationBelowCache();
+ _material = subtreeCollector.getMaterialBelowCache();
+ found_ground = true;
+ } else if (_localBvhTree) {
+ // We have nothing below us, so try starting with the lowest point
+ // upwards for a croase altitude value
+ SGLineSegmentd line(pt + reference_vehicle_radius*down, pt - 1e3*down);
+ simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, startSimTime);
_localBvhTree->accept(lineSegmentVisitor);
- // If this is successful, store this altitude for croase altitude values
- if (!lineSegmentVisitor.empty()) {
- SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
- _altitude = geodPt.getElevationM();
- _material = lineSegmentVisitor.getMaterial();
- found_ground = true;
- } else {
- // Else do a crude scene query for the current point
+ if (!lineSegmentVisitor.empty()) {
+ SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
+ _altitude = geodPt.getElevationM();
+ _material = lineSegmentVisitor.getMaterial();
+ found_ground = true;
+ }
+ }
+
+ if (!found_ground) {
+ // Ok, still nothing here?? Last resort ...
double alt = 0;
+ _material = 0;
found_ground = globals->get_scenery()->
- get_cart_elevation_m(pt, rad, alt, &_material);
+ get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
if (found_ground)
_altitude = alt;
}
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build "
"cache without any scenery below the aircraft");
+#ifdef GROUNDCACHE_DEBUG
+ t0 = SGTimeStamp::now() - t0;
+ _buildTime += t0;
+ _buildCount++;
+
+ if (_buildCount > 60) {
+ double buildTime = 0;
+ if (_buildCount)
+ buildTime = _buildTime.toSecs()/_buildCount;
+ double lookupTime = 0;
+ if (_lookupCount)
+ lookupTime = _lookupTime.toSecs()/_lookupCount;
+ _buildTime = SGTimeStamp::fromSec(0.0);
+ _buildCount = 0;
+ _lookupTime = SGTimeStamp::fromSec(0.0);
+ _lookupCount = 0;
+ SG_LOG(SG_FLIGHT, SG_ALERT, "build time = " << buildTime
+ << ", lookup Time = " << lookupTime);
+ }
+
+ if (!_group.valid()) {
+ _group = new osg::Group;
+ globals->get_scenery()->get_scene_graph()->addChild(_group);
+ fgSetInt("/fdm/groundcache-debug-level", -3);
+ }
+ _group->removeChildren(0, _group->getNumChildren());
+ if (_localBvhTree) {
+ int level = fgGetInt("/fdm/groundcache-debug-level");
+ if (-2 <= level) {
+ simgear::BVHDebugCollectVisitor debug(endSimTime, level);
+ _localBvhTree->accept(debug);
+ _group->addChild(debug.getNode());
+ }
+ }
+#endif
+
return found_ground;
}
return;
leaf.traverse(*this);
}
+ virtual void apply(BVHPageNode& leaf)
+ {
+ if (_foundId)
+ return;
+ leaf.traverse(*this);
+ }
virtual void apply(BVHTransform& transform)
{
if (_foundId)
if (_id == transform.getId()) {
_foundId = true;
- return;
+ } else {
+ transform.traverse(*this);
}
- transform.traverse(*this);
-
if (_foundId) {
SGMatrixd toWorld = transform.getToWorldTransform(_time);
SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
// Get the transform matrix and velocities of a moving body with id at t.
if (!_localBvhTree)
return false;
+ t += cache_time_offset;
BodyFinder bodyFinder(id, t);
_localBvhTree->accept(bodyFinder);
if (bodyFinder.empty())
class FGGroundCache::CatapultFinder : public BVHVisitor {
public:
CatapultFinder(const SGSphered& sphere, const double& t) :
+ _haveLineSegment(false),
_sphere(sphere),
- _time(t),
- _haveLineSegment(false)
+ _time(t)
{ }
virtual void apply(BVHGroup& leaf)
return;
leaf.traverse(*this);
}
+ virtual void apply(BVHPageNode& leaf)
+ {
+ if (!intersects(_sphere, leaf.getBoundingSphere()))
+ return;
+ leaf.traverse(*this);
+ }
virtual void apply(BVHTransform& transform)
{
if (!intersects(_sphere, transform.getBoundingSphere()))
double maxDistance = 1000;
// Get the wire in question
+ t += cache_time_offset;
CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
if (_localBvhTree)
_localBvhTree->accept(catapultFinder);
bool
FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
- simgear::BVHNode::Id& id, const SGMaterial*& material)
+ simgear::BVHNode::Id& id, const simgear::BVHMaterial*& material)
{
+#ifdef GROUNDCACHE_DEBUG
+ SGTimeStamp t0 = SGTimeStamp::now();
+#endif
+
// Just set up a ground intersection query for the given point
SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
+ t += cache_time_offset;
simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
if (_localBvhTree)
_localBvhTree->accept(lineSegmentVisitor);
+#ifdef GROUNDCACHE_DEBUG
+ t0 = SGTimeStamp::now() - t0;
+ _lookupTime += t0;
+ _lookupCount++;
+#endif
+
if (!lineSegmentVisitor.empty()) {
// Have an intersection
contact = lineSegmentVisitor.getPoint();
FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
SGVec3d& contact, SGVec3d& linearVel,
SGVec3d& angularVel, simgear::BVHNode::Id& id,
- const SGMaterial*& material)
+ const simgear::BVHMaterial*& material)
{
if (!_localBvhTree)
return false;
+#ifdef GROUNDCACHE_DEBUG
+ SGTimeStamp t0 = SGTimeStamp::now();
+#endif
+
// Just set up a ground intersection query for the given point
SGSphered sphere(pt, maxDist);
+ t += cache_time_offset;
simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
_localBvhTree->accept(nearestPointVisitor);
+#ifdef GROUNDCACHE_DEBUG
+ t0 = SGTimeStamp::now() - t0;
+ _lookupTime += t0;
+ _lookupCount++;
+#endif
+
if (nearestPointVisitor.empty())
return false;
leaf.traverse(*this);
}
+ virtual void apply(BVHPageNode& leaf)
+ {
+ if (!_intersects(leaf.getBoundingSphere()))
+ return;
+
+ leaf.traverse(*this);
+ }
virtual void apply(BVHTransform& transform)
{
if (!_intersects(transform.getBoundingSphere()))
bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
{
// Get the wire in question
+ t += cache_time_offset;
WireIntersector wireIntersector(pt, t);
if (_localBvhTree)
_localBvhTree->accept(wireIntersector);
_wire = wireIntersector.getWire();
- return _wire;
+ return (_wire != NULL);
}
class FGGroundCache::WireFinder : public BVHVisitor {
return;
leaf.traverse(*this);
}
+ virtual void apply(BVHPageNode& leaf)
+ {
+ if (_haveLineSegment)
+ return;
+ leaf.traverse(*this);
+ }
virtual void apply(BVHTransform& transform)
{
if (_haveLineSegment)
return false;
// Get the wire in question
+ t += cache_time_offset;
WireFinder wireFinder(_wire, t);
if (_localBvhTree)
_localBvhTree->accept(wireFinder);
projects / flightgear.git / blobdiff