+
+bool
+FGInterface::is_valid_m(double *ref_time, double pt[3], double *rad)
+{
+ SGVec3d _pt;
+ bool valid = ground_cache.is_valid(*ref_time, _pt, *rad);
+ assign(pt, _pt);
+ return valid;
+}
+
+bool FGInterface::is_valid_ft(double *ref_time, double pt[3], double *rad)
+{
+ // Convert units and do the real work.
+ SGVec3d _pt;
+ bool found_ground = ground_cache.is_valid(*ref_time, _pt, *rad);
+ assign(pt, SG_METER_TO_FEET*_pt);
+ *rad *= SG_METER_TO_FEET;
+ return found_ground;
+}
+
+double
+FGInterface::get_cat_m(double t, const double pt[3],
+ double end[2][3], double vel[2][3])
+{
+ SGVec3d _end[2], _vel[2];
+ double dist = ground_cache.get_cat(t, SGVec3d(pt), _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], _end[k] );
+ assign( vel[k], _vel[k] );
+ }
+ return dist;
+}
+
+double
+FGInterface::get_cat_ft(double t, const double pt[3],
+ double end[2][3], double vel[2][3])
+{
+ // Convert units and do the real work.
+ SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
+ SGVec3d _end[2], _vel[2];
+ double dist = ground_cache.get_cat(t, pt_m, _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], SG_METER_TO_FEET*_end[k] );
+ assign( vel[k], SG_METER_TO_FEET*_vel[k] );
+ }
+ return dist*SG_METER_TO_FEET;
+}
+
+bool
+FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
+ double bodyToWorld[16], double linearVel[3],
+ double angularVel[3])
+{
+ SGMatrixd _bodyToWorld;
+ SGVec3d _linearVel, _angularVel;
+ if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
+ return false;
+
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ for (unsigned i = 0; i < 16; ++i)
+ bodyToWorld[i] = _bodyToWorld.data()[i];
+
+ return true;
+}
+
+bool
+FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
+{
+ SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
+ SGVec3d _contact, _normal, _linearVel, _angularVel;
+ material = 0;
+ bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+ _angularVel, id, material);
+ // correct the linear velocity, since the line intersector delivers
+ // values for the start point and the get_agl function should
+ // traditionally deliver for the contact point
+ _linearVel += cross(_angularVel, _contact - pt_m);
+
+ assign(contact, _contact);
+ assign(normal, _normal);
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ return ret;
+}
+
+bool
+FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
+{
+ // Convert units and do the real work.
+ SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
+ pt_m *= SG_FEET_TO_METER;
+ SGVec3d _contact, _normal, _linearVel, _angularVel;
+ material = 0;
+ bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+ _angularVel, id, material);
+ // correct the linear velocity, since the line intersector delivers
+ // values for the start point and the get_agl function should
+ // traditionally deliver for the contact point
+ _linearVel += cross(_angularVel, _contact - pt_m);
+
+ // Convert units back ...
+ assign( contact, SG_METER_TO_FEET*_contact );
+ assign( normal, _normal );
+ assign( linearVel, SG_METER_TO_FEET*_linearVel );
+ assign( angularVel, _angularVel );
+ return ret;
+}
+
+bool
+FGInterface::get_nearest_m(double t, const double pt[3], double maxDist,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material,
+ simgear::BVHNode::Id& id)
+{
+ SGVec3d _contact, _linearVel, _angularVel;
+ if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel,
+ _angularVel, id, material))
+ return false;
+
+ assign(contact, _contact);
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ return true;
+}
+
+bool
+FGInterface::get_nearest_ft(double t, const double pt[3], double maxDist,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material,
+ simgear::BVHNode::Id& id)
+{
+ SGVec3d _contact, _linearVel, _angularVel;
+ if (!ground_cache.get_nearest(t, SG_FEET_TO_METER*SGVec3d(pt),
+ SG_FEET_TO_METER*maxDist, _contact, _linearVel,
+ _angularVel, id, material))
+ return false;
+
+ assign(contact, SG_METER_TO_FEET*_contact);
+ assign(linearVel, SG_METER_TO_FEET*_linearVel);
+ assign(angularVel, _angularVel);
+ return true;
+}
+
+double
+FGInterface::get_groundlevel_m(double lat, double lon, double alt)
+{
+ return get_groundlevel_m(SGGeod::fromRadM(lon, lat, alt));
+}
+
+double
+FGInterface::get_groundlevel_m(const SGGeod& geod)
+{
+ // Compute the cartesian position of the given lat/lon/alt.
+ SGVec3d pos = SGVec3d::fromGeod(geod);
+
+ // FIXME: how to handle t - ref_time differences ???
+ SGVec3d cpos;
+ double ref_time = 0, radius;
+ // Prepare the ground cache for that position.
+ if (!is_valid_m(&ref_time, cpos.data(), &radius)) {
+ double startTime = ref_time;
+ double endTime = startTime + 1;
+ bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), 10);
+ /// This is most likely the case when the given altitude is
+ /// too low, try with a new altitude of 10000m, that should be
+ /// sufficient to find a ground level below everywhere on our planet
+ if (!ok) {
+ pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(startTime, endTime, pos.data(), 10))
+ return 0;
+ }
+ } else if (radius*radius <= distSqr(pos, cpos)) {
+ double startTime = ref_time;
+ double endTime = startTime + 1;
+
+ /// We reuse the old radius value, but only if it is at least 10 Meters ..
+ if (!(10 < radius)) // Well this strange compare is nan safe
+ radius = 10;
+
+ bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), radius);
+ /// This is most likely the case when the given altitude is
+ /// too low, try with a new altitude of 10000m, that should be
+ /// sufficient to find a ground level below everywhere on our planet
+ if (!ok) {
+ pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
+ /// If there is still no ground, return sea level radius
+ if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius))
+ return 0;
+ }
+ }
+
+ double contact[3], normal[3], vel[3], angvel[3];
+ const SGMaterial* material;
+ simgear::BVHNode::Id id;
+ // Ignore the return value here, since it just tells us if
+ // the returns stem from the groundcache or from the coarse
+ // computations below the groundcache. The contact point is still something
+ // valid, the normals and the other returns just contain some defaults.
+ get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel,
+ material, id);
+ return SGGeod::fromCart(SGVec3d(contact)).getElevationM();
+}
+
+bool
+FGInterface::caught_wire_m(double t, const double pt[4][3])
+{
+ SGVec3d pt_m[4];
+ for (int i=0; i<4; ++i)
+ pt_m[i] = SGVec3d(pt[i]);
+
+ return ground_cache.caught_wire(t, pt_m);
+}
+
+bool
+FGInterface::caught_wire_ft(double t, const double pt[4][3])
+{
+ // Convert units and do the real work.
+ SGVec3d pt_m[4];
+ for (int i=0; i<4; ++i)
+ pt_m[i] = SG_FEET_TO_METER*SGVec3d(pt[i]);
+
+ return ground_cache.caught_wire(t, pt_m);
+}
+
+bool
+FGInterface::get_wire_ends_m(double t, double end[2][3], double vel[2][3])
+{
+ SGVec3d _end[2], _vel[2];
+ bool ret = ground_cache.get_wire_ends(t, _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], _end[k] );
+ assign( vel[k], _vel[k] );
+ }
+ return ret;
+}
+
+bool
+FGInterface::get_wire_ends_ft(double t, double end[2][3], double vel[2][3])
+{
+ // Convert units and do the real work.
+ SGVec3d _end[2], _vel[2];
+ bool ret = ground_cache.get_wire_ends(t, _end, _vel);
+ for (int k=0; k<2; ++k) {
+ assign( end[k], SG_METER_TO_FEET*_end[k] );
+ assign( vel[k], SG_METER_TO_FEET*_vel[k] );
+ }
+ return ret;
+}
+
+void
+FGInterface::release_wire(void)
+{
+ ground_cache.release_wire();
+}
+