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
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. */
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;
};
/******************************************************************************/
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();
{
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() );
}
}
+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);
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]));
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;
#include <config.h>
#endif
+#include <simgear/scene/material/mat.hxx>
+
#include <FDM/flight.hxx>
#include "Glue.hpp"
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];
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];
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]);
_extension = 1;
_castering = false;
_frac = 0;
- _ground_type = 0;
_ground_frictionFactor = 1;
_ground_rollingFriction = 0.02;
_ground_loadCapacity = 1e30;
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;
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;
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);
float _reduceFrictionByExtension;
bool _ignoreWhileSolving;
- int _ground_type;
double _ground_frictionFactor;
double _ground_rollingFriction;
double _ground_loadCapacity;
}
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);
}
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]);
// 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++) {
#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>
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();
-
- } else {
- *loadCapacity = DBL_MAX;
- *frictionFactor = 1.0;
- }
- return ret;
+ SGMatrixd _bodyToWorld;
+ SGVec3d _linearVel, _angularVel;
+ if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
+ return false;
+
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ for (unsigned i = 0; i < 16; ++i)
+ bodyToWorld[i] = _bodyToWorld.data()[i];
+
+ return true;
}
bool
-FGInterface::get_agl_m(double t, const double pt[3],
- double 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);
+FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
+{
+ SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
+ SGVec3d _contact, _normal, _linearVel, _angularVel;
+ material = 0;
+ 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 ret;
+ assign(linearVel, _linearVel);
+ assign(angularVel, _angularVel);
+ return true;
}
-// 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)
+FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
+ double contact[3], double normal[3],
+ double linearVel[3], double angularVel[3],
+ SGMaterial const*& material, simgear::BVHNode::Id& id)
{
// Convert units and do the real work.
- SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
+ 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);
- 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;
+ assign( linearVel, SG_METER_TO_FEET*_linearVel );
+ assign( angularVel, _angularVel );
+ 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)
+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, _normal, _vel;
- bool found = ground_cache.get_agl(t, SGVec3d(pt), max_altoff, _contact,
- _normal, _vel, type, material, agl);
+ SGVec3d _contact, _linearVel, _angularVel;
+ if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel,
+ _angularVel, id, material))
+ return false;
+
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)
+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)
{
- // 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);
- // 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;
+ 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
}
}
- 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();
}
// 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.
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.
//
// 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
#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>
#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>
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,
FGGroundCache::FGGroundCache() :
_altitude(0),
- _type(0),
_material(0),
cache_ref_time(0),
_wire(0),
_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);
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??
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) :
}
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);
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 {
// 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) :
// 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,
// 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*
private:
class CacheFill;
+ class BodyFinder;
class CatapultFinder;
class WireIntersector;
class WireFinder;
// 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.
projects / flightgear.git / commitdiff