Update groundcache api:

Make leaner interfaces to the groundcache.
Remove legacy interfaces.
Update users of them.
Add new query routines for 'nearest point' and 'body with given id'.

Modified Files:
	src/FDM/flight.cxx src/FDM/flight.hxx src/FDM/groundcache.cxx
	src/FDM/groundcache.hxx src/FDM/JSBSim/JSBSim.cxx
	src/FDM/JSBSim/JSBSim.hxx src/FDM/YASim/FGGround.cpp
	src/FDM/YASim/FGGround.hpp src/FDM/YASim/Gear.cpp
	src/FDM/YASim/Gear.hpp src/FDM/YASim/Ground.cpp
	src/FDM/YASim/Ground.hpp src/FDM/YASim/Model.cpp

src/FDM/JSBSim/JSBSim.cxx

@@ -82,7 +82,7 @@ FMAX (double a, double b)
 
 class FGFSGroundCallback : public FGGroundCallback {
 public:
-  FGFSGroundCallback(FGInterface* ifc) : mInterface(ifc) {}
+  FGFSGroundCallback(FGJSBsim* ifc) : mInterface(ifc) {}
   virtual ~FGFSGroundCallback() {}
 
   /** Get the altitude above sea level depenent on the location. */
@@ -100,17 +100,16 @@ public:
                             FGLocation& cont,
                             FGColumnVector3& n, FGColumnVector3& v) const {
     double loc_cart[3] = { l(eX), l(eY), l(eZ) };
-    double contact[3], normal[3], vel[3], lc, ff, agl;
+    double contact[3], normal[3], vel[3], agl = 0;
-    int groundtype;
+    mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal,
-    mInterface->get_agl_ft(t, loc_cart, contact, normal, vel,
+                           vel, &agl);
-                           &groundtype, &lc, &ff, &agl);
     n = l.GetTec2l()*FGColumnVector3( normal[0], normal[1], normal[2] );
     v = l.GetTec2l()*FGColumnVector3( vel[0], vel[1], vel[2] );
     cont = FGColumnVector3( contact[0], contact[1], contact[2] );
     return agl;
   }
 private:
-  FGInterface* mInterface;
+  FGJSBsim* mInterface;
 };
 
 /******************************************************************************/
@@ -465,10 +464,9 @@ void FGJSBsim::update( double dt )
 
     if ( needTrim ) {
       if ( startup_trim->getBoolValue() ) {
-        double contact[3], dummy[3], lc, ff, agl;
+        double contact[3], d[3], agl;
-        int groundtype;
+        get_agl_ft(State->Getsim_time(), cart_pos, SG_METER_TO_FEET*2, contact,
-        get_agl_ft(State->Getsim_time(), cart_pos, contact,
+                   d, d, &agl);
-                   dummy, dummy, &groundtype, &lc, &ff, &agl);
         double terrain_alt = sqrt(contact[0]*contact[0] + contact[1]*contact[1]
              + contact[2]*contact[2]) - fgic->GetSeaLevelRadiusFtIC();
 
@@ -709,9 +707,8 @@ bool FGJSBsim::copy_from_JSBsim()
     {
       double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) };
       double contact[3], d[3], sd, t;
-      int id;
       is_valid_m(&t, d, &sd);
-      get_agl_ft(t, loc_cart, contact, d, d, &id, &sd, &sd, &sd);
+      get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, d, d, &sd);
       double rwrad
         = FGColumnVector3( contact[0], contact[1], contact[2] ).Magnitude();
       _set_Runway_altitude( rwrad - get_Sea_level_radius() );
@@ -1169,6 +1166,23 @@ void FGJSBsim::update_ic(void)
    }
 }
 
+bool
+FGJSBsim::get_agl_ft(double t, const double pt[3], double alt_off,
+                     double contact[3], double normal[3], double vel[3],
+                     double *agl)
+{
+   double angularVel[3];
+   const SGMaterial* material;
+   simgear::BVHNode::Id id;
+   if (!FGInterface::get_agl_ft(t, pt, alt_off, contact, normal, vel,
+                                angularVel, material, id))
+       return false;
+   SGGeod geodPt = SGGeod::fromCart(SG_FEET_TO_METER*SGVec3d(pt));
+   SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
+   *agl = dot(hlToEc.rotate(SGVec3d(0, 0, 1)), SGVec3d(contact) - SGVec3d(pt));
+   return true;
+}
+
 inline static double dot3(const FGColumnVector3& a, const FGColumnVector3& b)
 {
     return a(1) * b(1) + a(2) * b(2) + a(3) * b(3);
@@ -1241,12 +1255,10 @@ void FGJSBsim::update_external_forces(double t_off)
     double contact[3];
     double ground_normal[3];
     double ground_vel[3];
-    int ground_type;
-    const SGMaterial* ground_material;
     double root_agl_ft;
 
     if (!got_wire) {
-        bool got = get_agl_ft(t_off, hook_area[1], 0, contact, ground_normal, ground_vel, &ground_type, &ground_material, &root_agl_ft);
+        bool got = get_agl_ft(t_off, hook_area[1], 0, contact, ground_normal, ground_vel, &root_agl_ft);
         if (got && root_agl_ft > 0 && root_agl_ft < hook_length) {
             FGColumnVector3 ground_normal_body = Tl2b * (Tec2l * FGColumnVector3(ground_normal[0], ground_normal[1], ground_normal[2]));
             FGColumnVector3 contact_body = Tl2b * Location.LocationToLocal(FGColumnVector3(contact[0], contact[1], contact[2]));

src/FDM/JSBSim/JSBSim.hxx

@@ -207,6 +207,9 @@ public:
     void do_trim(void);
     void update_ic(void);
 
+    bool get_agl_ft(double t, const double pt[3], double alt_off,
+                    double contact[3], double normal[3], double vel[3],
+                    double *agl);
 private:
     JSBSim::FGFDMExec *fdmex;
     JSBSim::FGInitialCondition *fgic;

src/FDM/YASim/FGGround.cpp

@@ -2,6 +2,8 @@
   #include <config.h>
 #endif
 
+#include <simgear/scene/material/mat.hxx>
+
 #include <FDM/flight.hxx>
 
 #include "Glue.hpp"
@@ -23,11 +25,10 @@ void FGGround::getGroundPlane(const double pos[3],
                               double plane[4], float vel[3])
 {
     // Return values for the callback.
-    double loadCapacity, frictionFactor, agl;
+    double cp[3], dvel[3], dangvel[3];
-    double cp[3], dvel[3];
+    const SGMaterial* material;
-    int type;
+    simgear::BVHNode::Id id;
-    _iface->get_agl_m(_toff, pos, cp, plane, dvel,
+    _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, material, id);
-                      &type, &loadCapacity, &frictionFactor, &agl);
 
     // The plane below the actual contact point.
     plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];
@@ -37,14 +38,12 @@ void FGGround::getGroundPlane(const double pos[3],
 
 void FGGround::getGroundPlane(const double pos[3],
                               double plane[4], float vel[3],
-                              int *type, const SGMaterial **material
+                              const SGMaterial **material)
-                              )
 {
     // Return values for the callback.
-    double agl;
+    double cp[3], dvel[3], dangvel[3];
-    double cp[3], dvel[3];
+    simgear::BVHNode::Id id;
-    _iface->get_agl_m(_toff, pos, cp, plane, dvel,
+    _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, *material, id);
-                      type, material, &agl);
 
     // The plane below the actual contact point.
     plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];

src/FDM/YASim/FGGround.hpp

@@ -20,14 +20,8 @@ public:
                                 double plane[4], float vel[3]);
 
     virtual void getGroundPlane(const double pos[3],
-                              double plane[4], float vel[3],
+                                double plane[4], float vel[3],
-                              int *type, const SGMaterial **material);/*
+                                const SGMaterial **material);
-                              double *frictionFactor, 
-                              double *rollingFriction,
-                              double *loadCapacity,
-                              double *loadResistance,
-                              double *bumpiness,
-                              bool *isSolid);*/
 
     virtual bool caughtWire(const double pos[4][3]);
 

src/FDM/YASim/Gear.cpp

@@ -25,7 +25,6 @@ Gear::Gear()
     _extension = 1;
     _castering = false;
     _frac = 0;
-    _ground_type = 0;
     _ground_frictionFactor = 1;
     _ground_rollingFriction = 0.02;
     _ground_loadCapacity = 1e30;
@@ -142,7 +141,7 @@ void Gear::setInitialLoad(float l)
 
 void Gear::setGlobalGround(double *global_ground, float* global_vel,
                            double globalX, double globalY,
-                           int type, const SGMaterial *material)
+                           const SGMaterial *material)
 {
     int i;
     double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness;
@@ -159,30 +158,14 @@ void Gear::setGlobalGround(double *global_ground, float* global_vel,
         bumpiness = (*material).get_bumpiness();
         isSolid = (*material).get_solid();
     } else {
-        if (type == FGInterface::Solid) {
+        // no material, assume solid
-            loadCapacity = DBL_MAX;
+        loadCapacity = DBL_MAX;
-            frictionFactor = 1.0;
+        frictionFactor = 1.0;
-            rollingFriction = 0.02;
+        rollingFriction = 0.02;
-            loadResistance = DBL_MAX;
+        loadResistance = DBL_MAX;
-            bumpiness = 0.0;
+        bumpiness = 0.0;
-            isSolid = true;
+        isSolid = true;
-        } else if (type == FGInterface::Water) {
-            loadCapacity = DBL_MAX;
-            frictionFactor = 1.0;
-            rollingFriction = 2;
-            loadResistance = DBL_MAX;
-            bumpiness = 0.8;
-            isSolid = false;
-        } else {
-            loadCapacity = DBL_MAX;
-            frictionFactor = 0.9;
-            rollingFriction = 0.1;
-            loadResistance = DBL_MAX;
-            bumpiness = 0.2;
-            isSolid = true;
-        }
     }
-    _ground_type = type;
     _ground_frictionFactor = frictionFactor;
     _ground_rollingFriction = rollingFriction;
     _ground_loadCapacity = loadCapacity;

src/FDM/YASim/Gear.hpp

@@ -50,7 +50,7 @@ public:
     void setIgnoreWhileSolving(bool c);
     void setGlobalGround(double* global_ground, float* global_vel,
         double globalX, double globalY,
-        int type, const SGMaterial *material);
+        const SGMaterial *material);
     void getPosition(float* out);
     void getCompression(float* out);
     void getGlobalGround(double* global_ground);
@@ -117,7 +117,6 @@ private:
     float _reduceFrictionByExtension;
     bool _ignoreWhileSolving;
 
-    int _ground_type;
     double _ground_frictionFactor;
     double _ground_rollingFriction;
     double _ground_loadCapacity;

src/FDM/YASim/Ground.cpp

@@ -30,8 +30,8 @@ void Ground::getGroundPlane(const double pos[3],
 }
 
 void Ground::getGroundPlane(const double pos[3],
-                              double plane[4], float vel[3],
+                            double plane[4], float vel[3],
-                              int *type, const SGMaterial **material)
+                            const SGMaterial **material)
 {
     getGroundPlane(pos,plane,vel);
 }

src/FDM/YASim/Ground.hpp

@@ -13,8 +13,8 @@ public:
                                 double plane[4], float vel[3]);
 
     virtual void getGroundPlane(const double pos[3],
-                              double plane[4], float vel[3],
+                                double plane[4], float vel[3],
-                              int *type, const SGMaterial **material);
+                                const SGMaterial **material);
 
     virtual bool caughtWire(const double pos[4][3]);
 

src/FDM/YASim/Model.cpp

@@ -321,13 +321,9 @@ void Model::updateGround(State* s)
         // Ask for the ground plane in the global coordinate system
         double global_ground[4];
         float global_vel[3];
-        int type;
         const SGMaterial* material;
-        _ground_cb->getGroundPlane(pt, global_ground, global_vel,
+        _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material);
-                              &type,&material);
+        g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material);
-        static int h=0;
-        g->setGlobalGround(global_ground, global_vel, pt[0], pt[1],
-            type,material);
     }
 
     for(i=0; i<_hitches.size(); i++) {

src/FDM/flight.cxx

@@ -29,7 +29,6 @@
 #include <simgear/constants.h>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/SGMath.hxx>
-#include <simgear/scene/material/mat.hxx>
 #include <simgear/timing/timestamp.hxx>
 
 #include <Scenery/scenery.hxx>
@@ -705,108 +704,110 @@ FGInterface::get_cat_ft(double t, const double pt[3],
   return dist*SG_METER_TO_FEET;
 }
 
-// Legacy interface just kept because of JSBSim
 bool
-FGInterface::get_agl_m(double t, const double pt[3],
+FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
-                       double contact[3], double normal[3], double vel[3],
+                        double bodyToWorld[16], double linearVel[3],
-                       int *type, double *loadCapacity,
+                        double angularVel[3])
-                       double *frictionFactor, double *agl)
 {
-  const SGMaterial* material;
+  SGMatrixd _bodyToWorld;
-  SGVec3d _contact, _normal, _vel;
+  SGVec3d _linearVel, _angularVel;
-  bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal,
+  if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
-                                  _vel, type, &material, agl);
+    return false;
-  assign(contact, _contact);
-  assign(normal, _normal);
-  assign(vel, _vel);
-  if (material) {
-    *loadCapacity = material->get_load_resistance();
-    *frictionFactor = material->get_friction_factor();
 
-  } else {
+  assign(linearVel, _linearVel);
-    *loadCapacity = DBL_MAX;
+  assign(angularVel, _angularVel);
-    *frictionFactor = 1.0;
+  for (unsigned i = 0; i < 16; ++i)
-  }
+      bodyToWorld[i] = _bodyToWorld.data()[i];
-  return ret;
-}
 
-bool
+  return true;
-FGInterface::get_agl_m(double t, const double pt[3],
-                       double contact[3], double normal[3], double vel[3],
-                       int *type, const SGMaterial **material, double *agl)
-{
-  SGVec3d _contact, _normal, _vel;
-  bool ret = ground_cache.get_agl(t, SGVec3d(pt), 2.0, _contact, _normal,
-                                  _vel, type, material, agl);
-  assign(contact, _contact);
-  assign(normal, _normal);
-  assign(vel, _vel);
-  return ret;
-}
-
-// Legacy interface just kept because of JSBSim
-bool
-FGInterface::get_agl_ft(double t, const double pt[3],
-                        double contact[3], double normal[3], double vel[3],
-                        int *type, double *loadCapacity,
-                        double *frictionFactor, double *agl)
-{
-  // Convert units and do the real work.
-  SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
-
-  const SGMaterial* material;
-  SGVec3d _contact, _normal, _vel;
-  bool ret = ground_cache.get_agl(t, pt_m, 2.0, _contact, _normal, _vel,
-                                  type, &material, agl);
-  // Convert units back ...
-  assign( contact, SG_METER_TO_FEET*_contact );
-  assign( vel, SG_METER_TO_FEET*_vel );
-  assign( normal, _normal );
-  *agl *= SG_METER_TO_FEET;
-
-  // return material properties if available
-  if (material) {
-    // FIXME: convert units?? now pascal to lbf/ft^2
-    *loadCapacity = 0.020885434*material->get_load_resistance();
-    *frictionFactor = material->get_friction_factor();
-  } else {
-    *loadCapacity = DBL_MAX;
-    *frictionFactor = 1.0;
-  }
-  return ret;
 }
 
 bool
 FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
-                       double contact[3], double normal[3], double vel[3],
+                       double contact[3], double normal[3],
-                       int *type, const SGMaterial** material, double *agl)
+                       double linearVel[3], double angularVel[3],
+                       SGMaterial const*& material, simgear::BVHNode::Id& id)
 {
-  SGVec3d _contact, _normal, _vel;
+  SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
-  bool found = ground_cache.get_agl(t, SGVec3d(pt), max_altoff, _contact,
+  SGVec3d _contact, _normal, _linearVel, _angularVel;
-                                    _normal, _vel, type, material, agl);
+  material = 0;
+  if (!ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+                            _angularVel, id, material))
+      return false;
+  // 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(vel, _vel);
+  assign(linearVel, _linearVel);
-  return found;
+  assign(angularVel, _angularVel);
+  return true;
 }
 
 bool
 FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
-                        double contact[3], double normal[3], double vel[3],
+                        double contact[3], double normal[3],
-                        int *type, const SGMaterial** material, double *agl)
+                        double linearVel[3], double angularVel[3],
+                        SGMaterial const*& material, simgear::BVHNode::Id& id)
 {
   // Convert units and do the real work.
-  SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
+  SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
-  SGVec3d _contact, _normal, _vel;
+  pt_m *= SG_FEET_TO_METER;
-  bool ret = ground_cache.get_agl(t, pt_m, SG_FEET_TO_METER * max_altoff,
+  SGVec3d _contact, _normal, _linearVel, _angularVel;
-                                  _contact, _normal, _vel,
+  material = 0;
-                                  type, material, agl);
+  if (!ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
+                            _angularVel, id, material))
+      return false;
+  // 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( vel, SG_METER_TO_FEET*_vel );
   assign( normal, _normal );
-  *agl *= SG_METER_TO_FEET;
+  assign( linearVel, SG_METER_TO_FEET*_linearVel );
-  return ret;
+  assign( angularVel, _angularVel );
+  return true;
+}
+
+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
@@ -853,13 +854,15 @@ FGInterface::get_groundlevel_m(const SGGeod& geod)
     }
   }
   
-  double contact[3], normal[3], vel[3], agl;
+  double contact[3], normal[3], vel[3], angvel[3];
-  int type;
+  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, &type, 0, &agl);
+  get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel,
+            material, id);
   return SGGeod::fromCart(SGVec3d(contact)).getElevationM();
 }
   

src/FDM/flight.hxx

@@ -600,14 +600,6 @@ public:
     // Ground handling routines
     //////////////////////////////////////////////////////////////////////////
 
-    enum GroundType {
-      /// I sused at least in YAsim. Deprecate this ad get it from the
-      /// Material instead
-      Unknown = 0, //??
-      Solid, // Whatever we will roll on with infinite load factor.
-      Water // For the beaver ...
-    };
-
     // Prepare the ground cache for the wgs84 position pt_*.
     // That is take all vertices in the ball with radius rad around the
     // position given by the pt_* and store them in a local scene graph.
@@ -631,38 +623,42 @@ public:
                       double end[2][3], double vel[2][3]);
   
 
-    // Return the altitude above ground below the wgs84 point pt
+    // Return the orientation and position matrix and the linear and angular
-    // Search for the nearest triangle to pt.
+    // velocity of that local coordinate systems origin for a given time and
-    // Return ground properties like the ground type, the maximum load
+    // body id. The velocities are in the wgs84 frame at the bodys origin.
-    // this kind kind of ground can carry, the friction factor between
+    bool get_body_m(double t, simgear::BVHNode::Id id, double bodyToWorld[16],
-    // 0 and 1 which can be used to model lower friction with wet runways
+                    double linearVel[3], double angularVel[3]);
-    // and finally the altitude above ground.
+
-    bool get_agl_m(double t, const double pt[3],
-                   double contact[3], double normal[3], double vel[3],
-                   int *type, double *loadCapacity,
-                   double *frictionFactor, double *agl);
-    bool get_agl_m(double t, const double pt[3],
-                       double contact[3], double normal[3], double vel[3],
-                       int *type, const SGMaterial **material,double *agl);
-    bool get_agl_ft(double t, const double pt[3],
-                    double contact[3], double normal[3], double vel[3],
-                    int *type, double *loadCapacity,
-                    double *frictionFactor, double *agl);
 
     // Return the altitude above ground below the wgs84 point pt
-    // Search for the nearest triangle to pt.
+    // Search for the nearest triangle to pt in downward direction.
-    // Return ground properties like the ground type, a pointer to the
+    // Return ground properties. The velocities are in the wgs84 frame at the
-    // material and finally the altitude above ground.
+    // contact point.
     bool get_agl_m(double t, const double pt[3], double max_altoff,
-                   double contact[3], double normal[3], double vel[3],
+                   double contact[3], double normal[3], double linearVel[3],
-                   int *type, const SGMaterial** material, double *agl);
+                   double angularVel[3], SGMaterial const*& material,
+                   simgear::BVHNode::Id& id);
     bool get_agl_ft(double t, const double pt[3], double max_altoff,
-                    double contact[3], double normal[3], double vel[3],
+                    double contact[3], double normal[3], double linearVel[3],
-                    int *type, const SGMaterial** material, double *agl);
+                    double angularVel[3], SGMaterial const*& material,
+                    simgear::BVHNode::Id& id);
     double get_groundlevel_m(double lat, double lon, double alt);
     double get_groundlevel_m(const SGGeod& geod);
 
 
+    // Return the nearest point in any direction to the point pt with a maximum
+    // distance maxDist. The velocities are in the wgs84 frame at the query
+    // position pt.
+    bool 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);
+    bool 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);
+
+
     // Return 1 if the hook intersects with a wire.
     // That test is done by checking if the quad spanned by the points pt*
     // intersects with the line representing the wire.

src/FDM/groundcache.cxx

@@ -2,7 +2,7 @@
 //
 // Written by Mathias Froehlich, started Nov 2004.
 //
-// Copyright (C) 2004  Mathias Froehlich - Mathias.Froehlich@web.de
+// Copyright (C) 2004, 2009  Mathias Froehlich - Mathias.Froehlich@web.de
 //
 // This program is free software; you can redistribute it and/or
 // modify it under the terms of the GNU General Public License as
@@ -39,10 +39,8 @@
 #include <simgear/constants.h>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
-#include <simgear/scene/material/mat.hxx>
 #include <simgear/scene/util/SGNodeMasks.hxx>
 #include <simgear/scene/util/SGSceneUserData.hxx>
-#include <simgear/scene/model/placementtrans.hxx>
 
 #include <simgear/scene/bvh/BVHNode.hxx>
 #include <simgear/scene/bvh/BVHGroup.hxx>
@@ -56,6 +54,7 @@
 #include <simgear/scene/bvh/BVHStaticBinary.hxx>
 #include <simgear/scene/bvh/BVHSubTreeCollector.hxx>
 #include <simgear/scene/bvh/BVHLineSegmentVisitor.hxx>
+#include <simgear/scene/bvh/BVHNearestPointVisitor.hxx>
 
 #include <Main/globals.hxx>
 #include <Scenery/scenery.hxx>
@@ -66,16 +65,6 @@
 
 using namespace simgear;
 
-static FGInterface::GroundType
-materialToGroundType(const SGMaterial* material)
-{
-    if (!material)
-        return FGInterface::Solid;
-    if (material->get_solid())
-        return FGInterface::Solid;
-    return FGInterface::Water;
-}
-
 class FGGroundCache::CacheFill : public osg::NodeVisitor {
 public:
     CacheFill(const SGVec3d& center, const double& radius,
@@ -232,7 +221,6 @@ private:
 
 FGGroundCache::FGGroundCache() :
     _altitude(0),
-    _type(0),
     _material(0),
     cache_ref_time(0),
     _wire(0),
@@ -284,7 +272,7 @@ FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
     _localBvhTree = subtreeCollector.getBVHNode();
 
     // Try to get a croase altitude value for the ground cache
-    SGLineSegmentd line(pt, pt + 1e4*down);
+    SGLineSegmentd line(pt, pt + 2*reference_vehicle_radius*down);
     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time);
     if (_localBvhTree)
         _localBvhTree->accept(lineSegmentVisitor);
@@ -294,13 +282,11 @@ FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
         SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint());
         _altitude = geodPt.getElevationM();
         _material = lineSegmentVisitor.getMaterial();
-        _type = materialToGroundType(_material);
         found_ground = true;
     } else {
         // Else do a crude scene query for the current point
         found_ground = globals->get_scenery()->
             get_cart_elevation_m(pt, rad, _altitude, &_material);
-        _type = materialToGroundType(_material);
     }
     
     // Still not sucessful??
@@ -320,6 +306,105 @@ FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
     return found_ground;
 }
 
+class FGGroundCache::BodyFinder : public BVHVisitor {
+public:
+    BodyFinder(BVHNode::Id id, const double& t) :
+        _id(id),
+        _bodyToWorld(SGMatrixd::unit()),
+        _linearVelocity(0, 0, 0),
+        _angularVelocity(0, 0, 0),
+        _time(t)
+    { }
+    
+    virtual void apply(BVHGroup& leaf)
+    {
+        if (_foundId)
+            return;
+        leaf.traverse(*this);
+    }
+    virtual void apply(BVHTransform& transform)
+    {
+        if (_foundId)
+            return;
+
+        transform.traverse(*this);
+        
+        if (_foundId) {
+            _linearVelocity = transform.vecToWorld(_linearVelocity);
+            _angularVelocity = transform.vecToWorld(_angularVelocity);
+            _bodyToWorld = transform.getToWorldTransform()*_bodyToWorld;
+        }
+    }
+    virtual void apply(BVHMotionTransform& transform)
+    {
+        if (_foundId)
+            return;
+
+        if (_id == transform.getId()) {
+            _foundId = true;
+            return;
+        }
+        
+        transform.traverse(*this);
+        
+        if (_foundId) {
+            SGMatrixd toWorld = transform.getToWorldTransform(_time);
+            SGVec3d referencePoint = _bodyToWorld.xformPt(SGVec3d::zeros());
+            _linearVelocity += transform.getLinearVelocityAt(referencePoint);
+            _angularVelocity += transform.getAngularVelocity();
+            _linearVelocity = toWorld.xformVec(_linearVelocity);
+            _angularVelocity = toWorld.xformVec(_angularVelocity);
+            _bodyToWorld = toWorld*_bodyToWorld;
+        }
+    }
+    virtual void apply(BVHLineGeometry& node) { }
+    virtual void apply(BVHStaticGeometry& node) { }
+    
+    virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { }
+    virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { }
+    
+    const SGMatrixd& getBodyToWorld() const
+    { return _bodyToWorld; }
+    const SGVec3d& getLinearVelocity() const
+    { return _linearVelocity; }
+    const SGVec3d& getAngularVelocity() const
+    { return _angularVelocity; }
+    
+    bool empty() const
+    { return !_foundId; }
+    
+protected:
+    simgear::BVHNode::Id _id;
+
+    SGMatrixd _bodyToWorld;
+
+    SGVec3d _linearVelocity;
+    SGVec3d _angularVelocity;
+    
+    bool _foundId;
+
+    double _time;
+};
+
+bool
+FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
+                        SGVec3d& angularVel, simgear::BVHNode::Id id)
+{
+    // Get the transform matrix and velocities of a moving body with id at t.
+    if (!_localBvhTree)
+        return false;
+    BodyFinder bodyFinder(id, t);
+    _localBvhTree->accept(bodyFinder);
+    if (bodyFinder.empty())
+        return false;
+
+    bodyToWorld = bodyFinder.getBodyToWorld();
+    linearVel = bodyFinder.getLinearVelocity();
+    angularVel = bodyFinder.getAngularVelocity();
+
+    return true;
+}
+
 class FGGroundCache::CatapultFinder : public BVHVisitor {
 public:
     CatapultFinder(const SGSphered& sphere, const double& t) :
@@ -456,12 +541,12 @@ FGGroundCache::get_cat(double t, const SGVec3d& pt,
 }
 
 bool
-FGGroundCache::get_agl(double t, const SGVec3d& pt, double max_altoff,
+FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
-                       SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
+                       SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
-                       int *type, const SGMaterial** material, double *agl)
+                       simgear::BVHNode::Id& id, const SGMaterial*& material)
 {
     // Just set up a ground intersection query for the given point
-    SGLineSegmentd line(pt - max_altoff*down, pt + 1e4*down);
+    SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
     simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
     if (_localBvhTree)
         _localBvhTree->accept(lineSegmentVisitor);
@@ -472,16 +557,10 @@ FGGroundCache::get_agl(double t, const SGVec3d& pt, double max_altoff,
         normal = lineSegmentVisitor.getNormal();
         if (0 < dot(normal, down))
             normal = -normal;
-        *agl = dot(down, contact - pt);
+        linearVel = lineSegmentVisitor.getLinearVelocity();
-        vel = lineSegmentVisitor.getLinearVelocity();
+        angularVel = lineSegmentVisitor.getAngularVelocity();
-        // correct the linear velocity, since the line intersector delivers
+        material = lineSegmentVisitor.getMaterial();
-        // values for the start point and the get_agl function should
+        id = lineSegmentVisitor.getId();
-        // traditionally deliver for the contact point
-        vel += cross(lineSegmentVisitor.getAngularVelocity(),
-                     contact - line.getStart());
-        *type = materialToGroundType(lineSegmentVisitor.getMaterial());
-        if (material)
-            *material = lineSegmentVisitor.getMaterial();
 
         return true;
     } else {
@@ -489,19 +568,47 @@ FGGroundCache::get_agl(double t, const SGVec3d& pt, double max_altoff,
         // take the ground level we found during the current cache build.
         // This is as good as what we had before for agl.
         SGGeod geodPt = SGGeod::fromCart(pt);
-        *agl = geodPt.getElevationM() - _altitude;
         geodPt.setElevationM(_altitude);
         contact = SGVec3d::fromGeod(geodPt);
         normal = -down;
-        vel = SGVec3d(0, 0, 0);
+        linearVel = SGVec3d(0, 0, 0);
-        *type = _type;
+        angularVel = SGVec3d(0, 0, 0);
-        if (material)
+        material = _material;
-            *material = _material;
+        id = 0;
 
         return found_ground;
     }
 }
 
+
+bool
+FGGroundCache::get_nearest(double t, const SGVec3d& pt, double maxDist,
+                           SGVec3d& contact, SGVec3d& linearVel,
+                           SGVec3d& angularVel, simgear::BVHNode::Id& id,
+                           const SGMaterial*& material)
+{
+    if (!_localBvhTree)
+        return false;
+
+    // Just set up a ground intersection query for the given point
+    SGSphered sphere(pt, maxDist);
+    simgear::BVHNearestPointVisitor nearestPointVisitor(sphere, t);
+    _localBvhTree->accept(nearestPointVisitor);
+
+    if (nearestPointVisitor.empty())
+        return false;
+
+    // Have geometry in the range of maxDist
+    contact = nearestPointVisitor.getPoint();
+    linearVel = nearestPointVisitor.getLinearVelocity();
+    angularVel = nearestPointVisitor.getAngularVelocity();
+    material = nearestPointVisitor.getMaterial();
+    id = nearestPointVisitor.getId();
+    
+    return true;
+}
+
+
 class FGGroundCache::WireIntersector : public BVHVisitor {
 public:
     WireIntersector(const SGVec3d pt[4], const double& t) :

src/FDM/groundcache.hxx

@@ -58,6 +58,14 @@ public:
     // is valid for are returned.
     bool is_valid(double& ref_time, SGVec3d& pt, double& rad);
 
+    // Returns the unit down vector at the ground cache
+    const SGVec3d& get_down() const
+    { return down; }
+
+
+    bool get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
+                  SGVec3d& angularVel, simgear::BVHNode::Id id);
+
     // Return the nearest catapult to the given point
     // pt in wgs84 coordinates.
     double get_cat(double t, const SGVec3d& pt,
@@ -66,13 +74,16 @@ public:
 
     // Return the altitude above ground below the wgs84 point pt
     // Search for highest triangle not higher than pt + max_altoff.
-    // Return ground properties like the ground type, the maximum load
+    // Return ground properties like the maximum load
     // this kind kind of ground can carry, the friction factor between
-    // 0 and 1 which can be used to model lower friction with wet runways
+    // 0 and 1 which can be used to model lower friction with wet runways.
-    // and finally the altitude above ground.
+    bool get_agl(double t, const SGVec3d& pt, SGVec3d& contact,
-    bool get_agl(double t, const SGVec3d& pt, double max_altoff,
+                 SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel,
-                 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
+                 simgear::BVHNode::Id& id, const SGMaterial*& material);
-                 int *type, const SGMaterial** material, double *agl);
+
+    bool get_nearest(double t, const SGVec3d& pt, double maxDist,
+                     SGVec3d& contact, SGVec3d& linearVel, SGVec3d& angularVel,
+                     simgear::BVHNode::Id& id, const SGMaterial*& material);
 
     // Return 1 if the hook intersects with a wire.
     // That test is done by checking if the quad spanned by the points pt*
@@ -90,6 +101,7 @@ public:
 
 private:
     class CacheFill;
+    class BodyFinder;
     class CatapultFinder;
     class WireIntersector;
     class WireFinder;
@@ -97,8 +109,6 @@ private:
     // Approximate ground radius.
     // In case the aircraft is too high above ground.
     double _altitude;
-    // Ground type
-    int _type;
     // the simgear material reference, contains friction coeficients ...
     const SGMaterial* _material;
     // The time reference for later call to intersection test routines.