X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=0b341159aadd04bae91e51e22971dc989d2fd461;hb=2314ccfe13e5e175763000e4c8ba86be79aa3a97;hp=c6f5cf8692b2013655fd4a5440fe2a3d0c4b4388;hpb=821436441499c1f8233aa3957b0143d8cf1f85f6;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index c6f5cf869..0b341159a 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -2,7 +2,7 @@ // // 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 @@ -16,683 +16,1037 @@ // // 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 +#include "groundcache.hxx" + +#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef GROUNDCACHE_DEBUG +#include +#include
+#endif #include
#include #include -#include #include "flight.hxx" -#include "groundcache.hxx" -FGGroundCache::FGGroundCache() +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); + } + virtual void apply(osg::Node& node) + { + if (!testBoundingSphere(node.getBound())) + return; + + addBoundingVolume(node); + } + + virtual void apply(osg::Group& group) + { + if (!testBoundingSphere(group.getBound())) + return; + + 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 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); + + 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); + } + + 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; + } + + 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; + } + + // 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 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) { - 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; +#ifdef GROUNDCACHE_DEBUG + _lookupTime = SGTimeStamp::fromSec(0.0); + _lookupCount = 0; + _buildTime = SGTimeStamp::fromSec(0.0); + _buildCount = 0; +#endif } FGGroundCache::~FGGroundCache() { - cache_root.removeAllKids(); } -FGGroundCache::GroundProperty* -FGGroundCache::extractGroundProperty( ssgLeaf* l ) +bool +FGGroundCache::prepare_ground_cache(double startSimTime, double endSimTime, + const SGVec3d& pt, double rad) { - // 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 = new GroundProperty; - gp->wire_id = -1; - - FGAICarrierHardware *ud = - dynamic_cast(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 ); - } - - else { - - // Initialize velocity field. - sgSetVec3( gp->vel, 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(); - - SG_LOG(SG_FLIGHT,SG_INFO, - "New triangle in cache: " << category << " " << 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; -} +#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; -// 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 bool -sgIsectSphereInfLine(const sgSphere *sp, - const sgVec3 pt_on_line, const sgVec3 dir) -{ - sgVec3 r ; - sgSubVec3 ( r, sp->getCenter(), pt_on_line ) ; + // 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; + _material = 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); + } - SGfloat projectedDistance = sgScalarProductVec3(r, dir); - - SGfloat dist = sgScalarProductVec3 ( r, r ) - - projectedDistance * projectedDistance; + 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 - SGfloat radius = sp->getRadius(); - return dist < radius*radius; + return found_ground; } -void -FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia, - const sgMat4 xform) +bool +FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) { - // Extract data from the leaf which is just copied. - ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices(); - // Create a new leaf. - ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 0, 0, ia ); - // Clones data ... - vtxa->removeUnusedVertices(); - // Apply transform. We won't store transforms in our cache. - vtxa->transform( xform ); - // Check for magic texture names object names and such ... - vtxa->setUserData( extractGroundProperty( l ) ); - // Finally append to cache. - cache_root.addKid((ssgEntity*)vtxa); + pt = reference_wgs84_point; + rad = reference_vehicle_radius; + ref_time = cache_ref_time; + return found_ground; } -void -FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform, - ssgLeaf *l) -{ - ssgIndexArray *ia = 0; - - // Lines must have special meanings. - // Wires and catapults are done with lines. - int nl = l->getNumLines(); - for (int i = 0; i < nl; ++i) { - sgSphere tmp; - short v[2]; - l->getLine(i, v, v+1 ); - for (int k=0; k<2; ++k) - tmp.extend( l->getVertex( v[k] ) ); - tmp.orthoXform(xform); - - if (wsp->intersects( &tmp )) { - if (ia == 0) - ia = new ssgIndexArray(); +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(BVHPageNode& leaf) + { + if (_foundId) + return; + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (_foundId) + return; + + transform.traverse(*this); - ia->add( v[0] ); - ia->add( v[1] ); - } - } - if (!ia) - return; - - addAndFlattenLeaf(GL_LINES, l, ia, xform); -} + 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::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform, - bool sphIsec, sgVec3 down, ssgLeaf *l) -{ - ssgIndexArray *ia = 0; - - int nt = l->getNumTriangles(); - for (int i = 0; i < nt; ++i) { - // Build up a sphere around that particular triangle- - sgSphere tmp; - short v[3]; - l->getTriangle(i, v, v+1, v+2 ); - for (int k=0; k<3; ++k) - tmp.extend( l->getVertex( v[k] ) ); - tmp.orthoXform(xform); - - // 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( &tmp )) { - if (ia == 0) - ia = new ssgIndexArray(); - - ia->add( v[0] ); - ia->add( v[1] ); - ia->add( v[2] ); - } - - // 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 ( sgIsectSphereInfLine(&tmp, sp->getCenter(), down) ) { - sgVec3 tri[3]; - for (int k=0; k<3; ++k) { - sgCopyVec3( tri[k], l->getVertex( v[k] ) ); - sgXformPnt3( tri[k], xform ); - } - - sgVec4 plane; - sgMakePlane( plane, tri[0], tri[1], tri[2] ); - sgVec3 ac_cent; - sgCopyVec3(ac_cent, sp->getCenter()); - sgVec3 dst; - sgIsectInfLinePlane( dst, ac_cent, down, plane ); - if ( sgPointInTriangle ( dst, tri ) ) { - found_ground = true; - sgdVec3 ddst; - sgdSetVec3(ddst, dst); - sgdAddVec3(ddst, cache_center); - double this_radius = sgdLengthVec3(ddst); - if (ground_radius < this_radius) - ground_radius = this_radius; - } - } - } - if (!ia) - return; - - addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform); -} + SGMatrixd _bodyToWorld; -// Here is the point where rotation should be handled -void -FGGroundCache::extractCacheRelativeVertex(double t, ssgVtxArray *va, - GroundProperty *gp, - short i, sgVec3 rel_pos, - sgdVec3 wgs84_vel) -{ - sgCopyVec3( rel_pos, va->getVertex( i ) ); - sgAddScaledVec3( rel_pos, gp->vel, t ); + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _foundId; - // Set velocity. - sgdSetVec3( wgs84_vel, gp->vel ); -} + double _time; +}; -void -FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va, - GroundProperty *gp, short i, - sgdVec3 wgs84_pos, sgdVec3 wgs84_vel) +bool +FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel, + SGVec3d& angularVel, simgear::BVHNode::Id id) { - sgVec3 rel_pos; - extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel); - sgdSetVec3( wgs84_pos, rel_pos ); - sgdAddVec3( wgs84_pos, cache_center ); + // 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; } +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(BVHPageNode& 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; + + SGSphered _sphere; + double _time; +}; -void -FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform, - sgSphere* sp, sgVec3 down, sgSphere* 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; - - // Wee 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. - sgSphere esphere = *(e->getBSphere()); - esphere.orthoXform(xform); - bool wspIsec = wsp->intersects(&esphere); - bool downIsec = sgIsectSphereInfLine(&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 ); - sgPostMultMat4( xform2, xform ); - cache_fill( b, xform2, 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 simgear::BVHMaterial*& material) { - // Empty cache. - cache_root.removeAllKids(); - ground_radius = 0.0; - found_ground = false; - - // 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; - - // The center of the cache. - Point3D psc = globals->get_tile_mgr()->get_current_center(); - sgdSetVec3(cache_center, psc[0], psc[1], psc[2]); - - // Prepare sphere around the aircraft. - sgSphere acSphere; - acSphere.setRadius(rad); - - // Compute the postion of the aircraft relative to the scenery center. - sgdVec3 doffset; - sgdSubVec3( doffset, pt, cache_center ); - sgVec3 offset; - sgSetVec3( offset, doffset[0], doffset[1], doffset[2] ); - acSphere.setCenter( offset ); - - // 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; - sgSphere wireSphere; - wireSphere.setRadius( max_wire_dist < rad ? rad : max_wire_dist ); - wireSphere.setCenter( offset ); - - // Down vector. Is used for croase agl computations when we are far enough - // from ground that we have an empty cache. - sgVec3 down; - sgSetVec3(down, -pt[0], -pt[1], -pt[2]); - sgNormalizeVec3(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. - sgMat4 xform; - sgMakeIdentMat4(xform); - - // Walk the terrain branch for now. - ssgBranch *terrain = globals->get_scenery()->get_scene_graph(); - cache_fill(terrain, xform, &acSphere, down, &wireSphere); - - // some stats - SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad - << ", # leafs = " << cache_root.getNumKids() - << ", 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" ); - - 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 simgear::BVHMaterial*& 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; - - // We know that we have a flat cache ... - ssgEntity *e; - for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) { - // We just know that, because we build that ourselfs ... - ssgVtxArray *va = (ssgVtxArray *)e; - // Only lines are interresting ... - if (va->getPrimitiveType() != GL_LINES) - continue; - GroundProperty *gp = dynamic_cast(va->getUserData()); - // Assertation??? - if ( !gp ) - continue; - // Check if we have a catapult ... - if ( gp->type != FGInterface::Catapult ) - continue; - - int nl = va->getNumLines(); - for (int i=0; i < nl; ++i) { - sgdLineSegment3 ls; - sgdVec3 lsVel[2]; - short vi[2]; - va->getLine(i, vi, vi+1 ); - extractWgs84Vertex(t, va, gp, vi[0], ls.a, lsVel[0]); - extractWgs84Vertex(t, va, gp, vi[1], ls.b, lsVel[1]); - - double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt ); - if (this_dist < dist) { - dist = this_dist; + +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]); + } + + virtual void apply(BVHGroup& leaf) + { + if (!_intersects(leaf.getBoundingSphere())) + return; + + 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())) + return; - // end[0] is the end where the cat starts. - // end[1] is the end where the cat ends. - // The carrier code takes care of that ordering. - sgdCopyVec3( end[0], ls.a ); - sgdCopyVec3( end[1], ls.b ); - sgdCopyVec3( vel[0], lsVel[0] ); - sgdCopyVec3( vel[1], lsVel[1] ); - } - } - } - - // At the end take the root, we only computed squared distances ... - return sqrt(dist); -} + 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); -bool -FGGroundCache::get_agl(double t, const double dpt[3], - 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 tmp; - sgdSubVec3( tmp, dpt, cache_center ); - sgVec3 pt; - sgSetVec3( pt, tmp ); - - // The search direction - sgVec3 dir; - sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] ); - - // Initialize to something sensible - double sqdist = DBL_MAX; - - // We know that we have a flat cache ... - // We just know that, because we build that ourselfs ... - ssgEntity *e; - for ( e = cache_root.getKid(0) ; e != NULL ; e = cache_root.getNextKid() ) { - // We just know that, because we build that ourselfs ... - ssgVtxArray *va = (ssgVtxArray *)e; - // AGL computations are done with triangle/surface leafs. - if (va->getPrimitiveType() != GL_TRIANGLES) - continue; - GroundProperty *gp = dynamic_cast(va->getUserData()); - // Assertation??? - if ( !gp ) - continue; - - int nt = va->getNumTriangles(); - for (int i=0; i < nt; ++i) { - short vi[3]; - va->getTriangle( i, vi, vi+1, vi+2 ); - - sgVec3 tri[3]; - sgdVec3 dvel[3]; - for (int k=0; k<3; ++k) - extractCacheRelativeVertex(t, va, gp, vi[k], tri[k], dvel[k]); - sgVec4 plane; - sgMakePlane( plane, tri[0], tri[1], tri[2] ); - - // Check for intersection. - sgVec3 isecpoint; - if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) && - sgPointInTriangle3( isecpoint, tri ) ) { - // Check for the closest intersection point. - // FIXME: is this the right one? - double newSqdist = sgDistanceSquaredVec3( isecpoint, pt ); - if ( newSqdist < sqdist ) { - sqdist = newSqdist; - ret = true; - // Save the new potential intersection point. - sgdSetVec3( contact, isecpoint ); - sgdAddVec3( contact, cache_center ); - // The first three values in the vector are the plane normal. - sgdSetVec3( normal, plane ); - // The velocity wrt earth. - /// FIXME: only true for non rotating objects!!!! - sgdCopyVec3( vel, dvel[0] ); - // Save the ground type. - *type = gp->type; - // FIXME: figure out how to get that sign ... -// *agl = sqrt(sqdist); - *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); -// *loadCapacity = DBL_MAX; -// *frictionFactor = 1.0; + 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; - if (ret) - return true; + SGTriangled _triangles[2]; + double _time; +}; - // 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; +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 != NULL); } -bool FGGroundCache::caught_wire(double t, const double cpt[4][3]) -{ - bool ret = false; - - // Time difference to the reference time. - t -= cache_ref_time; - - bool firsttime = true; - sgVec4 plane[2]; - sgVec3 tri[2][3]; - - // We know that we have a flat cache ... - ssgEntity *e; - for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) { - // We just know that, because we build that ourselfs ... - ssgVtxArray *va = (ssgVtxArray *)e; - // Only lines are interresting ... - if (va->getPrimitiveType() != GL_LINES) - continue; - GroundProperty *gp = dynamic_cast(va->getUserData()); - // Assertation??? - if ( !gp ) - continue; - // Check if we have a catapult ... - if ( gp->type != FGInterface::Wire ) - continue; - - // Lazy compute the values required for intersectiion tests. - // Since we normally do not have wires in the cache this is a - // huge benefit. - if (firsttime) { - firsttime = false; - sgVec3 pt[4]; - for (int k=0; k<4; ++k) { - sgdVec3 tmp; - sgdSubVec3( tmp, cpt[k], cache_center ); - sgSetVec3( pt[k], tmp ); - } - sgMakePlane( plane[0], pt[0], pt[1], pt[2] ); - sgCopyVec3( tri[0][0], pt[0] ); - sgCopyVec3( tri[0][1], pt[1] ); - sgCopyVec3( tri[0][2], pt[2] ); - sgMakePlane( plane[1], pt[0], pt[2], pt[3] ); - sgCopyVec3( tri[1][0], pt[0] ); - sgCopyVec3( tri[1][1], pt[2] ); - sgCopyVec3( tri[1][2], pt[3] ); - } - - int nl = va->getNumLines(); - for (int i=0; i < nl; ++i) { - short vi[2]; - va->getLine(i, vi, vi+1 ); - sgVec3 le[2]; - sgdVec3 dummy; - extractCacheRelativeVertex(t, va, gp, vi[0], le[0], dummy); - extractCacheRelativeVertex(t, va, gp, vi[1], le[1], dummy); - - for (int k=0; k<2; ++k) { - sgVec3 isecpoint; - float isecval = sgIsectLinesegPlane( isecpoint, le[0], le[1], plane[k] ); +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(BVHPageNode& leaf) + { + if (_haveLineSegment) + return; + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (_haveLineSegment) + return; + + transform.traverse(*this); - if ( 0.0 <= isecval && isecval <= 1.0 && - sgPointInTriangle( isecpoint, tri[k] ) ) { - // Store the wire id. - wire_id = gp->wire_id; - ret = true; + if (_haveLineSegment) { + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + _lineSegment = transform.lineSegmentToWorld(_lineSegment); } - } } - } + virtual void apply(BVHMotionTransform& transform) + { + if (_haveLineSegment) + return; - return ret; -} + 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; } -bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3]) + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + +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; - - bool ret = false; - - // Time difference to th reference time. - t -= cache_ref_time; - - // We know that we have a flat cache ... - ssgEntity *e; - for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) { - // We just know that, because we build that ourselfs ... - ssgVtxArray *va = (ssgVtxArray *)e; - // Only lines are interresting ... - if (va->getPrimitiveType() != GL_LINES) - continue; - GroundProperty *gp = dynamic_cast(va->getUserData()); - // Assertation??? - if ( !gp ) - continue; - // Check if we have a catapult ... - if ( gp->type != FGInterface::Wire ) - continue; - if ( gp->wire_id != wire_id ) - continue; - - // Get the line ends, that are the wire endpoints. - short vi[2]; - va->getLine(0, vi, vi+1 ); - extractWgs84Vertex(t, va, gp, vi[0], end[0], vel[0]); - extractWgs84Vertex(t, va, gp, vi[1], end[1], vel[1]); - - ret = true; - } - - return ret; + // 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; }