X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fgroundcache.cxx;h=4bbdfef82e58e26ef96ba11962daac0069b551f8;hb=3ec74d79c23347add2afa088b05ad29af975f65f;hp=ff146e62c0d5a564016409f381269005dc70475a;hpb=9148f9065abcc396e1701a5d6843afd55d46d3a5;p=flightgear.git diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index ff146e62c..4bbdfef82 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -49,6 +49,73 @@ #include "flight.hxx" #include "groundcache.hxx" +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( const SGVec3d& point, const SGVec3d tri[3] ) { @@ -56,21 +123,20 @@ fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] ) // 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; @@ -117,8 +183,7 @@ fgdPointInTriangle( const SGVec3d& point, const SGVec3d 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; @@ -128,8 +193,7 @@ fgdPointInTriangle( const SGVec3d& point, const SGVec3d 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; @@ -139,8 +203,7 @@ fgdPointInTriangle( const SGVec3d& point, const SGVec3d 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; @@ -301,6 +364,7 @@ public: 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; @@ -438,8 +502,10 @@ public: if (mBackfaceCulling) { // Surface points downwards, ignore for altitude computations. return; - } else + } else { n = -n; + std::swap(v[1], v[2]); + } } // Only check if the triangle is in the cache sphere if the plane @@ -462,6 +528,7 @@ public: 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); } } @@ -471,19 +538,16 @@ public: // the whole lifetime of this cache. SGVec4d plane = SGVec4d(n[0], n[1], n[2], -dot(n, v[0])); SGVec3d isectpoint; - if ( sgdIsectInfLinePlane( isectpoint.sg(), mLocalCacheReference.sg(), - mLocalDown.sg(), plane.sg() ) ) { - if (fgdPointInTriangle(isectpoint, v)) { - // Only accept the altitude if the intersection point is below the - // ground cache midpoint - if (0 < dot(isectpoint - mLocalCacheReference, mLocalDown)) { - 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; - } + + 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; } } } @@ -514,7 +578,7 @@ public: // 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]) { + if (v1[0] > v2[0]) { cat.start = gv1; cat.end = gv2; } else { @@ -723,6 +787,8 @@ FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, // The double valued point we start to search for intersection. 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; @@ -736,32 +802,29 @@ FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, // Check for intersection. SGVec3d isecpoint; - if ( sgdIsectInfLinePlane( isecpoint.sg(), pt.sg(), down.sg(), triangle.plane.sg() ) && - fgdPointInTriangle( isecpoint, triangle.vertices ) ) { + 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 - if (-max_altoff < dot(off, down)) { - // 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; - } + // 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; } } } @@ -781,7 +844,9 @@ FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, // The altitude is the distance of the requested point from the // contact point. *agl = dot(down, contact - dpt); - *type = FGInterface::Unknown; + *type = _type; + if (material) + *material = _material; return ret; }