X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=0b341159aadd04bae91e51e22971dc989d2fd461;hb=2314ccfe13e5e175763000e4c8ba86be79aa3a97;hp=d4cfb80a9612a8b4a8de9b9b4b0f959dcea5ba70;hpb=14fe03ba6fd65efdf3283fd692fe6347af722934;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index d4cfb80a9..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 @@ -24,766 +24,1029 @@ # 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 +#include + +#ifdef GROUNDCACHE_DEBUG +#include +#include
+#endif #include
#include #include -#include #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 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); + } - // 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 getBVHNode() const + { return mSubTreeCollector.getNode(); } + + bool getHaveElevationBelowCache() const + { return _haveHit; } + double getElevationBelowCache() const + { return SGGeod::fromCart(_sceneryHit).getElevationM(); } + const simgear::BVHMaterial* getMaterialBelowCache() const + { return _material; } + +private: + SGVec3d _center; + SGVec3d _down; + double _radius; + double _startTime; + double _endTime; + + simgear::BVHSubTreeCollector mSubTreeCollector; + SGVec3d _sceneryHit; + double _maxDown; + const simgear::BVHMaterial* _material; + bool _haveHit; +}; + +FGGroundCache::FGGroundCache() : + _altitude(0), + _material(0), + cache_ref_time(0), + cache_time_offset(0), + _wire(0), + reference_wgs84_point(SGVec3d(0, 0, 0)), + reference_vehicle_radius(0), + down(0.0, 0.0, 0.0), + found_ground(false) +{ +#ifdef GROUNDCACHE_DEBUG + _lookupTime = SGTimeStamp::fromSec(0.0); + _lookupCount = 0; + _buildTime = SGTimeStamp::fromSec(0.0); + _buildCount = 0; +#endif +} - 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 - SGDfloat projectedDistance = sgdScalarProductVec3(r, dir); - - SGDfloat dist = sgdScalarProductVec3 ( r, r ) - - projectedDistance * projectedDistance; + // Empty cache. + found_ground = false; - SGDfloat radius = sp.getRadius(); - return dist < radius*radius; -} + 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; -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; -} + // 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); + } -FGGroundCache::~FGGroundCache() -{ -} + 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::GroundProperty -FGGroundCache::extractGroundProperty( ssgLeaf* l ) -{ - // FIXME: Do more ... - // Idea: have a get_globals() function which knows about that stuff. - // Or most probably 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(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 some material information for use in the gear model - gp.material = globals->get_matlib()->findMaterial(l); - if (gp.material) - gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water; - } - - return gp; + return found_ground; } -void -FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform, - ssgLeaf *l) +bool +FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) { - 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); - } - } - } + pt = reference_wgs84_point; + rad = reference_vehicle_radius; + ref_time = cache_ref_time; + return found_ground; } -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(); - t.material = gp.material; - 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 ) ) { - // Compute the offset to the ground cache midpoint - sgdVec3 off; - sgdSubVec3(off, isectpoint, tmp); - // Only accept the altitude if the intersection point is below the - // ground cache midpoint - if (0 < sgdScalarProductVec3( off, down )) { - found_ground = true; - sgdAddVec3(isectpoint, cache_center); - double this_radius = sgdLengthVec3(isectpoint); - if (ground_radius < this_radius) - ground_radius = this_radius; +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); + + 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; + + SGMatrixd _bodyToWorld; + + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _foundId; + + double _time; +}; -inline void -FGGroundCache::velocityTransformTriangle(double dt, - FGGroundCache::Triangle& dst, - const FGGroundCache::Triangle& src) +bool +FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel, + SGVec3d& angularVel, simgear::BVHNode::Id id) { - 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; - dst.material = src.material; - - 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); - } + // 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::cache_fill(ssgBranch *branch, sgdMat4 xform, - sgdSphere* sp, sgdVec3 down, sgdSphere* wsp) +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; +}; + +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 transforms 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 probably 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 coarse 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. - 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 unacceptable 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 collapse 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 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; - - 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, const SGMaterial** material, double *agl) -{ - bool ret = false; - - *type = FGInterface::Unknown; -// *agl = 0.0; - if (material) - *material = 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 ) ) { - // Compute the vector from pt to the intersection point ... - sgdVec3 off; - sgdSubVec3(off, isecpoint, pt); - // ... and check if it is too high or not - if (-max_altoff < sgdScalarProductVec3( off, dir )) { - // 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) { - 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; - sgdVec3 dstToContact; - sgdSubVec3(dstToContact, contact, dpt); - *agl = sgdScalarProductVec3(dir, dstToContact); - if (material) - *material = triangle.material; + 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; + + 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. - sgdVec3 dstToContact; - sgdSubVec3(dstToContact, contact, dpt); - *agl = sgdScalarProductVec3(dir, dstToContact); - *type = FGInterface::Unknown; - - 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 caught 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 != NULL); +} + +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; - 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; } -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; - - // 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; }