-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)
-{
- 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 );
-
- // Time difference to th reference time.
- t -= cache_ref_time;
-
- // The double valued point we start to search for intersection.
- sgdVec3 pt;
- sgdSubVec3( pt, dpt, cache_center );
-
- // The search direction
- sgdVec3 dir;
- sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
- sgdNormaliseVec3( dir );
-
- // Initialize to something sensible
- double current_radius = 0.0;
-
- size_t sz = triangles.size();
- for (size_t i = 0; i < sz; ++i) {
- Triangle triangle;
- velocityTransformTriangle(t, triangle, triangles[i]);
- if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
- continue;
-
- // Check for intersection.
- sgdVec3 isecpoint;
- if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
- sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
- // Transform to the wgs system
- sgdAddVec3( isecpoint, cache_center );
- // compute the radius, good enough approximation to take the geocentric radius
- SGDfloat radius = sgdLengthSquaredVec3(isecpoint);
- if (current_radius < radius) {
- // 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.
- sgdVec3 pivotoff;
- sgdSubVec3(pivotoff, pt, triangle.rotation_pivot);
- sgdVectorProductVec3(vel, triangle.rotation, pivotoff);
- sgdAddVec3(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;