X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=4bbdfef82e58e26ef96ba11962daac0069b551f8;hb=3ec74d79c23347add2afa088b05ad29af975f65f;hp=871bb56eaf5ce41fd1fa953415affedac6be55ca;hpb=4df7a3e9f8c1e8b450a3674ec4aab497fda0a514;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 871bb56ea..4bbdfef82 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -16,19 +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 @@ -38,53 +49,94 @@ #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] ; - } +static inline bool +fgdRayTriangle(SGVec3d& x, const SGVec3d& point, const SGVec3d& dir, + const SGVec3d v[3]) +{ + 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; } -static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] ) +static inline bool +fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] ) { - sgdVec3 dif; + SGVec3d 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) ) + 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 - eps) || (point[1] > max + eps) ) + 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 - eps) || (point[2] > max + eps) ) + if( (point[2] < min) || (point[2] > max) ) return false; dif[2] = max - min; @@ -131,8 +183,7 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] ) 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)); + int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn); if ( side1 != side2 ) { // printf("failed side 1 check\n"); return false; @@ -142,8 +193,7 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] ) 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)); + side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn); if ( side1 != side2 ) { // printf("failed side 2 check\n"); return false; @@ -153,8 +203,7 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] ) 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)); + side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn); if ( side1 != side2 ) { // printf("failed side 3 check\n"); return false; @@ -167,51 +216,163 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] ) // 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) +fgdIsectSphereInfLine(const SGVec3d& sphereCenter, double radius, + const SGVec3d& pt_on_line, const SGVec3d& dir) { - sgdVec3 r; - sgdSubVec3( r, sp.getCenter(), pt_on_line ) ; - - SGDfloat projectedDistance = sgdScalarProductVec3(r, dir); - - SGDfloat dist = sgdScalarProductVec3 ( r, r ) - - projectedDistance * projectedDistance; - - SGDfloat radius = sp.getRadius(); + SGVec3d r = sphereCenter - pt_on_line; + double projectedDistance = dot(r, dir); + double dist = dot(r, r) - projectedDistance * projectedDistance; return dist < radius*radius; } -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; -} - -FGGroundCache::~FGGroundCache() -{ -} +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(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: + osg::TriangleFunctor::drawArrays(mode, first, count); + break; + } + } +protected: + using osg::TriangleFunctor::_vertexArrayPtr; + using osg::TriangleFunctor::_treatVertexDataAsTemporary; +}; -FGGroundCache::GroundProperty -FGGroundCache::extractGroundProperty( ssgLeaf* l ) -{ - // FIXME: Do more ... - // Idea: have a get_globals() function which knows about that stuff. - // Or most propably read that from a configuration file, - // from property tree or whatever ... - - // Get ground dependent data. - GroundProperty gp; - gp.wire_id = -1; +class GroundCacheFillVisitor : public osg::NodeVisitor { +public: - FGAICarrierHardware *ud = - dynamic_cast(l->getUserData()); - if (ud) { + /// class to just redirect triangles to the GroundCacheFillVisitor + class GroundCacheFill { + public: + void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv) + { mGroundCacheFillVisitor = gcfv; } + + 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); } + + 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; @@ -224,232 +385,271 @@ FGGroundCache::extractGroundProperty( ssgLeaf* l ) gp.type = FGInterface::Solid; break; } - // Copy the velocity from the carrier class. - ud->carrier->getVelocityWrtEarth( gp.vel ); + ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot); + + return true; } - else { + void fillWith(osg::Drawable* drawable) + { + bool oldSphIsec = sphIsec; + if (!enterBoundingSphere(drawable->getBound())) + return; - // Initialize velocity field. - sgSetVec3( gp.vel, 0.0, 0.0, 0.0 ); - } - - // Get the texture name and decide what ground type we have. - ssgState *st = l->getState(); - if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) { - ssgSimpleState *ss = (ssgSimpleState*)st; - SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" ); - string file = fullPath.file(); - SGPath dirPath(fullPath.dir()); - string category = dirPath.file(); - - if (category == "Runway") - gp.type = FGInterface::Solid; - else { - if (file == "asphault.rgb" || file == "airport.rgb") - gp.type = FGInterface::Solid; - else if (file == "water.rgb" || file == "water-lake.rgb") - gp.type = FGInterface::Water; - else if (file == "forest.rgb" || file == "cropwood.rgb") - gp.type = FGInterface::Forest; - } - } - - return gp; -} + bool oldBackfaceCulling = mBackfaceCulling; + updateCullMode(drawable->getStateSet()); -void -FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform, - ssgLeaf *l) -{ - 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]); - } + drawable->accept(mTriangleFunctor); - if (wsp->intersects( &sphere )) { - if (gp.type == FGInterface::Wire) { - Wire wire; - sgdCopyVec3(wire.ends[0], ends[0]); - sgdCopyVec3(wire.ends[1], ends[1]); - sgdSetVec3(wire.velocity, gp.vel); - wire.wire_id = gp.wire_id; + mBackfaceCulling = oldBackfaceCulling; + sphIsec = oldSphIsec; + } - wires.push_back(wire); - } - if (gp.type == FGInterface::Catapult) { - Catapult cat; - sgdCopyVec3(cat.start, ends[0]); - sgdCopyVec3(cat.end, ends[1]); - sgdSetVec3(cat.velocity, gp.vel); + 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; + } - catapults.push_back(cat); - } - } + 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; } -} -void -FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp, - const sgdMat4 xform, bool sphIsec, - sgdVec3 down, ssgLeaf *l) -{ - GroundProperty gp = extractGroundProperty(l); - - int nt = l->getNumTriangles(); - for (int i = 0; i < nt; ++i) { - Triangle t; - t.sphere.empty(); - short v[3]; - l->getTriangle(i, &v[0], &v[1], &v[2]); - for (int k = 0; k < 3; ++k) { - sgdSetVec3(t.vertices[k], l->getVertex(v[k])); - sgdXformPnt3(t.vertices[k], xform); - t.sphere.extend(t.vertices[k]); - } + 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; + } + + 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; - sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]); - SGDfloat dot = sgdScalarProductVec3(down, t.plane); - if (dot > 0) { - if (!l->getCullFace()) { + + // 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. - continue; - } else - sgdScaleVec4( t.plane, -1 ); + return; + } else { + n = -n; + std::swap(v[1], v[2]); + } } - - // 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)) { - sgdSetVec3(t.velocity, gp.vel); - t.type = gp.type; - triangles.push_back(t); + + // 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 ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) { - sgdVec3 tmp; - sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]); - sgdVec3 isectpoint; - if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) && - fgdPointInTriangle( isectpoint, t.vertices ) ) { - found_ground = true; - sgdAddVec3(isectpoint, cache_center); - double this_radius = sgdLengthVec3(isectpoint); - if (ground_radius < this_radius) - ground_radius = this_radius; + 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; } } } -} - -inline void -FGGroundCache::velocityTransformTriangle(double dt, - FGGroundCache::Triangle& dst, - const FGGroundCache::Triangle& src) -{ - sgdCopyVec3(dst.vertices[0], src.vertices[0]); - sgdCopyVec3(dst.vertices[1], src.vertices[1]); - sgdCopyVec3(dst.vertices[2], src.vertices[2]); - - sgdCopyVec4(dst.plane, src.plane); - sgdCopyVec3(dst.sphere.center, src.sphere.center); - dst.sphere.radius = src.sphere.radius; + 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; - sgdCopyVec3(dst.velocity, src.velocity); + mGroundCache->catapults.push_back(cat); + } + } + } - dst.type = src.type; + 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; +}; - if (dt*sgdLengthSquaredVec3(src.velocity) != 0) { - sgdAddScaledVec3(dst.vertices[0], src.velocity, dt); - sgdAddScaledVec3(dst.vertices[1], src.velocity, dt); - sgdAddScaledVec3(dst.vertices[2], src.velocity, dt); - - dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, src.velocity); +FGGroundCache::FGGroundCache() +{ + 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; +} - sgdAddScaledVec3(dst.sphere.center, src.velocity, dt); - } +FGGroundCache::~FGGroundCache() +{ } -void -FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform, - sgdSphere* sp, sgdVec3 down, sgdSphere* 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; - - // 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; + dst = src; - // For branches collect up the transforms to reach that branch and - // call cache_fill recursively. - if ( e->isAKindOf( ssgTypeBranch() ) ) { - ssgBranch *b = (ssgBranch *)e; - if ( b->isAKindOf( ssgTypeTransform() ) ) { - // Collect up the transfors required to reach that part of - // the branch. - sgMat4 xform2; - sgMakeIdentMat4( xform2 ); - ssgTransform *t = (ssgTransform*)b; - t->getTransform( xform2 ); - sgdMat4 xform3; - fgMultMat4(xform3, xform, xform2); - cache_fill( b, xform3, sp, down, wsp ); - } else - cache_fill( b, xform, sp, down, wsp ); - } - - // For leafs, check each triangle for intersection. - // This will minimize the number of vertices/triangles in the cache. - else if (e->isAKindOf(ssgTypeLeaf())) { - // Since we reach that leaf if we have an intersection with the - // most propably bigger wire/catapult cache sphere, we need to check - // that here, if the smaller cache for the surface has a chance for hits. - // Also, if the spheres do not intersect compute a croase agl value - // by following the line downwards originating at the aircraft. - bool spIsec = sp->intersects(&esphere); - putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e); - - // If we are here, we need to put all special hardware here into - // the cache. - if (wspIsec) - putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e); - } + 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], +FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, double rad) { // Empty cache. @@ -460,61 +660,43 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3], 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. - Point3D old_cntr = globals->get_scenery()->get_center(); - Point3D cntr(pt[0], pt[1], pt[2]); - // Only move the cache center if it is unaccaptable far away. - if (40*40 < old_cntr.distance3Dsquared(cntr)) + 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. - sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]); + cache_center = cntr; - sgdVec3 ptoff; - sgdSubVec3(ptoff, pt, cache_center); // Prepare sphere around the aircraft. - sgdSphere acSphere; - acSphere.setRadius(rad); - acSphere.setCenter(ptoff); + 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; - sgdSphere wireSphere; - wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist); - wireSphere.setCenter(ptoff); - - // Down vector. Is used for croase agl computations when we are far enough - // from ground that we have an empty cache. - sgdVec3 down; - sgdSetVec3(down, -pt[0], -pt[1], -pt[2]); - sgdNormalizeVec3(down); - - // We collaps all transforms we need to reach a particular leaf. - // The leafs itself will be then transformed later. - // So our cache is just flat. - // For leafs which are moving (carriers surface, etc ...) - // we will later store a speed in the GroundType class. We can then apply - // some translations to that nodes according to the time which has passed - // compared to that snapshot. - sgdMat4 xform; - sgdMakeIdentMat4( xform ); - + double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist; // 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); + 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 + SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad << ", # triangles = " << triangles.size() << ", # wires = " << wires.size() << ", # catapults = " << catapults.size() @@ -534,17 +716,17 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3], } 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; @@ -554,27 +736,30 @@ FGGroundCache::get_cat(double t, const double dpt[3], size_t sz = catapults.size(); for (size_t i = 0; i < sz; ++i) { - sgdLineSegment3 ls; - sgdCopyVec3(ls.a, catapults[i].start); - sgdCopyVec3(ls.b, catapults[i].end); + 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); - sgdAddVec3(ls.a, cache_center); - sgdAddVec3(ls.b, cache_center); + 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() ); - sgdAddScaledVec3(ls.a, catapults[i].velocity, t); - sgdAddScaledVec3(ls.b, catapults[i].velocity, 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], catapults[i].velocity ); - sgdCopyVec3( vel[1], catapults[i].velocity ); + end[0] = thisEnd[0]; + end[1] = thisEnd[1]; + vel[0] = rvel[0]; + vel[1] = rvel[1]; } } @@ -583,32 +768,27 @@ FGGroundCache::get_cat(double t, const double dpt[3], } bool -FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff, - double contact[3], double normal[3], double vel[3], - int *type, double *loadCapacity, - double *frictionFactor, double *agl) +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 pt; - sgdSubVec3( pt, dpt, cache_center ); - - // The search direction - sgdVec3 dir; - sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] ); - sgdNormaliseVec3( dir ); + 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 current_radius = 0.0; @@ -617,40 +797,34 @@ FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff, for (size_t i = 0; i < sz; ++i) { Triangle triangle; velocityTransformTriangle(t, triangle, triangles[i]); - if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir)) + if (!fgdIsectSphereInfLine(triangle.boundCenter, triangle.boundRadius, pt, down)) continue; // Check for intersection. - sgdVec3 isecpoint; - if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) && - sgdPointInTriangle( isecpoint, triangle.vertices ) ) { + 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 - sgdAddVec3( isecpoint, cache_center ); + isecpoint += cache_center; // compute the radius, good enough approximation to take the geocentric radius - SGDfloat radius = sgdLengthSquaredVec3(isecpoint); + double radius = dot(isecpoint, isecpoint); if (current_radius < radius) { - // Compute the vector from pt to the intersection point ... - sgdVec3 off; - sgdSubVec3(off, pt, isecpoint); - // ... and check if it is too high or not - if (-max_altoff < sgdScalarProductVec3( off, dir )) { - current_radius = radius; - ret = true; - // Save the new potential intersection point. - sgdCopyVec3( contact, isecpoint ); - // The first three values in the vector are the plane normal. - sgdCopyVec3( normal, triangle.plane ); - // The velocity wrt earth. - /// FIXME: only true for non rotating objects!!!! - sgdCopyVec3( vel, triangle.velocity ); - // Save the ground type. - *type = triangle.type; - // FIXME: figure out how to get that sign ... -// *agl = sqrt(sqdist); - *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); -// *loadCapacity = DBL_MAX; -// *frictionFactor = 1.0; - } + 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; } } } @@ -661,24 +835,23 @@ FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff, // 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 pt[4][3]) +bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4]) { size_t sz = wires.size(); if (sz == 0) @@ -689,35 +862,34 @@ bool FGGroundCache::caught_wire(double t, const double pt[4][3]) // 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] ); + 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 cautght a wire if they intersect. + // You have caught a wire if they intersect. for (size_t i = 0; i < sz; ++i) { - sgdVec3 le[2]; - sgdCopyVec3(le[0], wires[i].ends[0]); - sgdCopyVec3(le[1], wires[i].ends[1]); - - sgdAddVec3(le[0], cache_center); - sgdAddVec3(le[1], cache_center); - - sgdAddScaledVec3(le[0], wires[i].velocity, t); - sgdAddScaledVec3(le[1], wires[i].velocity, t); + 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; + } for (int k=0; k<2; ++k) { - sgdVec3 isecpoint; - double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]); + SGVec3d isecpoint; + double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(), + le[1].sg(), plane[k].sg()); if ( 0.0 <= isecval && isecval <= 1.0 && - sgdPointInTriangle( isecpoint, tri[k] ) ) { + fgdPointInTriangle( isecpoint, tri[k] ) ) { SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire"); // Store the wire id. wire_id = wires[i].wire_id; @@ -729,7 +901,7 @@ bool FGGroundCache::caught_wire(double t, const double pt[4][3]) 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) @@ -742,17 +914,11 @@ bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3]) size_t sz = wires.size(); for (size_t i = 0; i < sz; ++i) { if (wires[i].wire_id == wire_id) { - sgdCopyVec3(end[0], wires[i].ends[0]); - sgdCopyVec3(end[1], wires[i].ends[1]); - - sgdAddVec3(end[0], cache_center); - sgdAddVec3(end[1], cache_center); - - sgdAddScaledVec3(end[0], wires[i].velocity, t); - sgdAddScaledVec3(end[1], wires[i].velocity, t); - - sgdCopyVec3(vel[0], wires[i].velocity); - sgdCopyVec3(vel[1], wires[i].velocity); + 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; } }