-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)
-{
- 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 tmp;
- sgdSubVec3( tmp, dpt, cache_center );
- sgVec3 pt;
- sgSetVec3( pt, tmp );
-
- // The search direction
- sgVec3 dir;
- sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
-
- // 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;
- GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
- // Assertation???
- if ( !gp )
- continue;
-
- 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 ) ) {
- // Check for the closest intersection point.
- // FIXME: is this the right one?
- double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
- if ( newSqdist < sqdist ) {
- 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 );
- // 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;