diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index ae53988be..83a8eac66 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/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; - int groundtype; - mInterface->get_agl_ft(t, loc_cart, contact, normal, vel, - &groundtype, &lc, &ff, &agl); + double contact[3], normal[3], vel[3], agl = 0; + mInterface->get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, normal, + vel, &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; - int groundtype; - get_agl_ft(State->Getsim_time(), cart_pos, contact, - dummy, dummy, &groundtype, &lc, &ff, &agl); + double contact[3], d[3], agl; + get_agl_ft(State->Getsim_time(), cart_pos, SG_METER_TO_FEET*2, contact, + d, d, &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])); diff --git a/src/FDM/JSBSim/JSBSim.hxx b/src/FDM/JSBSim/JSBSim.hxx index 89ae03289..084d53d4c 100644 --- a/src/FDM/JSBSim/JSBSim.hxx +++ b/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; diff --git a/src/FDM/YASim/FGGround.cpp b/src/FDM/YASim/FGGround.cpp index d39235cf1..7085f5677 100644 --- a/src/FDM/YASim/FGGround.cpp +++ b/src/FDM/YASim/FGGround.cpp @@ -2,6 +2,8 @@ #include #endif +#include + #include #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]; - int type; - _iface->get_agl_m(_toff, pos, cp, plane, dvel, - &type, &loadCapacity, &frictionFactor, &agl); + double cp[3], dvel[3], dangvel[3]; + const SGMaterial* material; + simgear::BVHNode::Id id; + _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, material, id); // 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]; - _iface->get_agl_m(_toff, pos, cp, plane, dvel, - type, material, &agl); + double cp[3], dvel[3], dangvel[3]; + simgear::BVHNode::Id id; + _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, *material, id); // The plane below the actual contact point. plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2]; diff --git a/src/FDM/YASim/FGGround.hpp b/src/FDM/YASim/FGGround.hpp index 87f125aff..5f5c7e946 100644 --- a/src/FDM/YASim/FGGround.hpp +++ b/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], - int *type, const SGMaterial **material);/* - double *frictionFactor, - double *rollingFriction, - double *loadCapacity, - double *loadResistance, - double *bumpiness, - bool *isSolid);*/ + double plane[4], float vel[3], + const SGMaterial **material); virtual bool caughtWire(const double pos[4][3]); diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp index c741038bd..886a1a989 100644 --- a/src/FDM/YASim/Gear.cpp +++ b/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) { - loadCapacity = DBL_MAX; - frictionFactor = 1.0; - rollingFriction = 0.02; - loadResistance = DBL_MAX; - bumpiness = 0.0; - 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; - } + // no material, assume solid + loadCapacity = DBL_MAX; + frictionFactor = 1.0; + rollingFriction = 0.02; + loadResistance = DBL_MAX; + bumpiness = 0.0; + isSolid = true; } - _ground_type = type; _ground_frictionFactor = frictionFactor; _ground_rollingFriction = rollingFriction; _ground_loadCapacity = loadCapacity; diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp index c3b82cba5..62da887b8 100644 --- a/src/FDM/YASim/Gear.hpp +++ b/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; diff --git a/src/FDM/YASim/Ground.cpp b/src/FDM/YASim/Ground.cpp index 5cafd00fe..32313f343 100644 --- a/src/FDM/YASim/Ground.cpp +++ b/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], - int *type, const SGMaterial **material) + double plane[4], float vel[3], + const SGMaterial **material) { getGroundPlane(pos,plane,vel); } diff --git a/src/FDM/YASim/Ground.hpp b/src/FDM/YASim/Ground.hpp index 4fbcf7400..8b9045357 100644 --- a/src/FDM/YASim/Ground.hpp +++ b/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], - int *type, const SGMaterial **material); + double plane[4], float vel[3], + const SGMaterial **material); virtual bool caughtWire(const double pos[4][3]); diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index e328c6c5a..80f7a8dee 100644 --- a/src/FDM/YASim/Model.cpp +++ b/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, - &type,&material); - static int h=0; - g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], - type,material); + _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material); + g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material); } for(i=0; i<_hitches.size(); i++) { diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index f1b88d365..e749b76fd 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -29,7 +29,6 @@ #include #include #include -#include #include #include @@ -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], - double contact[3], double normal[3], double vel[3], - int *type, double *loadCapacity, - double *frictionFactor, double *agl) +FGInterface::get_body_m(double t, simgear::BVHNode::Id id, + double bodyToWorld[16], double linearVel[3], + double angularVel[3]) { - const SGMaterial* material; - 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); - if (material) { - *loadCapacity = material->get_load_resistance(); - *frictionFactor = material->get_friction_factor(); + SGMatrixd _bodyToWorld; + SGVec3d _linearVel, _angularVel; + if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id)) + return false; - } else { - *loadCapacity = DBL_MAX; - *frictionFactor = 1.0; - } - return ret; -} + assign(linearVel, _linearVel); + assign(angularVel, _angularVel); + for (unsigned i = 0; i < 16; ++i) + bodyToWorld[i] = _bodyToWorld.data()[i]; -bool -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; + return true; } bool FGInterface::get_agl_m(double t, const double pt[3], double max_altoff, - double contact[3], double normal[3], double vel[3], - int *type, const SGMaterial** material, double *agl) + double contact[3], double normal[3], + double linearVel[3], double angularVel[3], + SGMaterial const*& material, simgear::BVHNode::Id& id) { - SGVec3d _contact, _normal, _vel; - bool found = ground_cache.get_agl(t, SGVec3d(pt), max_altoff, _contact, - _normal, _vel, type, material, agl); + SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down(); + SGVec3d _contact, _normal, _linearVel, _angularVel; + 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); - return found; + assign(linearVel, _linearVel); + 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], - int *type, const SGMaterial** material, double *agl) + 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 = SG_FEET_TO_METER*SGVec3d(pt); - SGVec3d _contact, _normal, _vel; - bool ret = ground_cache.get_agl(t, pt_m, SG_FEET_TO_METER * max_altoff, - _contact, _normal, _vel, - type, material, agl); + SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down(); + pt_m *= SG_FEET_TO_METER; + SGVec3d _contact, _normal, _linearVel, _angularVel; + 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); + // 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 ret; + assign( linearVel, SG_METER_TO_FEET*_linearVel ); + 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; - int type; + 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, &type, 0, &agl); + get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel, + material, id); return SGGeod::fromCart(SGVec3d(contact)).getElevationM(); } diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index cd953167e..658835f55 100644 --- a/src/FDM/flight.hxx +++ b/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 - // Search for the nearest triangle to pt. - // Return ground properties like the ground type, 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 - // 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 orientation and position matrix and the linear and angular + // velocity of that local coordinate systems origin for a given time and + // body id. The velocities are in the wgs84 frame at the bodys origin. + bool get_body_m(double t, simgear::BVHNode::Id id, double bodyToWorld[16], + double linearVel[3], double angularVel[3]); + // Return the altitude above ground below the wgs84 point pt - // Search for the nearest triangle to pt. - // Return ground properties like the ground type, a pointer to the - // material and finally the altitude above ground. + // Search for the nearest triangle to pt in downward direction. + // Return ground properties. The velocities are in the wgs84 frame at the + // contact point. bool get_agl_m(double t, const double pt[3], double max_altoff, - double contact[3], double normal[3], double vel[3], - int *type, const SGMaterial** material, double *agl); + double contact[3], double normal[3], double linearVel[3], + 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], - int *type, const SGMaterial** material, double *agl); + double contact[3], double normal[3], double linearVel[3], + 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. diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 9932aa681..b55cd81d3 100644 --- a/src/FDM/groundcache.cxx +++ b/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 #include #include -#include #include #include -#include #include #include @@ -56,6 +54,7 @@ #include #include #include +#include #include
#include @@ -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, - SGVec3d& contact, SGVec3d& normal, SGVec3d& vel, - int *type, const SGMaterial** material, double *agl) +FGGroundCache::get_agl(double t, const SGVec3d& pt, SGVec3d& contact, + SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel, + 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); - vel = lineSegmentVisitor.getLinearVelocity(); - // 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 - vel += cross(lineSegmentVisitor.getAngularVelocity(), - contact - line.getStart()); - *type = materialToGroundType(lineSegmentVisitor.getMaterial()); - if (material) - *material = lineSegmentVisitor.getMaterial(); + linearVel = lineSegmentVisitor.getLinearVelocity(); + angularVel = lineSegmentVisitor.getAngularVelocity(); + material = lineSegmentVisitor.getMaterial(); + id = lineSegmentVisitor.getId(); 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); - *type = _type; - if (material) - *material = _material; + linearVel = SGVec3d(0, 0, 0); + angularVel = SGVec3d(0, 0, 0); + 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) : diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx index ea8cc16d7..c548679a2 100644 --- a/src/FDM/groundcache.hxx +++ b/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 - // and finally the altitude above ground. - bool get_agl(double t, const SGVec3d& pt, double max_altoff, - SGVec3d& contact, SGVec3d& normal, SGVec3d& vel, - int *type, const SGMaterial** material, double *agl); + // 0 and 1 which can be used to model lower friction with wet runways. + bool get_agl(double t, const SGVec3d& pt, SGVec3d& contact, + SGVec3d& normal, SGVec3d& linearVel, SGVec3d& angularVel, + simgear::BVHNode::Id& id, const SGMaterial*& material); + + 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.