X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=4bbdfef82e58e26ef96ba11962daac0069b551f8;hb=3ec74d79c23347add2afa088b05ad29af975f65f;hp=3a944a42b5fda8007664d217ca30333621e22baf;hpb=6a4b7302b05924cb8ce23ffbf201847543439544;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 3a944a42b..4bbdfef82 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -16,18 +16,30 @@ // // 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 +#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include #include
#include @@ -37,372 +49,657 @@ #include "flight.hxx" #include "groundcache.hxx" -FGGroundCache::FGGroundCache() +static inline bool +fgdRayTriangle(SGVec3d& x, const SGVec3d& point, const SGVec3d& dir, + const SGVec3d v[3]) { - 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; + double eps = 1e-4; + // Method based on the observation that we are looking for a + // point x that can be expressed in terms of the triangle points + // x = p_0 + \mu_1*(p_1 - p_0) + \mu_2*(p_2 - p_0) + // with 0 <= \mu_1, \mu_2 and \mu_1 + \mu_2 <= 1. + // OTOH it could be expressed in terms of the ray + // x = point + \lambda*dir + // Now we can compute \mu_i and \lambda. + // define + SGVec3d d1 = v[1] - v[0]; + SGVec3d d2 = v[2] - v[0]; + SGVec3d b = point - v[0]; + + // the vector in normal direction, but not normalized + SGVec3d d1crossd2 = cross(d1, d2); + + double denom = -dot(dir, d1crossd2); + double signDenom = copysign(1, denom); + // return if paralell ??? FIXME what if paralell and in plane? + // may be we are ok below than anyway?? + // if (SGMiscd::abs(denom) <= SGLimitsd::min()) + // return false; + + // Now \lambda would read + // lambda = 1/denom*dot(b, d1crossd2); + // To avoid an expensive division we multiply by |denom| + double lambdaDenom = signDenom*dot(b, d1crossd2); + if (lambdaDenom < 0) + return false; + // For line segment we would test against + // if (1 < lambda) + // return false; + // with the original lambda. The multiplied test would read + // if (absDenom < lambdaDenom) + // return false; + + double absDenom = fabs(denom); + double absDenomEps = absDenom*eps; + + SGVec3d bcrossr = cross(b, dir); + // double mu1 = 1/denom*dot(d2, bcrossr); + double mu1 = signDenom*dot(d2, bcrossr); + if (mu1 < -absDenomEps) + return false; + // double mu2 = -1/denom*dot(d1, bcrossr); + // if (mu2 < -eps) + // return false; + double mmu2 = signDenom*dot(d1, bcrossr); + if (mmu2 > absDenomEps) + return false; + + if (mu1 - mmu2 > absDenom + absDenomEps) + return false; + + x = point; + // if we have survived here it could only happen with denom == 0 + // that the point is already in plane. Then return the origin ... + if (SGLimitsd::min() < absDenom) + x += (lambdaDenom/absDenom)*dir; + + return true; } -FGGroundCache::~FGGroundCache() +static inline bool +fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] ) { - cache_root.removeAllKids(); + SGVec3d dif; + + // Some tolerance in meters we accept a point to be outside of the triangle + // and still return that it is inside. + 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) || (point[0] > max) ) + return false; + dif[0] = max - min; + + SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] ); + if( (point[1] < min) || (point[1] > max) ) + return false; + dif[1] = max - min; + + SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] ); + if( (point[2] < min) || (point[2] > max) ) + 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; + } + + // 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); + if ( side1 != side2 ) { + // printf("failed side 1 check\n"); + return false; + } + + // 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); + if ( side1 != side2 ) { + // printf("failed side 2 check\n"); + return false; + } + + // 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); + if ( side1 != side2 ) { + // printf("failed side 3 check\n"); + return false; + } + + return true; } -FGGroundCache::GroundProperty* -FGGroundCache::extractGroundProperty( ssgLeaf* l ) +// 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 SGVec3d& sphereCenter, double radius, + const SGVec3d& pt_on_line, const SGVec3d& dir) { - // 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; + SGVec3d r = sphereCenter - pt_on_line; + double projectedDistance = dot(r, dir); + double dist = dot(r, r) - projectedDistance * projectedDistance; + return dist < radius*radius; +} + +template +class SGExtendedTriangleFunctor : public osg::TriangleFunctor { +public: + // Ok, to be complete we should also implement the indexed variants + // For now this one appears to be enough ... + void drawArrays(GLenum mode, GLint first, GLsizei count) + { + if (_vertexArrayPtr==0 || count==0) return; + + const osg::Vec3* vlast; + const osg::Vec3* vptr; + switch(mode) { + case(GL_LINES): + vlast = &_vertexArrayPtr[first+count]; + for(vptr=&_vertexArrayPtr[first];vptroperator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary); break; - case FGAICarrierHardware::Catapult: - gp->type = FGInterface::Catapult; + case(GL_LINE_STRIP): + vlast = &_vertexArrayPtr[first+count-1]; + for(vptr=&_vertexArrayPtr[first];vptroperator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary); + break; + case(GL_LINE_LOOP): + vlast = &_vertexArrayPtr[first+count-1]; + for(vptr=&_vertexArrayPtr[first];vptroperator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary); + this->operator()(_vertexArrayPtr[first+count-1], + _vertexArrayPtr[first],_treatVertexDataAsTemporary); break; default: - gp->type = FGInterface::Solid; + osg::TriangleFunctor::drawArrays(mode, first, count); break; } - - // Copy the velocity from the carrier class. - ud->carrier->getVelocityWrtEarth( gp->vel ); } +protected: + using osg::TriangleFunctor::_vertexArrayPtr; + using osg::TriangleFunctor::_treatVertexDataAsTemporary; +}; - else { - - // Initialize velocity field. - sgSetVec3( gp->vel, 0.0, 0.0, 0.0 ); - } +class GroundCacheFillVisitor : public osg::NodeVisitor { +public: - // 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(); + /// class to just redirect triangles to the GroundCacheFillVisitor + class GroundCacheFill { + public: + void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv) + { mGroundCacheFillVisitor = gcfv; } - SG_LOG(SG_FLIGHT,SG_INFO, - "New triangle in cache: " << category << " " << file ); + void operator () (const osg::Vec3& v1, const osg::Vec3& v2, + const osg::Vec3& v3, bool) + { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); } + + void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool) + { mGroundCacheFillVisitor->addLine(v1, v2); } - 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; + private: + GroundCacheFillVisitor* mGroundCacheFillVisitor; + }; + + + GroundCacheFillVisitor(FGGroundCache* groundCache, + const SGVec3d& down, + const SGVec3d& cacheReference, + double cacheRadius, + double wireCacheRadius) : + osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN), + mGroundCache(groundCache) + { + setTraversalMask(SG_NODEMASK_TERRAIN_BIT); + mDown = down; + mLocalDown = down; + sphIsec = true; + mBackfaceCulling = false; + mCacheReference = cacheReference; + mLocalCacheReference = cacheReference; + mCacheRadius = cacheRadius; + mWireCacheRadius = wireCacheRadius; + + mTriangleFunctor.setGroundCacheFillVisitor(this); + + mGroundProperty.wire_id = -1; + mGroundProperty.vel = SGVec3d(0, 0, 0); + mGroundProperty.rot = SGVec3d(0, 0, 0); + mGroundProperty.pivot = SGVec3d(0, 0, 0); + } + + void updateCullMode(osg::StateSet* stateSet) + { + if (!stateSet) + return; + + osg::StateAttribute* stateAttribute; + stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE); + if (!stateAttribute) + return; + osg::CullFace* cullFace = static_cast(stateAttribute); + mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK; + } + + bool enterBoundingSphere(const osg::BoundingSphere& bs) + { + if (!bs.valid()) + return false; + + SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal); + double rc = bs.radius() + mCacheRadius; + // Ok, this node might intersect the cache. Visit it in depth. + double centerDist2 = distSqr(mCacheReference, cntr); + if (centerDist2 < rc*rc) { + sphIsec = true; + } else { + // Check if the down direction touches the bounding sphere of the node + // if so, do at least croase agl computations. + // Ther other thing is that we must check if we are in range of + // cats or wires + double rw = bs.radius() + mWireCacheRadius; + if (rw*rw < centerDist2 && + !fgdIsectSphereInfLine(cntr, bs.radius(), mCacheReference, mDown)) + return false; + sphIsec = false; } + + return true; } + + bool enterNode(osg::Node& node) + { + if (!enterBoundingSphere(node.getBound())) + return false; + + updateCullMode(node.getStateSet()); + + FGGroundCache::GroundProperty& gp = mGroundProperty; + // get some material information for use in the gear model + gp.material = globals->get_matlib()->findMaterial(&node); + if (gp.material) { + gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water; + return true; + } + gp.type = FGInterface::Unknown; + osg::Referenced* base = node.getUserData(); + if (!base) + return true; + FGAICarrierHardware *ud = + dynamic_cast(base); + if (!ud) + return true; + + 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); - return gp; -} + return true; + } -// 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 ) ; + void fillWith(osg::Drawable* drawable) + { + bool oldSphIsec = sphIsec; + if (!enterBoundingSphere(drawable->getBound())) + return; - SGfloat projectedDistance = sgScalarProductVec3(r, dir); - - SGfloat dist = sgScalarProductVec3 ( r, r ) - - projectedDistance * projectedDistance; + bool oldBackfaceCulling = mBackfaceCulling; + updateCullMode(drawable->getStateSet()); - SGfloat radius = sp->getRadius(); - return dist < radius*radius; -} + drawable->accept(mTriangleFunctor); -void -FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia, - const sgMat4 xform) -{ - // Extract data from the leaf which is just copied. - ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices(); - ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals(); - // Create a new leaf. - ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 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 ... - GroundProperty *gp = extractGroundProperty( l ); - // Assertation??? - if ( !gp ) { - cerr << "Newly created cache leaf where userdata is not a Ground property!" << endl; + mBackfaceCulling = oldBackfaceCulling; + sphIsec = oldSphIsec; } - vtxa->setUserData( gp ); - vtxa->setCullFace( l->getCullFace() ); - - // Finally append to cache. - cache_root.addKid((ssgEntity*)vtxa); -} -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(); - - ia->add( v[0] ); - ia->add( v[1] ); - } + virtual void apply(osg::Geode& geode) + { + bool oldBackfaceCulling = mBackfaceCulling; + bool oldSphIsec = sphIsec; + FGGroundCache::GroundProperty oldGp = mGroundProperty; + if (!enterNode(geode)) + return; + + for(unsigned i = 0; i < geode.getNumDrawables(); ++i) + fillWith(geode.getDrawable(i)); + sphIsec = oldSphIsec; + mGroundProperty = oldGp; + mBackfaceCulling = oldBackfaceCulling; } - if (!ia) - return; - - addAndFlattenLeaf(GL_LINES, l, ia, xform); -} -void -FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform, - bool sphIsec, sgVec3 down, ssgLeaf *l) -{ - ssgIndexArray *ia = 0; + virtual void apply(osg::Group& group) + { + bool oldBackfaceCulling = mBackfaceCulling; + bool oldSphIsec = sphIsec; + FGGroundCache::GroundProperty oldGp = mGroundProperty; + if (!enterNode(group)) + return; + traverse(group); + sphIsec = oldSphIsec; + mBackfaceCulling = oldBackfaceCulling; + mGroundProperty = oldGp; + } + + virtual void apply(osg::Transform& transform) + { + if (!enterNode(transform)) + return; + bool oldBackfaceCulling = mBackfaceCulling; + bool oldSphIsec = sphIsec; + FGGroundCache::GroundProperty oldGp = mGroundProperty; + /// transform the caches center to local coords + osg::Matrix oldLocalToGlobal = mLocalToGlobal; + osg::Matrix oldGlobalToLocal = mGlobalToLocal; + transform.computeLocalToWorldMatrix(mLocalToGlobal, this); + transform.computeWorldToLocalMatrix(mGlobalToLocal, this); + + SGVec3d oldLocalCacheReference = mLocalCacheReference; + mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal; + SGVec3d oldLocalDown = mLocalDown; + mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal); + + // walk the children + traverse(transform); + + // Restore that one + mLocalDown = oldLocalDown; + mLocalCacheReference = oldLocalCacheReference; + mLocalToGlobal = oldLocalToGlobal; + mGlobalToLocal = oldGlobalToLocal; + sphIsec = oldSphIsec; + mBackfaceCulling = oldBackfaceCulling; + mGroundProperty = oldGp; + } - 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] ); + void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2, + const osg::Vec3& v3) + { + SGVec3d v[3] = { + SGVec3d(v1), + SGVec3d(v2), + SGVec3d(v3) + }; + + // a bounding sphere in the node local system + SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]); +#if 0 + double boundRadius = std::max(norm1(v[0] - boundCenter), + norm1(v[1] - boundCenter)); + boundRadius = std::max(boundRadius, norm1(v[2] - boundCenter)); + // Ok, we take the 1-norm instead of the expensive 2 norm. + // Therefore we need that scaling factor - roughly sqrt(3) + boundRadius = 1.733*boundRadius; +#else + double boundRadius = std::max(distSqr(v[0], boundCenter), + distSqr(v[1], boundCenter)); + boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter)); + boundRadius = sqrt(boundRadius); +#endif + + // if we are not in the downward cylinder bail out + if (!fgdIsectSphereInfLine(boundCenter, boundRadius + mCacheRadius, + mLocalCacheReference, mLocalDown)) + return; + + + // The normal and plane in the node local coordinate system + SGVec3d n = normalize(cross(v[1] - v[0], v[2] - v[0])); + if (0 < dot(mLocalDown, n)) { + if (mBackfaceCulling) { + // Surface points downwards, ignore for altitude computations. + return; + } else { + n = -n; + std::swap(v[1], v[2]); + } + } + + // Only check if the triangle is in the cache sphere if the plane + // containing the triangle is near enough + if (sphIsec && fabs(dot(n, v[0] - mLocalCacheReference)) < mCacheRadius) { + // Check if the sphere around the vehicle intersects the sphere + // around that triangle. If so, put that triangle into the cache. + double r2 = boundRadius + mCacheRadius; + if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) { + FGGroundCache::Triangle t; + for (unsigned i = 0; i < 3; ++i) + t.vertices[i].osg() = v[i].osg()*mLocalToGlobal; + t.boundCenter.osg() = boundCenter.osg()*mLocalToGlobal; + t.boundRadius = boundRadius; + + SGVec3d tmp; + tmp.osg() = osg::Matrixd::transform3x3(n.osg(), mLocalToGlobal); + t.plane = SGVec4d(tmp[0], tmp[1], tmp[2], -dot(tmp, t.vertices[0])); + t.velocity = mGroundProperty.vel; + t.rotation = mGroundProperty.rot; + t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center; + t.type = mGroundProperty.type; + t.material = mGroundProperty.material; + mGroundCache->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 ( 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; + SGVec4d plane = SGVec4d(n[0], n[1], n[2], -dot(n, v[0])); + SGVec3d isectpoint; + + if (fgdRayTriangle(isectpoint, mLocalCacheReference, mLocalDown, v)) { + mGroundCache->found_ground = true; + isectpoint.osg() = isectpoint.osg()*mLocalToGlobal; + isectpoint += mGroundCache->cache_center; + double this_radius = length(isectpoint); + if (mGroundCache->ground_radius < this_radius) { + mGroundCache->ground_radius = this_radius; + mGroundCache->_type = mGroundProperty.type; + mGroundCache->_material = mGroundProperty.material; } } } - if (!ia) - return; - addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform); -} + void addLine(const osg::Vec3& v1, const osg::Vec3& v2) + { + SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal); + SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal); + + SGVec3d boundCenter = 0.5*(gv1 + gv2); + double boundRadius = length(gv1 - boundCenter); + + if (distSqr(boundCenter, mCacheReference) + < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) { + if (mGroundProperty.type == FGInterface::Wire) { + FGGroundCache::Wire wire; + wire.ends[0] = gv1; + wire.ends[1] = gv2; + wire.velocity = mGroundProperty.vel; + wire.rotation = mGroundProperty.rot; + wire.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center; + wire.wire_id = mGroundProperty.wire_id; + + mGroundCache->wires.push_back(wire); + } + if (mGroundProperty.type == FGInterface::Catapult) { + FGGroundCache::Catapult cat; + // Trick to get the ends in the right order. + // Use the x axis in the original coordinate system. Choose the + // most negative x-axis as the one pointing forward + if (v1[0] > v2[0]) { + cat.start = gv1; + cat.end = gv2; + } else { + cat.start = gv2; + cat.end = gv1; + } + cat.velocity = mGroundProperty.vel; + cat.rotation = mGroundProperty.rot; + cat.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center; -// 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 ); + mGroundCache->catapults.push_back(cat); + } + } + } - // Set velocity. - sgdSetVec3( wgs84_vel, gp->vel ); -} + SGExtendedTriangleFunctor mTriangleFunctor; + FGGroundCache* mGroundCache; + SGVec3d mCacheReference; + double mCacheRadius; + double mWireCacheRadius; + osg::Matrix mLocalToGlobal; + osg::Matrix mGlobalToLocal; + SGVec3d mDown; + SGVec3d mLocalDown; + SGVec3d mLocalCacheReference; + bool sphIsec; + bool mBackfaceCulling; + FGGroundCache::GroundProperty mGroundProperty; +}; -void -FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va, - GroundProperty *gp, short i, - sgdVec3 wgs84_pos, sgdVec3 wgs84_vel) +FGGroundCache::FGGroundCache() { - sgVec3 rel_pos; - extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel); - sgdSetVec3( wgs84_pos, rel_pos ); - sgdAddVec3( wgs84_pos, cache_center ); + cache_center = SGVec3d(0, 0, 0); + ground_radius = 0.0; + cache_ref_time = 0.0; + wire_id = 0; + reference_wgs84_point = SGVec3d(0, 0, 0); + reference_vehicle_radius = 0.0; + found_ground = false; } +FGGroundCache::~FGGroundCache() +{ +} -void -FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform, - sgSphere* sp, sgVec3 down, sgSphere* wsp) +inline void +FGGroundCache::velocityTransformTriangle(double dt, + FGGroundCache::Triangle& dst, + const FGGroundCache::Triangle& src) { - // 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); - } + dst = src; + + if (fabs(dt*dot(src.velocity, src.velocity)) < SGLimitsd::epsilon()) + return; + + for (int i = 0; i < 3; ++i) { + SGVec3d pivotoff = src.vertices[i] - src.rotation_pivot; + dst.vertices[i] += dt*(src.velocity + cross(src.rotation, pivotoff)); } + + // Transform the plane equation + SGVec3d pivotoff, vel; + sgdSubVec3(pivotoff.sg(), dst.plane.sg(), src.rotation_pivot.sg()); + vel = src.velocity + cross(src.rotation, pivotoff); + dst.plane[3] += dt*sgdScalarProductVec3(dst.plane.sg(), vel.sg()); + + dst.boundCenter += dt*src.velocity; } bool -FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3], - double rad) +FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, + double rad) { // Empty cache. - cache_root.removeAllKids(); 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_wgs84_point = pt; reference_vehicle_radius = rad; // Store the time reference used to compute movements of moving triangles. cache_ref_time = ref_time; + // Get a normalized down vector valid for the whole cache + SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt)); + down = hlToEc.rotate(SGVec3d(0, 0, 1)); + + // Decide where we put the scenery center. + SGVec3d old_cntr = globals->get_scenery()->get_center(); + SGVec3d cntr(pt); + // Only move the cache center if it is unacceptable far away. + if (40*40 < distSqr(old_cntr, cntr)) + globals->get_scenery()->set_center(cntr); + else + cntr = old_cntr; + // The center of the cache. - sgdCopyVec3(cache_center, pt); + cache_center = cntr; - sgVec3 zero; - sgZeroVec3(zero); // Prepare sphere around the aircraft. - sgSphere acSphere; - acSphere.setRadius(rad); - acSphere.setCenter(zero); + SGVec3d ptoff = pt - cache_center; + double cacheRadius = rad; // 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(zero); - - // 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 need the offset to the scenery scenery center. - sgdVec3 doffset; - Point3D psc = globals->get_tile_mgr()->get_current_center(); - sgdSetVec3(doffset, psc[0], psc[1], psc[2]); - sgdSubVec3(doffset, doffset, pt); - - // 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. - sgVec3 offset; - sgSetVec3(offset, doffset[0], doffset[1], doffset[2]); - sgMat4 xform; - sgMakeTransMat4(xform, offset); - - - // Walk the terrain branch for now. - ssgBranch *terrain = globals->get_scenery()->get_scene_graph(); - cache_fill(terrain, xform, &acSphere, down, &wireSphere); + double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist; + + // Walk the scene graph and extract solid ground triangles and carrier data. + GroundCacheFillVisitor gcfv(this, down, ptoff, cacheRadius, wireCacheRadius); + globals->get_scenery()->get_scene_graph()->accept(gcfv); // some stats - SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad - << ", # leafs = " << cache_root.getNumKids() + 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 @@ -412,21 +709,24 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3], 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; } bool -FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad) +FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) { - sgdCopyVec3(pt, reference_wgs84_point); - *rad = reference_vehicle_radius; - *ref_time = cache_ref_time; + pt = reference_wgs84_point; + rad = reference_vehicle_radius; + ref_time = cache_ref_time; return found_ground; } double -FGGroundCache::get_cat(double t, const double dpt[3], - double end[2][3], double vel[2][3]) +FGGroundCache::get_cat(double t, const SGVec3d& dpt, + SGVec3d end[2], SGVec3d vel[2]) { // start with a distance of 1e10 meters... double dist = 1e10; @@ -434,40 +734,32 @@ FGGroundCache::get_cat(double t, const double dpt[3], // 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 = static_cast(va->getUserData()); - // 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; + size_t sz = catapults.size(); + for (size_t i = 0; i < sz; ++i) { + SGVec3d pivotoff, rvel[2]; + pivotoff = catapults[i].start - catapults[i].rotation_pivot; + rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff); + pivotoff = catapults[i].end - catapults[i].rotation_pivot; + rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff); + + SGVec3d thisEnd[2]; + thisEnd[0] = cache_center + catapults[i].start + t*rvel[0]; + thisEnd[1] = cache_center + catapults[i].end + t*rvel[1]; + + sgdLineSegment3 ls; + sgdCopyVec3(ls.a, thisEnd[0].sg()); + sgdCopyVec3(ls.b, thisEnd[1].sg()); + double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt.sg() ); + + if (this_dist < dist) { + SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult " + << this_dist << " meters away"); + dist = this_dist; - // 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] ); - } + end[0] = thisEnd[0]; + end[1] = thisEnd[1]; + vel[0] = rvel[0]; + vel[1] = rvel[1]; } } @@ -476,104 +768,63 @@ FGGroundCache::get_cat(double t, const double dpt[3], } 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) +FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, + SGVec3d& contact, SGVec3d& normal, SGVec3d& vel, + int *type, const SGMaterial** material, 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 ); + if (material) + *material = 0; + vel = SGVec3d(0, 0, 0); + contact = SGVec3d(0, 0, 0); + normal = SGVec3d(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] ); + SGVec3d pt = dpt - cache_center; + // shift the start of our ray by maxaltoff upwards + SGVec3d raystart = pt - max_altoff*down; // 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; - ssgBase *gpb = va->getUserData(); - // Assertation??? - if ( !gpb ) { - cerr << "Found cache leaf without userdata!" << endl; + 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.boundCenter, triangle.boundRadius, pt, down)) continue; - } - GroundProperty *gp = static_cast(gpb); - - 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 ) ) { - // Only accept surfaces with the normal pointing upwards. - // For double sided surfaces flip the normal in this case. - float dirDot = sgScalarProductVec3(plane, dir); - if ( dirDot >= 0 && va->getCullFace() == 1 ) { - sgScaleVec4( plane, -1 ); - dirDot = -dirDot; - } - // Check for the closest intersection point. - // FIXME: is this the right one? - double newSqdist = sgDistanceSquaredVec3( isecpoint, pt ); - if ( newSqdist < sqdist && dirDot < 0 ) { - 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 ); - // Remormalize that as double, else it *can* have surprising effects - // when used as plane normal together with a 6000000m offset in a - // plane equation. - sgdNormalizeVec3( normal ); - // 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; - } + // Check for intersection. + SGVec3d isecpoint; + if (fgdRayTriangle(isecpoint, raystart, down, triangle.vertices)) { + // Compute the vector from pt to the intersection point ... + SGVec3d off = isecpoint - pt; + // ... and check if it is too high or not + // Transform to the wgs system + isecpoint += cache_center; + // compute the radius, good enough approximation to take the geocentric radius + double radius = dot(isecpoint, isecpoint); + if (current_radius < radius) { + current_radius = radius; + ret = true; + // Save the new potential intersection point. + contact = isecpoint; + // The first three values in the vector are the plane normal. + sgdCopyVec3( normal.sg(), triangle.plane.sg() ); + // The velocity wrt earth. + SGVec3d pivotoff = pt - triangle.rotation_pivot; + vel = triangle.velocity + cross(triangle.rotation, pivotoff); + // Save the ground type. + *type = triangle.type; + *agl = dot(down, contact - dpt); + if (material) + *material = triangle.material; } } } @@ -584,130 +835,95 @@ FGGroundCache::get_agl(double t, const double dpt[3], // 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 ); + double r = length(dpt); + contact = dpt; + contact *= ground_radius/r; + normal = -down; + vel = SGVec3d(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; + *agl = dot(down, contact - dpt); + *type = _type; + if (material) + *material = _material; return ret; } -bool FGGroundCache::caught_wire(double t, const double cpt[4][3]) +bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4]) { - bool ret = false; + size_t sz = wires.size(); + if (sz == 0) + return 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 = static_cast(va->getUserData()); - // 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] ); + // Build the two triangles spanning the area where the hook has moved + // during the past step. + SGVec4d plane[2]; + SGVec3d tri[2][3]; + sgdMakePlane( plane[0].sg(), pt[0].sg(), pt[1].sg(), pt[2].sg() ); + tri[0][0] = pt[0]; + tri[0][1] = pt[1]; + tri[0][2] = pt[2]; + sgdMakePlane( plane[1].sg(), pt[0].sg(), pt[2].sg(), pt[3].sg() ); + tri[1][0] = pt[0]; + tri[1][1] = pt[2]; + 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) { + SGVec3d le[2]; + for (int k = 0; k < 2; ++k) { + le[k] = wires[i].ends[k]; + SGVec3d pivotoff = le[k] - wires[i].rotation_pivot; + SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff); + le[k] += t*vel + cache_center; } - 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] ); - - if ( 0.0 <= isecval && isecval <= 1.0 && - sgPointInTriangle( isecpoint, tri[k] ) ) { - // Store the wire id. - wire_id = gp->wire_id; - ret = true; - } + for (int k=0; k<2; ++k) { + SGVec3d isecpoint; + double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(), + le[1].sg(), plane[k].sg()); + if ( 0.0 <= isecval && isecval <= 1.0 && + fgdPointInTriangle( isecpoint, tri[k] ) ) { + SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire"); + // Store the wire id. + wire_id = wires[i].wire_id; + return true; } } } - return ret; + return false; } -bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3]) +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. + // 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 = static_cast(va->getUserData()); - // 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; + // 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) { + SGVec3d pivotoff = end[k] - wires[i].rotation_pivot; + vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff); + end[k] = cache_center + wires[i].ends[k] + t*vel[k]; + } + return true; + } } - return ret; + return false; } void FGGroundCache::release_wire(void)