fly/flightgear
1
0
Fork 0
Code Activity

Update groundcache api:

Browse source 
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
This commit is contained in:
frohlich 2009-03-06 16:37:12 +00:00 committed by Tim Moore
parent d2466d605f
commit 55a978f2a8
13 changed files with 335 additions and 233 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

42
src/FDM/JSBSim/JSBSim.cxx
View file

@ -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]));

3
src/FDM/JSBSim/JSBSim.hxx
View file

@ -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;

21
src/FDM/YASim/FGGround.cpp
View file

@ -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];
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];

10
src/FDM/YASim/FGGround.hpp
View file

@ -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]);

33
src/FDM/YASim/Gear.cpp
View file

@ -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;

3
src/FDM/YASim/Gear.hpp
View file

@ -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;

4
src/FDM/YASim/Ground.cpp
View file

@ -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);
}

4
src/FDM/YASim/Ground.hpp
View file

@ -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]);

8
src/FDM/YASim/Model.cpp
View file

@ -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++) {

175
src/FDM/flight.cxx
View file

@ -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],
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();
}

60
src/FDM/flight.hxx
View file

@ -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.

179
src/FDM/groundcache.cxx
View file

@ -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,
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) :

26
src/FDM/groundcache.hxx
View file

@ -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.