- 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);
+ }