//
// Written by Mathias Froehlich, started Nov 2004.
//
-// Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de
+// Copyright (C) 2004, 2009 Mathias Froehlich - Mathias.Froehlich@web.de
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
-#include <float.h>
+#include "groundcache.hxx"
+
+#include <utility>
-#include <plib/sg.h>
+#include <osg/Drawable>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/Camera>
+#include <osg/Transform>
+#include <osg/MatrixTransform>
+#include <osg/PositionAttitudeTransform>
+#include <osg/CameraView>
#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/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>
+
+#ifdef GROUNDCACHE_DEBUG
+#include <simgear/scene/bvh/BVHDebugCollectVisitor.hxx>
+#include <Main/fg_props.hxx>
+#endif
#include <Main/globals.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
-#include <AIModel/AICarrier.hxx>
#include "flight.hxx"
-#include "groundcache.hxx"
-// Specialized version of sgMultMat4 needed because of mixed matrix
-// types
-static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
- for ( int j = 0 ; j < 4 ; j++ ) {
- dst[0][j] = m2[0][0] * m1[0][j] +
- m2[0][1] * m1[1][j] +
- m2[0][2] * m1[2][j] +
- m2[0][3] * m1[3][j] ;
-
- dst[1][j] = m2[1][0] * m1[0][j] +
- m2[1][1] * m1[1][j] +
- m2[1][2] * m1[2][j] +
- m2[1][3] * m1[3][j] ;
-
- dst[2][j] = m2[2][0] * m1[0][j] +
- m2[2][1] * m1[1][j] +
- m2[2][2] * m1[2][j] +
- m2[2][3] * m1[3][j] ;
-
- dst[3][j] = m2[3][0] * m1[0][j] +
- m2[3][1] * m1[1][j] +
- m2[3][2] * m1[2][j] +
- m2[3][3] * m1[3][j] ;
+using namespace simgear;
+
+class FGGroundCache::CacheFill : public osg::NodeVisitor {
+public:
+ 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),
+ _sceneryHit(0, 0, 0),
+ _maxDown(SGGeod::fromCart(center).getElevationM() + 9999),
+ _material(0),
+ _haveHit(false)
+ {
+ setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
}
-}
-
-static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
-{
- sgdVec3 dif;
-
- // Some tolerance in meters we accept a point to be outside of the triangle
- // and still return that it is inside.
- SGDfloat eps = 1e-2;
- SGDfloat min, max;
- // punt if outside bouding cube
- SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
- if( (point[0] < min - eps) || (point[0] > max + eps) )
- return false;
- dif[0] = max - min;
+ virtual void apply(osg::Node& node)
+ {
+ if (!testBoundingSphere(node.getBound()))
+ return;
- SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
- if( (point[1] < min - eps) || (point[1] > max + eps) )
- return false;
- dif[1] = max - min;
+ addBoundingVolume(node);
+ }
+
+ virtual void apply(osg::Group& group)
+ {
+ if (!testBoundingSphere(group.getBound()))
+ return;
- SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
- if( (point[2] < min - eps) || (point[2] > max + eps) )
- return false;
- dif[2] = max - min;
-
- // drop the smallest dimension so we only have to work in 2d.
- SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
- SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
- if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
- // x is the smallest dimension
- x1 = point[1];
- y1 = point[2];
- x2 = tri[0][1];
- y2 = tri[0][2];
- x3 = tri[1][1];
- y3 = tri[1][2];
- rx = tri[2][1];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
- // y is the smallest dimension
- x1 = point[0];
- y1 = point[2];
- x2 = tri[0][0];
- y2 = tri[0][2];
- x3 = tri[1][0];
- y3 = tri[1][2];
- rx = tri[2][0];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
- // z is the smallest dimension
- x1 = point[0];
- y1 = point[1];
- x2 = tri[0][0];
- y2 = tri[0][1];
- x3 = tri[1][0];
- y3 = tri[1][1];
- rx = tri[2][0];
- ry = tri[2][1];
- } else {
- // all dimensions are really small so lets call it close
- // enough and return a successful match
- return true;
+ simgear::BVHSubTreeCollector::NodeList parentNodeList;
+ mSubTreeCollector.pushNodeList(parentNodeList);
+
+ traverse(group);
+ addBoundingVolume(group);
+
+ mSubTreeCollector.popNodeList(parentNodeList);
}
+
+ virtual void apply(osg::Transform& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::Camera& camera)
+ {
+ if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER)
+ return;
+ handleTransform(camera);
+ }
+ virtual void apply(osg::CameraView& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::MatrixTransform& transform)
+ { handleTransform(transform); }
+ virtual void apply(osg::PositionAttitudeTransform& transform)
+ { handleTransform(transform); }
+
+ void handleTransform(osg::Transform& transform)
+ {
+ // Hmm, may be this needs to be refined somehow ...
+ if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF)
+ return;
+
+ if (!testBoundingSphere(transform.getBound()))
+ return;
+
+ osg::Matrix inverseMatrix;
+ if (!transform.computeWorldToLocalMatrix(inverseMatrix, this))
+ return;
+ osg::Matrix matrix;
+ if (!transform.computeLocalToWorldMatrix(matrix, this))
+ return;
+
+ // Look for a velocity note
+ const SGSceneUserData::Velocity* velocity = getVelocity(transform);
+
+ SGVec3d center = _center;
+ SGVec3d down = _down;
+ double radius = _radius;
+ bool haveHit = _haveHit;
+ const SGMaterial* 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);
+
+ addBoundingVolume(transform);
+ traverse(transform);
+
+ if (mSubTreeCollector.haveChildren()) {
+ if (velocity) {
+ simgear::BVHMotionTransform* bvhTransform;
+ bvhTransform = new simgear::BVHMotionTransform;
+ bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
+ bvhTransform->setLinearVelocity(velocity->linear);
+ bvhTransform->setAngularVelocity(velocity->angular);
+ bvhTransform->setReferenceTime(velocity->referenceTime);
+ bvhTransform->setStartTime(_startTime);
+ bvhTransform->setEndTime(_endTime);
+ bvhTransform->setId(velocity->id);
+
+ mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
+ } else {
+ simgear::BVHTransform* bvhTransform;
+ bvhTransform = new simgear::BVHTransform;
+ bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr()));
+
+ mSubTreeCollector.popNodeList(parentNodeList, bvhTransform);
+ }
+ } else {
+ mSubTreeCollector.popNodeList(parentNodeList);
+ }
- // check if intersection point is on the same side of p1 <-> p2 as p3
- SGDfloat tmp = (y2 - y3);
- SGDfloat tmpn = (x2 - x3);
- int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
- int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 1 check\n");
- return false;
+ 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;
}
- // check if intersection point is on correct side of p2 <-> p3 as p1
- tmp = (y3 - ry);
- tmpn = (x3 - rx);
- side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 2 check\n");
- return false;
+ const SGSceneUserData::Velocity* getVelocity(osg::Node& node)
+ {
+ SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
+ if (!userData)
+ return 0;
+ return userData->getVelocity();
+ }
+ simgear::BVHNode* getNodeBoundingVolume(osg::Node& node)
+ {
+ SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node);
+ if (!userData)
+ return 0;
+ return userData->getBVHNode();
}
+ void addBoundingVolume(osg::Node& node)
+ {
+ simgear::BVHNode* bvNode = getNodeBoundingVolume(node);
+ 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;
+ }
- // check if intersection point is on correct side of p1 <-> p3 as p2
- tmp = (y2 - ry);
- tmpn = (x2 - rx);
- side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 3 check\n");
- return false;
+ // Get that part of the local bv tree that intersects our sphere
+ // of interrest.
+ mSubTreeCollector.setSphere(SGSphered(_center, _radius));
+ bvNode->accept(mSubTreeCollector);
}
+
+ bool testBoundingSphere(const osg::BoundingSphere& bound) const
+ {
+ if (!bound.valid())
+ return false;
+
+ SGLineSegmentd downSeg(_center, _center + _maxDown*_down);
+ double maxDist = bound._radius + _radius;
+ 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 SGMaterial* getMaterialBelowCache() const
+ { return _material; }
+
+private:
+ SGVec3d _center;
+ SGVec3d _down;
+ double _radius;
+ double _startTime;
+ double _endTime;
+
+ simgear::BVHSubTreeCollector mSubTreeCollector;
+ SGVec3d _sceneryHit;
+ double _maxDown;
+ const SGMaterial* _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
+}
- return true;
+FGGroundCache::~FGGroundCache()
+{
}
-// Test if the line given by the point on the line pt_on_line and the
-// line direction dir intersects the sphere sp.
-// Adapted from plib.
-static inline bool
-fgdIsectSphereInfLine(const sgdSphere& sp,
- const sgdVec3 pt_on_line, const sgdVec3 dir)
+bool
+FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime,
+ const SGVec3d& pt, double rad)
{
- sgdVec3 r;
- sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
+#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()->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;
+ }
+ _material = 0;
- SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
-
- SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
- projectedDistance * projectedDistance;
+ // If we have an active wire, get some more area into the groundcache
+ if (_wire)
+ rad = SGMiscd::max(200, rad);
+
+ // Store the parameters we used to build up that cache.
+ reference_wgs84_point = pt;
+ reference_vehicle_radius = rad;
+ // Store the time reference used to compute movements of moving triangles.
+ 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
+ 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();
+
+ 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 (!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;
+ found_ground = globals->get_scenery()->
+ get_elevation_m(SGGeod::fromGeodM(geodPt, 10000), alt, &_material);
+ if (found_ground)
+ _altitude = alt;
+ }
+
+ // Still not sucessful??
+ if (!found_ground)
+ 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);
+ }
- SGDfloat radius = sp.getRadius();
- return dist < radius*radius;
-}
+ 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
-FGGroundCache::FGGroundCache()
-{
- sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
- ground_radius = 0.0;
- cache_ref_time = 0.0;
- wire_id = 0;
- sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
- reference_vehicle_radius = 0.0;
- found_ground = false;
+ return found_ground;
}
-FGGroundCache::~FGGroundCache()
+bool
+FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
{
+ pt = reference_wgs84_point;
+ rad = reference_vehicle_radius;
+ ref_time = cache_ref_time;
+ return found_ground;
}
-FGGroundCache::GroundProperty
-FGGroundCache::extractGroundProperty( ssgLeaf* l )
-{
- // FIXME: Do more ...
- // Idea: have a get_globals() function which knows about that stuff.
- // Or most propably read that from a configuration file,
- // from property tree or whatever ...
-
- // Get ground dependent data.
- GroundProperty gp;
- gp.wire_id = -1;
-
- FGAICarrierHardware *ud =
- dynamic_cast<FGAICarrierHardware*>(l->getUserData());
- if (ud) {
- switch (ud->type) {
- case FGAICarrierHardware::Wire:
- gp.type = FGInterface::Wire;
- gp.wire_id = ud->id;
- break;
- case FGAICarrierHardware::Catapult:
- gp.type = FGInterface::Catapult;
- break;
- default:
- gp.type = FGInterface::Solid;
- break;
- }
-
- // Copy the velocity from the carrier class.
- ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
- }
-
- else {
-
- // Initialize velocity field.
- sgdSetVec3( gp.vel, 0.0, 0.0, 0.0 );
- sgdSetVec3( gp.rot, 0.0, 0.0, 0.0 );
- sgdSetVec3( gp.pivot, 0.0, 0.0, 0.0 );
- }
-
- // Get the texture name and decide what ground type we have.
- ssgState *st = l->getState();
- if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) {
- ssgSimpleState *ss = (ssgSimpleState*)st;
- SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" );
- string file = fullPath.file();
- SGPath dirPath(fullPath.dir());
- string category = dirPath.file();
-
- if (category == "Runway")
- gp.type = FGInterface::Solid;
- else {
- if (file == "asphault.rgb" || file == "airport.rgb")
- gp.type = FGInterface::Solid;
- else if (file == "water.rgb" || file == "water-lake.rgb")
- gp.type = FGInterface::Water;
- else if (file == "forest.rgb" || file == "cropwood.rgb")
- gp.type = FGInterface::Forest;
- }
- }
-
- return gp;
-}
+class FGGroundCache::BodyFinder : public BVHVisitor {
+public:
+ BodyFinder(BVHNode::Id id, const double& t) :
+ _id(id),
+ _bodyToWorld(SGMatrixd::unit()),
+ _linearVelocity(0, 0, 0),
+ _angularVelocity(0, 0, 0),
+ _time(t)
+ { }
+
+ virtual void apply(BVHGroup& leaf)
+ {
+ if (_foundId)
+ return;
+ leaf.traverse(*this);
+ }
+ virtual void apply(BVHTransform& transform)
+ {
+ if (_foundId)
+ return;
+
+ transform.traverse(*this);
+
+ if (_foundId) {
+ _linearVelocity = transform.vecToWorld(_linearVelocity);
+ _angularVelocity = transform.vecToWorld(_angularVelocity);
+ _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
+ }
+ }
+ virtual void apply(BVHMotionTransform& transform)
+ {
+ if (_foundId)
+ return;
+
+ if (_id == transform.getId()) {
+ _foundId = true;
+ } else {
+ transform.traverse(*this);
+ }
+
+ if (_foundId) {
+ SGMatrixd toWorld = transform.getToWorldTransform(_time);
+ SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
+ _linearVelocity += transform.getLinearVelocityAt(referencePoint);
+ _angularVelocity += transform.getAngularVelocity();
+ _linearVelocity = toWorld.xformVec(_linearVelocity);
+ _angularVelocity = toWorld.xformVec(_angularVelocity);
+ _bodyToWorld = toWorld*_bodyToWorld;
+ }
+ }
+ virtual void apply(BVHLineGeometry& node) { }
+ virtual void apply(BVHStaticGeometry& node) { }
+
+ virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
+ virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
+
+ const SGMatrixd& getBodyToWorld() const
+ { return _bodyToWorld; }
+ const SGVec3d& getLinearVelocity() const
+ { return _linearVelocity; }
+ const SGVec3d& getAngularVelocity() const
+ { return _angularVelocity; }
+
+ bool empty() const
+ { return !_foundId; }
+
+protected:
+ simgear::BVHNode::Id _id;
-void
-FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
- ssgLeaf *l)
+ SGMatrixd _bodyToWorld;
+
+ SGVec3d _linearVelocity;
+ SGVec3d _angularVelocity;
+
+ bool _foundId;
+
+ double _time;
+};
+
+bool
+FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
+ SGVec3d& angularVel, simgear::BVHNode::Id id)
{
- GroundProperty gp = extractGroundProperty(l);
-
- // Lines must have special meanings.
- // Wires and catapults are done with lines.
- int nl = l->getNumLines();
- for (int i = 0; i < nl; ++i) {
- sgdSphere sphere;
- sphere.empty();
- sgdVec3 ends[2];
- short v[2];
- l->getLine(i, v, v+1 );
- for (int k=0; k<2; ++k) {
- sgdSetVec3(ends[k], l->getVertex(v[k]));
- sgdXformPnt3(ends[k], xform);
- sphere.extend(ends[k]);
- }
-
- if (wsp->intersects( &sphere )) {
- if (gp.type == FGInterface::Wire) {
- Wire wire;
- sgdCopyVec3(wire.ends[0], ends[0]);
- sgdCopyVec3(wire.ends[1], ends[1]);
- sgdCopyVec3(wire.velocity, gp.vel);
- sgdCopyVec3(wire.rotation, gp.rot);
- sgdSubVec3(wire.rotation_pivot, gp.pivot, cache_center);
- wire.wire_id = gp.wire_id;
-
- wires.push_back(wire);
- }
- if (gp.type == FGInterface::Catapult) {
- Catapult cat;
- sgdCopyVec3(cat.start, ends[0]);
- sgdCopyVec3(cat.end, ends[1]);
- sgdCopyVec3(cat.velocity, gp.vel);
- sgdCopyVec3(cat.rotation, gp.rot);
- sgdSubVec3(cat.rotation_pivot, gp.pivot, cache_center);
-
- catapults.push_back(cat);
- }
- }
- }
+ // 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())
+ return false;
+
+ bodyToWorld = bodyFinder.getBodyToWorld();
+ linearVel = bodyFinder.getLinearVelocity();
+ angularVel = bodyFinder.getAngularVelocity();
+
+ return true;
}
-void
-FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
- const sgdMat4 xform, bool sphIsec,
- sgdVec3 down, ssgLeaf *l)
-{
- GroundProperty gp = extractGroundProperty(l);
-
- int nt = l->getNumTriangles();
- for (int i = 0; i < nt; ++i) {
- Triangle t;
- t.sphere.empty();
- short v[3];
- l->getTriangle(i, &v[0], &v[1], &v[2]);
- for (int k = 0; k < 3; ++k) {
- sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
- sgdXformPnt3(t.vertices[k], xform);
- t.sphere.extend(t.vertices[k]);
- }
-
- sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
- SGDfloat dot = sgdScalarProductVec3(down, t.plane);
- if (dot > 0) {
- if (!l->getCullFace()) {
- // Surface points downwards, ignore for altitude computations.
- continue;
- } else
- sgdScaleVec4( t.plane, -1 );
- }
-
- // Check if the sphere around the vehicle intersects the sphere
- // around that triangle. If so, put that triangle into the cache.
- if (sphIsec && sp->intersects(&t.sphere)) {
- sgdCopyVec3(t.velocity, gp.vel);
- sgdCopyVec3(t.rotation, gp.rot);
- sgdSubVec3(t.rotation_pivot, gp.pivot, cache_center);
- t.type = gp.type;
- triangles.push_back(t);
- }
-
- // In case the cache is empty, we still provide agl computations.
- // But then we use the old way of having a fixed elevation value for
- // the whole lifetime of this cache.
- if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
- sgdVec3 tmp;
- sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
- sgdVec3 isectpoint;
- if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
- fgdPointInTriangle( isectpoint, t.vertices ) ) {
- found_ground = true;
- sgdAddVec3(isectpoint, cache_center);
- double this_radius = sgdLengthVec3(isectpoint);
- if (ground_radius < this_radius)
- ground_radius = this_radius;
- }
+class FGGroundCache::CatapultFinder : public BVHVisitor {
+public:
+ CatapultFinder(const SGSphered& sphere, const double& t) :
+ _haveLineSegment(false),
+ _sphere(sphere),
+ _time(t)
+ { }
+
+ virtual void apply(BVHGroup& leaf)
+ {
+ if (!intersects(_sphere, leaf.getBoundingSphere()))
+ return;
+ leaf.traverse(*this);
}
- }
-}
+ virtual void apply(BVHTransform& transform)
+ {
+ if (!intersects(_sphere, transform.getBoundingSphere()))
+ return;
+
+ SGSphered sphere = _sphere;
+ _sphere = transform.sphereToLocal(sphere);
+ bool haveLineSegment = _haveLineSegment;
+ _haveLineSegment = false;
+
+ transform.traverse(*this);
+
+ if (_haveLineSegment) {
+ _lineSegment = transform.lineSegmentToWorld(_lineSegment);
+ _linearVelocity = transform.vecToWorld(_linearVelocity);
+ _angularVelocity = transform.vecToWorld(_angularVelocity);
+ }
+ _haveLineSegment |= haveLineSegment;
+ _sphere.setCenter(sphere.getCenter());
+ }
+ virtual void apply(BVHMotionTransform& transform)
+ {
+ if (!intersects(_sphere, transform.getBoundingSphere()))
+ return;
+
+ SGSphered sphere = _sphere;
+ _sphere = transform.sphereToLocal(sphere, _time);
+ bool haveLineSegment = _haveLineSegment;
+ _haveLineSegment = false;
+
+ transform.traverse(*this);
+
+ if (_haveLineSegment) {
+ SGMatrixd toWorld = transform.getToWorldTransform(_time);
+ _linearVelocity
+ += transform.getLinearVelocityAt(_lineSegment.getStart());
+ _angularVelocity += transform.getAngularVelocity();
+ _linearVelocity = toWorld.xformVec(_linearVelocity);
+ _angularVelocity = toWorld.xformVec(_angularVelocity);
+ _lineSegment = _lineSegment.transform(toWorld);
+ }
+ _haveLineSegment |= haveLineSegment;
+ _sphere.setCenter(sphere.getCenter());
+ }
+ virtual void apply(BVHLineGeometry& node)
+ {
+ if (node.getType() != BVHLineGeometry::CarrierCatapult)
+ return;
+
+ SGLineSegmentd lineSegment(node.getLineSegment());
+ if (!intersects(_sphere, lineSegment))
+ return;
+
+ _lineSegment = lineSegment;
+ double dist = distSqr(lineSegment, getSphere().getCenter());
+ _sphere.setRadius(sqrt(dist));
+ _linearVelocity = SGVec3d::zeros();
+ _angularVelocity = SGVec3d::zeros();
+ _haveLineSegment = true;
+ }
+ virtual void apply(BVHStaticGeometry& node)
+ { }
+
+ virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
+ virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
+
+ void setSphere(const SGSphered& sphere)
+ { _sphere = sphere; }
+ const SGSphered& getSphere() const
+ { return _sphere; }
+
+ const SGLineSegmentd& getLineSegment() const
+ { return _lineSegment; }
+ const SGVec3d& getLinearVelocity() const
+ { return _linearVelocity; }
+ const SGVec3d& getAngularVelocity() const
+ { return _angularVelocity; }
+
+ bool getHaveLineSegment() const
+ { return _haveLineSegment; }
+
+protected:
+ SGLineSegmentd _lineSegment;
+ SGVec3d _linearVelocity;
+ SGVec3d _angularVelocity;
+
+ bool _haveLineSegment;
-inline void
-FGGroundCache::velocityTransformTriangle(double dt,
- FGGroundCache::Triangle& dst,
- const FGGroundCache::Triangle& src)
-{
- sgdCopyVec3(dst.vertices[0], src.vertices[0]);
- sgdCopyVec3(dst.vertices[1], src.vertices[1]);
- sgdCopyVec3(dst.vertices[2], src.vertices[2]);
-
- sgdCopyVec4(dst.plane, src.plane);
-
- sgdCopyVec3(dst.sphere.center, src.sphere.center);
- dst.sphere.radius = src.sphere.radius;
-
- sgdCopyVec3(dst.velocity, src.velocity);
- sgdCopyVec3(dst.rotation, src.rotation);
- sgdCopyVec3(dst.rotation_pivot, src.rotation_pivot);
-
- dst.type = src.type;
-
- if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
- sgdVec3 pivotoff, vel;
- for (int i = 0; i < 3; ++i) {
- sgdSubVec3(pivotoff, src.vertices[i], src.rotation_pivot);
- sgdVectorProductVec3(vel, src.rotation, pivotoff);
- sgdAddVec3(vel, src.velocity);
- sgdAddScaledVec3(dst.vertices[i], vel, dt);
- }
-
- // Transform the plane equation
- sgdSubVec3(pivotoff, dst.plane, src.rotation_pivot);
- sgdVectorProductVec3(vel, src.rotation, pivotoff);
- sgdAddVec3(vel, src.velocity);
- dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, vel);
-
- sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
- }
-}
+ SGSphered _sphere;
+ double _time;
+};
-void
-FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
- sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
+double
+FGGroundCache::get_cat(double t, const SGVec3d& pt,
+ SGVec3d end[2], SGVec3d vel[2])
{
- // Travel through all kids.
- ssgEntity *e;
- for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
- if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
- continue;
- if ( e->getBSphere()->isEmpty() )
- continue;
-
- // We need to check further if either the sphere around the branch
- // intersects the sphere around the aircraft or the line downwards from
- // the aircraft intersects the branchs sphere.
- sgdSphere esphere;
- sgdSetVec3(esphere.center, e->getBSphere()->center);
- esphere.radius = e->getBSphere()->radius;
- esphere.orthoXform(xform);
- bool wspIsec = wsp->intersects(&esphere);
- bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
- if (!wspIsec && !downIsec)
- continue;
-
- // For branches collect up the transforms to reach that branch and
- // call cache_fill recursively.
- if ( e->isAKindOf( ssgTypeBranch() ) ) {
- ssgBranch *b = (ssgBranch *)e;
- if ( b->isAKindOf( ssgTypeTransform() ) ) {
- // Collect up the transfors required to reach that part of
- // the branch.
- sgMat4 xform2;
- sgMakeIdentMat4( xform2 );
- ssgTransform *t = (ssgTransform*)b;
- t->getTransform( xform2 );
- sgdMat4 xform3;
- fgMultMat4(xform3, xform, xform2);
- cache_fill( b, xform3, sp, down, wsp );
- } else
- cache_fill( b, xform, sp, down, wsp );
- }
-
- // For leafs, check each triangle for intersection.
- // This will minimize the number of vertices/triangles in the cache.
- else if (e->isAKindOf(ssgTypeLeaf())) {
- // Since we reach that leaf if we have an intersection with the
- // most propably bigger wire/catapult cache sphere, we need to check
- // that here, if the smaller cache for the surface has a chance for hits.
- // Also, if the spheres do not intersect compute a croase agl value
- // by following the line downwards originating at the aircraft.
- bool spIsec = sp->intersects(&esphere);
- putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
-
- // If we are here, we need to put all special hardware here into
- // the cache.
- if (wspIsec)
- putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
- }
- }
+ double maxDistance = 1000;
+
+ // Get the wire in question
+ t += cache_time_offset;
+ CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t);
+ if (_localBvhTree)
+ _localBvhTree->accept(catapultFinder);
+
+ if (!catapultFinder.getHaveLineSegment())
+ return maxDistance;
+
+ // prepare the returns
+ end[0] = catapultFinder.getLineSegment().getStart();
+ end[1] = catapultFinder.getLineSegment().getEnd();
+
+ // The linear velocity is the one at the start of the line segment ...
+ vel[0] = catapultFinder.getLinearVelocity();
+ // ... so the end point has the additional cross product.
+ vel[1] = catapultFinder.getLinearVelocity();
+ vel[1] += cross(catapultFinder.getAngularVelocity(),
+ catapultFinder.getLineSegment().getDirection());
+
+ // Return the distance to the cat
+ return sqrt(distSqr(catapultFinder.getLineSegment(), pt));
}
bool
-FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
- double rad)
+FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
+ SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
+ simgear::BVHNode::Id& id, const SGMaterial*& material)
{
- // Empty cache.
- ground_radius = 0.0;
- found_ground = false;
- triangles.resize(0);
- catapults.resize(0);
- wires.resize(0);
-
- // Store the parameters we used to build up that cache.
- sgdCopyVec3(reference_wgs84_point, pt);
- reference_vehicle_radius = rad;
- // Store the time reference used to compute movements of moving triangles.
- cache_ref_time = ref_time;
-
- // Decide where we put the scenery center.
- Point3D old_cntr = globals->get_scenery()->get_center();
- Point3D cntr(pt[0], pt[1], pt[2]);
- // Only move the cache center if it is unaccaptable far away.
- if (40*40 < old_cntr.distance3Dsquared(cntr))
- globals->get_scenery()->set_center(cntr);
- else
- cntr = old_cntr;
-
- // The center of the cache.
- sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
-
- sgdVec3 ptoff;
- sgdSubVec3(ptoff, pt, cache_center);
- // Prepare sphere around the aircraft.
- sgdSphere acSphere;
- acSphere.setRadius(rad);
- acSphere.setCenter(ptoff);
-
- // Prepare bigger sphere around the aircraft.
- // This one is required for reliably finding wires we have caught but
- // have already left the hopefully smaller sphere for the ground reactions.
- const double max_wire_dist = 300.0;
- sgdSphere wireSphere;
- wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
- wireSphere.setCenter(ptoff);
-
- // Down vector. Is used for croase agl computations when we are far enough
- // from ground that we have an empty cache.
- sgdVec3 down;
- sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
- sgdNormalizeVec3(down);
-
- // We collaps all transforms we need to reach a particular leaf.
- // The leafs itself will be then transformed later.
- // So our cache is just flat.
- // For leafs which are moving (carriers surface, etc ...)
- // we will later store a speed in the GroundType class. We can then apply
- // some translations to that nodes according to the time which has passed
- // compared to that snapshot.
- sgdMat4 xform;
- sgdMakeIdentMat4( xform );
-
-
- // Walk the scene graph and extract solid ground triangles and carrier data.
- ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
- cache_fill(terrain, xform, &acSphere, down, &wireSphere);
-
- // some stats
- SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
- << ", # triangles = " << triangles.size()
- << ", # wires = " << wires.size()
- << ", # catapults = " << catapults.size()
- << ", ground_radius = " << ground_radius );
-
- // If the ground radius is still below 5e6 meters, then we do not yet have
- // any scenery.
- found_ground = found_ground && 5e6 < ground_radius;
- if (!found_ground)
- SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
- "without any scenery below the aircraft" );
-
- if (cntr != old_cntr)
- globals->get_scenery()->set_center(old_cntr);
-
- return found_ground;
+#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();
+ normal = lineSegmentVisitor.getNormal();
+ if (0 < dot(normal, down))
+ normal = -normal;
+ linearVel = lineSegmentVisitor.getLinearVelocity();
+ angularVel = lineSegmentVisitor.getAngularVelocity();
+ material = lineSegmentVisitor.getMaterial();
+ id = lineSegmentVisitor.getId();
+
+ return true;
+ } else {
+ // Whenever we did not have a ground triangle for the requested point,
+ // take the ground level we found during the current cache build.
+ // This is as good as what we had before for agl.
+ SGGeod geodPt = SGGeod::fromCart(pt);
+ geodPt.setElevationM(_altitude);
+ contact = SGVec3d::fromGeod(geodPt);
+ normal = -down;
+ linearVel = SGVec3d(0, 0, 0);
+ angularVel = SGVec3d(0, 0, 0);
+ material = _material;
+ id = 0;
+
+ return found_ground;
+ }
}
+
bool
-FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
+FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
+ SGVec3d& contact, SGVec3d& linearVel,
+ SGVec3d& angularVel, simgear::BVHNode::Id& id,
+ const SGMaterial*& material)
{
- sgdCopyVec3(pt, reference_wgs84_point);
- *rad = reference_vehicle_radius;
- *ref_time = cache_ref_time;
- return found_ground;
+ 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;
+
+ // Have geometry in the range of maxDist
+ contact = nearestPointVisitor.getPoint();
+ linearVel = nearestPointVisitor.getLinearVelocity();
+ angularVel = nearestPointVisitor.getAngularVelocity();
+ material = nearestPointVisitor.getMaterial();
+ id = nearestPointVisitor.getId();
+
+ return true;
}
-double
-FGGroundCache::get_cat(double t, const double dpt[3],
- double end[2][3], double vel[2][3])
-{
- // start with a distance of 1e10 meters...
- double dist = 1e10;
-
- // Time difference to the reference time.
- t -= cache_ref_time;
-
- size_t sz = catapults.size();
- for (size_t i = 0; i < sz; ++i) {
- sgdVec3 pivotoff, rvel[2];
- sgdLineSegment3 ls;
- sgdCopyVec3(ls.a, catapults[i].start);
- sgdCopyVec3(ls.b, catapults[i].end);
-
- sgdSubVec3(pivotoff, ls.a, catapults[i].rotation_pivot);
- sgdVectorProductVec3(rvel[0], catapults[i].rotation, pivotoff);
- sgdAddVec3(rvel[0], catapults[i].velocity);
- sgdSubVec3(pivotoff, ls.b, catapults[i].rotation_pivot);
- sgdVectorProductVec3(rvel[1], catapults[i].rotation, pivotoff);
- sgdAddVec3(rvel[1], catapults[i].velocity);
-
- sgdAddVec3(ls.a, cache_center);
- sgdAddVec3(ls.b, cache_center);
-
- sgdAddScaledVec3(ls.a, rvel[0], t);
- sgdAddScaledVec3(ls.b, rvel[1], t);
-
- double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
- if (this_dist < dist) {
- SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
- << this_dist << " meters away");
- dist = this_dist;
-
- // The carrier code takes care of that ordering.
- sgdCopyVec3( end[0], ls.a );
- sgdCopyVec3( end[1], ls.b );
- sgdCopyVec3( vel[0], rvel[0] );
- sgdCopyVec3( vel[1], rvel[1] );
+
+class FGGroundCache::WireIntersector : public BVHVisitor {
+public:
+ WireIntersector(const SGVec3d pt[4], const double& t) :
+ _linearVelocity(SGVec3d::zeros()),
+ _angularVelocity(SGVec3d::zeros()),
+ _wire(0),
+ _time(t)
+ {
+ // Build the two triangles spanning the area where the hook has moved
+ // during the past step.
+ _triangles[0].set(pt[0], pt[1], pt[2]);
+ _triangles[1].set(pt[0], pt[2], pt[3]);
}
- }
- // At the end take the root, we only computed squared distances ...
- return sqrt(dist);
-}
+ virtual void apply(BVHGroup& leaf)
+ {
+ if (!_intersects(leaf.getBoundingSphere()))
+ return;
-bool
-FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff,
- double contact[3], double normal[3], double vel[3],
- int *type, double *loadCapacity,
- double *frictionFactor, double *agl)
-{
- bool ret = false;
-
- *type = FGInterface::Unknown;
-// *agl = 0.0;
- *loadCapacity = DBL_MAX;
- *frictionFactor = 1.0;
- sgdSetVec3( vel, 0.0, 0.0, 0.0 );
- sgdSetVec3( contact, 0.0, 0.0, 0.0 );
- sgdSetVec3( normal, 0.0, 0.0, 0.0 );
-
- // Time difference to th reference time.
- t -= cache_ref_time;
-
- // The double valued point we start to search for intersection.
- sgdVec3 pt;
- sgdSubVec3( pt, dpt, cache_center );
-
- // The search direction
- sgdVec3 dir;
- sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
- sgdNormaliseVec3( dir );
-
- // Initialize to something sensible
- double current_radius = 0.0;
-
- size_t sz = triangles.size();
- for (size_t i = 0; i < sz; ++i) {
- Triangle triangle;
- velocityTransformTriangle(t, triangle, triangles[i]);
- if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
- continue;
-
- // Check for intersection.
- sgdVec3 isecpoint;
- if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
- sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
- // Transform to the wgs system
- sgdAddVec3( isecpoint, cache_center );
- // compute the radius, good enough approximation to take the geocentric radius
- SGDfloat radius = sgdLengthSquaredVec3(isecpoint);
- if (current_radius < radius) {
- // Compute the vector from pt to the intersection point ...
- sgdVec3 off;
- sgdSubVec3(off, pt, isecpoint);
- // ... and check if it is too high or not
- if (-max_altoff < sgdScalarProductVec3( off, dir )) {
- current_radius = radius;
- ret = true;
- // Save the new potential intersection point.
- sgdCopyVec3( contact, isecpoint );
- // The first three values in the vector are the plane normal.
- sgdCopyVec3( normal, triangle.plane );
- // The velocity wrt earth.
- sgdVec3 pivotoff;
- sgdSubVec3(pivotoff, pt, triangle.rotation_pivot);
- sgdVectorProductVec3(vel, triangle.rotation, pivotoff);
- sgdAddVec3(vel, triangle.velocity);
- // Save the ground type.
- *type = triangle.type;
- // FIXME: figure out how to get that sign ...
-// *agl = sqrt(sqdist);
- *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
-// *loadCapacity = DBL_MAX;
-// *frictionFactor = 1.0;
+ leaf.traverse(*this);
+ }
+ virtual void apply(BVHTransform& transform)
+ {
+ if (!_intersects(transform.getBoundingSphere()))
+ return;
+
+ SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
+ _triangles[0] = triangles[0].transform(transform.getToLocalTransform());
+ _triangles[1] = triangles[1].transform(transform.getToLocalTransform());
+
+ transform.traverse(*this);
+
+ if (_wire) {
+ _lineSegment = transform.lineSegmentToWorld(_lineSegment);
+ _linearVelocity = transform.vecToWorld(_linearVelocity);
+ _angularVelocity = transform.vecToWorld(_angularVelocity);
}
- }
+ _triangles[0] = triangles[0];
+ _triangles[1] = triangles[1];
}
- }
+ virtual void apply(BVHMotionTransform& transform)
+ {
+ if (!_intersects(transform.getBoundingSphere()))
+ return;
+
+ SGMatrixd toLocal = transform.getToLocalTransform(_time);
- if (ret)
- return true;
+ SGTriangled triangles[2] = { _triangles[0], _triangles[1] };
+ _triangles[0] = triangles[0].transform(toLocal);
+ _triangles[1] = triangles[1].transform(toLocal);
+
+ transform.traverse(*this);
+
+ if (_wire) {
+ SGMatrixd toWorld = transform.getToWorldTransform(_time);
+ _linearVelocity
+ += transform.getLinearVelocityAt(_lineSegment.getStart());
+ _angularVelocity += transform.getAngularVelocity();
+ _linearVelocity = toWorld.xformVec(_linearVelocity);
+ _angularVelocity = toWorld.xformVec(_angularVelocity);
+ _lineSegment = _lineSegment.transform(toWorld);
+ }
+ _triangles[0] = triangles[0];
+ _triangles[1] = triangles[1];
+ }
+ virtual void apply(BVHLineGeometry& node)
+ {
+ if (node.getType() != BVHLineGeometry::CarrierWire)
+ return;
+ SGLineSegmentd lineSegment(node.getLineSegment());
+ if (!_intersects(lineSegment))
+ return;
+
+ _lineSegment = lineSegment;
+ _linearVelocity = SGVec3d::zeros();
+ _angularVelocity = SGVec3d::zeros();
+ _wire = &node;
+ }
+ virtual void apply(BVHStaticGeometry& node)
+ { }
+
+ virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
+ virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
+
+ bool _intersects(const SGSphered& sphere) const
+ {
+ if (_wire)
+ return false;
+ if (intersects(_triangles[0], sphere))
+ return true;
+ if (intersects(_triangles[1], sphere))
+ return true;
+ return false;
+ }
+ bool _intersects(const SGLineSegmentd& lineSegment) const
+ {
+ if (_wire)
+ return false;
+ if (intersects(_triangles[0], lineSegment))
+ return true;
+ if (intersects(_triangles[1], lineSegment))
+ return true;
+ return false;
+ }
+
+ const SGLineSegmentd& getLineSegment() const
+ { return _lineSegment; }
+ const SGVec3d& getLinearVelocity() const
+ { return _linearVelocity; }
+ const SGVec3d& getAngularVelocity() const
+ { return _angularVelocity; }
+
+ const BVHLineGeometry* getWire() const
+ { return _wire; }
+
+private:
+ SGLineSegmentd _lineSegment;
+ SGVec3d _linearVelocity;
+ SGVec3d _angularVelocity;
+ const BVHLineGeometry* _wire;
- // Whenever we did not have a ground triangle for the requested point,
- // take the ground level we found during the current cache build.
- // This is as good as what we had before for agl.
- double r = sgdLengthVec3( dpt );
- sgdCopyVec3( contact, dpt );
- sgdScaleVec3( contact, ground_radius/r );
- sgdCopyVec3( normal, dpt );
- sgdNormaliseVec3( normal );
- sgdSetVec3( vel, 0.0, 0.0, 0.0 );
-
- // The altitude is the distance of the requested point from the
- // contact point.
- *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
- *type = FGInterface::Unknown;
- *loadCapacity = DBL_MAX;
- *frictionFactor = 1.0;
-
- return ret;
-}
+ SGTriangled _triangles[2];
+ double _time;
+};
-bool FGGroundCache::caught_wire(double t, const double pt[4][3])
+bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
{
- size_t sz = wires.size();
- if (sz == 0)
- return false;
-
- // Time difference to the reference time.
- t -= cache_ref_time;
-
- // Build the two triangles spanning the area where the hook has moved
- // during the past step.
- sgdVec4 plane[2];
- sgdVec3 tri[2][3];
- sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
- sgdCopyVec3( tri[0][0], pt[0] );
- sgdCopyVec3( tri[0][1], pt[1] );
- sgdCopyVec3( tri[0][2], pt[2] );
- sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
- sgdCopyVec3( tri[1][0], pt[0] );
- sgdCopyVec3( tri[1][1], pt[2] );
- sgdCopyVec3( tri[1][2], pt[3] );
-
- // Intersect the wire lines with each of these triangles.
- // You have cautght a wire if they intersect.
- for (size_t i = 0; i < sz; ++i) {
- sgdVec3 le[2];
- for (int k = 0; k < 2; ++k) {
- sgdVec3 pivotoff, vel;
- sgdCopyVec3(le[k], wires[i].ends[k]);
- sgdSubVec3(pivotoff, le[k], wires[i].rotation_pivot);
- sgdVectorProductVec3(vel, wires[i].rotation, pivotoff);
- sgdAddVec3(vel, wires[i].velocity);
- sgdAddScaledVec3(le[k], vel, t);
- sgdAddVec3(le[k], cache_center);
- }
-
- for (int k=0; k<2; ++k) {
- sgdVec3 isecpoint;
- double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
- if ( 0.0 <= isecval && isecval <= 1.0 &&
- sgdPointInTriangle( isecpoint, tri[k] ) ) {
- SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
- // Store the wire id.
- wire_id = wires[i].wire_id;
- return true;
- }
+ // Get the wire in question
+ t += cache_time_offset;
+ WireIntersector wireIntersector(pt, t);
+ if (_localBvhTree)
+ _localBvhTree->accept(wireIntersector);
+
+ _wire = wireIntersector.getWire();
+ return _wire;
+}
+
+class FGGroundCache::WireFinder : public BVHVisitor {
+public:
+ WireFinder(const BVHLineGeometry* wire, const double& t) :
+ _wire(wire),
+ _time(t),
+ _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()),
+ _linearVelocity(SGVec3d::zeros()),
+ _angularVelocity(SGVec3d::zeros()),
+ _haveLineSegment(false)
+ { }
+
+ virtual void apply(BVHGroup& leaf)
+ {
+ if (_haveLineSegment)
+ return;
+ leaf.traverse(*this);
}
- }
+ virtual void apply(BVHTransform& transform)
+ {
+ if (_haveLineSegment)
+ return;
- return false;
-}
+ transform.traverse(*this);
+
+ if (_haveLineSegment) {
+ _linearVelocity = transform.vecToWorld(_linearVelocity);
+ _angularVelocity = transform.vecToWorld(_angularVelocity);
+ _lineSegment = transform.lineSegmentToWorld(_lineSegment);
+ }
+ }
+ virtual void apply(BVHMotionTransform& transform)
+ {
+ if (_haveLineSegment)
+ return;
+
+ transform.traverse(*this);
+
+ if (_haveLineSegment) {
+ SGMatrixd toWorld = transform.getToWorldTransform(_time);
+ _linearVelocity
+ += transform.getLinearVelocityAt(_lineSegment.getStart());
+ _angularVelocity += transform.getAngularVelocity();
+ _linearVelocity = toWorld.xformVec(_linearVelocity);
+ _angularVelocity = toWorld.xformVec(_angularVelocity);
+ _lineSegment = _lineSegment.transform(toWorld);
+ }
+ }
+ virtual void apply(BVHLineGeometry& node)
+ {
+ if (_haveLineSegment)
+ return;
+ if (_wire != &node)
+ return;
+ if (node.getType() != BVHLineGeometry::CarrierWire)
+ return;
+ _lineSegment = SGLineSegmentd(node.getLineSegment());
+ _linearVelocity = SGVec3d::zeros();
+ _angularVelocity = SGVec3d::zeros();
+ _haveLineSegment = true;
+ }
+ virtual void apply(BVHStaticGeometry&) { }
+
+ virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
+ virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
+
+ const SGLineSegmentd& getLineSegment() const
+ { return _lineSegment; }
+
+ bool getHaveLineSegment() const
+ { return _haveLineSegment; }
+
+ const SGVec3d& getLinearVelocity() const
+ { return _linearVelocity; }
+ const SGVec3d& getAngularVelocity() const
+ { return _angularVelocity; }
-bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
+private:
+ const BVHLineGeometry* _wire;
+ double _time;
+
+ SGLineSegmentd _lineSegment;
+ SGVec3d _linearVelocity;
+ SGVec3d _angularVelocity;
+
+ bool _haveLineSegment;
+};
+
+bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
{
- // Fast return if we do not have an active wire.
- if (wire_id < 0)
- return false;
-
- // Time difference to the reference time.
- t -= cache_ref_time;
-
- // Search for the wire with the matching wire id.
- size_t sz = wires.size();
- for (size_t i = 0; i < sz; ++i) {
- if (wires[i].wire_id == wire_id) {
- for (size_t k = 0; k < 2; ++k) {
- sgdVec3 pivotoff;
- sgdCopyVec3(end[k], wires[i].ends[k]);
- sgdSubVec3(pivotoff, end[k], wires[i].rotation_pivot);
- sgdVectorProductVec3(vel[k], wires[i].rotation, pivotoff);
- sgdAddVec3(vel[k], wires[i].velocity);
- sgdAddScaledVec3(end[k], vel[k], t);
- sgdAddVec3(end[k], cache_center);
- }
- return true;
- }
- }
-
- return false;
+ // Fast return if we do not have an active wire.
+ if (!_wire)
+ return false;
+
+ // Get the wire in question
+ t += cache_time_offset;
+ WireFinder wireFinder(_wire, t);
+ if (_localBvhTree)
+ _localBvhTree->accept(wireFinder);
+
+ if (!wireFinder.getHaveLineSegment())
+ return false;
+
+ // prepare the returns
+ end[0] = wireFinder.getLineSegment().getStart();
+ end[1] = wireFinder.getLineSegment().getEnd();
+
+ // The linear velocity is the one at the start of the line segment ...
+ vel[0] = wireFinder.getLinearVelocity();
+ // ... so the end point has the additional cross product.
+ vel[1] = wireFinder.getLinearVelocity();
+ vel[1] += cross(wireFinder.getAngularVelocity(),
+ wireFinder.getLineSegment().getDirection());
+
+ return true;
}
void FGGroundCache::release_wire(void)
{
- wire_id = -1;
+ _wire = 0;
}
projects / flightgear.git / blobdiff