fly/flightgear
1
0
Fork 0
Code Activity

Mathias Frohlich: Add carrier capabilities for YASim aircraft.

Browse source
This commit is contained in:
ehofman 2005-02-17 10:26:14 +00:00
parent 950ceb02f4
commit c2e688244d
13 changed files with 351 additions and 72 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

24
src/FDM/YASim/Airplane.cpp
View file

@ -137,6 +137,16 @@ Gear* Airplane::getGear(int g)
return ((GearRec*)_gears.get(g))->gear;
}
Hook* Airplane::getHook()
{
return _model.getHook();
}
Launchbar* Airplane::getLaunchbar()
{
return _model.getLaunchbar();
}
void Airplane::updateGearState()
{
for(int i=0; i<_gears.size(); i++) {
@ -285,6 +295,16 @@ void Airplane::addGear(Gear* gear)
_gears.add(g);
}
void Airplane::addHook(Hook* hook)
{
_model.addHook(hook);
}
void Airplane::addLaunchbar(Launchbar* launchbar)
{
_model.addLaunchbar(launchbar);
}
void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
{
ThrustRec* t = new ThrustRec();
@ -595,10 +615,6 @@ void Airplane::compileContactPoints()
void Airplane::compile()
{
double ground[3];
ground[0] = 0; ground[1] = 0; ground[2] = 1;
_model.setGroundPlane(ground, -100000);
RigidBody* body = _model.getBody();
int firstMass = body->numMasses();

6
src/FDM/YASim/Airplane.hpp
View file

@ -10,6 +10,8 @@
namespace yasim {
class Gear;
class Hook;
class Launchbar;
class Thruster;
class Airplane {
@ -42,6 +44,8 @@ public:
float taper=1, float mid=0.5);
int addTank(float* pos, float cap, float fuelDensity);
void addGear(Gear* g);
void addHook(Hook* h);
void addLaunchbar(Launchbar* l);
void addThruster(Thruster* t, float mass, float* cg);
void addBallast(float* pos, float mass);
@ -59,6 +63,8 @@ public:
int numGear();
Gear* getGear(int g);
Hook* getHook();
Launchbar* getLaunchbar();
int numThrusters() { return _thrusters.size(); }
Thruster* getThruster(int n) {

34
src/FDM/YASim/BodyEnvironment.hpp
View file

@ -2,6 +2,7 @@
#define _BODYENVIRONMENT_HPP
#include "RigidBody.hpp"
#include "Math.hpp"
namespace yasim {
@ -29,6 +30,39 @@ struct State {
orient[3*i+j] = i==j ? 1.0f : 0.0f;
}
}
void posLocalToGlobal(float* lpos, double *gpos) {
float tmp[3];
Math::tmul33(orient, lpos, tmp);
gpos[0] = tmp[0] + pos[0];
gpos[1] = tmp[1] + pos[1];
gpos[2] = tmp[2] + pos[2];
}
void posGlobalToLocal(double* gpos, float *lpos) {
lpos[0] = gpos[0] - pos[0];
lpos[1] = gpos[1] - pos[1];
lpos[2] = gpos[2] - pos[2];
Math::vmul33(orient, lpos, lpos);
}
void velLocalToGlobal(float* lvel, float *gvel) {
Math::tmul33(orient, lvel, gvel);
}
void velGlobalToLocal(float* gvel, float *lvel) {
Math::vmul33(orient, gvel, lvel);
}
void planeGlobalToLocal(double* gplane, float *lplane) {
// First the normal vector transformed to local coordinates.
lplane[0] = -gplane[0];
lplane[1] = -gplane[1];
lplane[2] = -gplane[2];
Math::vmul33(orient, lplane, lplane);
// Then the distance from the plane to the Aircraft's origin.
lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1]
+ pos[2]*gplane[2] - gplane[3]);
}
};
//

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

@ -4,6 +4,8 @@
#include "PistonEngine.hpp"
#include "TurbineEngine.hpp"
#include "Gear.hpp"
#include "Hook.hpp"
#include "Launchbar.hpp"
#include "Wing.hpp"
#include "Rotor.hpp"
#include "Math.hpp"
@ -198,6 +200,8 @@ void ControlMap::applyControls(float dt)
case BRAKE: ((Gear*)obj)->setBrake(lval); break;
case STEER: ((Gear*)obj)->setRotation(lval); break;
case EXTEND: ((Gear*)obj)->setExtension(lval); break;
case HEXTEND: ((Hook*)obj)->setExtension(lval); break;
case LEXTEND: ((Launchbar*)obj)->setExtension(lval); break;
case CASTERING:((Gear*)obj)->setCastering(lval != 0); break;
case SLAT: ((Wing*)obj)->setSlat(lval); break;
case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break;

36
src/FDM/YASim/FGFDM.cpp
View file

@ -7,6 +7,8 @@
#include "Jet.hpp"
#include "SimpleJet.hpp"
#include "Gear.hpp"
#include "Hook.hpp"
#include "Launchbar.hpp"
#include "Atmosphere.hpp"
#include "PropEngine.hpp"
#include "Propeller.hpp"
@ -46,6 +48,8 @@ static const float NM2FTLB = (1/(LBS2N*FT2M));
FGFDM::FGFDM()
{
_vehicle_radius = 0.0f;
_nextEngine = 0;
// Map /controls/flight/elevator to the approach elevator control. This
@ -231,6 +235,9 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
g->setPosition(v);
float nrm = Math::mag3(v);
if (_vehicle_radius < nrm)
_vehicle_radius = nrm;
v[0] = 0;
v[1] = 0;
v[2] = attrf(a, "compression", 1);
@ -242,7 +249,36 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
g->setDamping(attrf(a, "damp", 1));
_airplane.addGear(g);
} else if(eq(name, "hook")) {
Hook* h = new Hook();
_currObj = h;
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
h->setPosition(v);
float length = attrf(a, "length", 1.0);
h->setLength(length);
float nrm = length+Math::mag3(v);
if (_vehicle_radius < nrm)
_vehicle_radius = nrm;
h->setDownAngle(attrf(a, "down-angle", 70) * DEG2RAD);
h->setUpAngle(attrf(a, "up-angle", 0) * DEG2RAD);
_airplane.addHook(h);
} else if(eq(name, "launchbar")) {
Launchbar* l = new Launchbar();
_currObj = l;
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
l->setLaunchbarMount(v);
v[0] = attrf(a, "holdback-x", v[0]);
v[1] = attrf(a, "holdback-y", v[1]);
v[2] = attrf(a, "holdback-z", v[2]);
l->setHoldbackMount(v);
float length = attrf(a, "length", 1.0);
l->setLength(length);
l->setDownAngle(attrf(a, "down-angle", 30) * DEG2RAD);
l->setUpAngle(attrf(a, "up-angle", -30) * DEG2RAD);
_airplane.addLaunchbar(l);
} else if(eq(name, "fuselage")) {
float b[3];
v[0] = attrf(a, "ax");

5
src/FDM/YASim/FGFDM.hpp
View file

@ -27,6 +27,8 @@ public:
// XML parsing callback from XMLVisitor
virtual void startElement(const char* name, const XMLAttributes &atts);
float getVehicleRadius(void) const { return _vehicle_radius; }
private:
struct AxisRec { char* name; int handle; };
struct EngRec { char* prefix; Thruster* eng; };
@ -72,6 +74,9 @@ private:
// Output properties for the ControlMap
Vector _controlProps;
// Radius of the vehicle, for intersection testing.
float _vehicle_radius;
// Parsing temporaries
void* _currObj;
bool _cruiseCurr;

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

@ -1,4 +1,5 @@
#include "Math.hpp"
#include "BodyEnvironment.hpp"
#include "RigidBody.hpp"
#include "Gear.hpp"
@ -17,6 +18,12 @@ Gear::Gear()
_rot = 0;
_extension = 1;
_castering = false;
_frac = 0;
for(i=0; i<3; i++)
_global_ground[i] = _global_vel[i] = 0;
_global_ground[2] = 1;
_global_ground[3] = -1e3;
}
void Gear::setPosition(float* position)
@ -71,6 +78,13 @@ void Gear::setCastering(bool c)
_castering = c;
}
void Gear::setGlobalGround(double *global_ground, float* global_vel)
{
int i;
for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
}
void Gear::getPosition(float* out)
{
int i;
@ -83,6 +97,12 @@ void Gear::getCompression(float* out)
for(i=0; i<3; i++) out[i] = _cmpr[i];
}
void Gear::getGlobalGround(double* global_ground)
{
int i;
for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
}
float Gear::getSpring()
{
return _spring;
@ -139,7 +159,7 @@ bool Gear::getCastering()
return _castering;
}
void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
{
// Init the return values
int i;
@ -149,7 +169,13 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
if(_extension < 1)
return;
float tmp[3];
// The ground plane transformed to the local frame.
float ground[4];
s->planeGlobalToLocal(_global_ground, ground);
// The velocity of the contact patch transformed to local coordinates.
float glvel[3];
s->velGlobalToLocal(_global_vel, glvel);
// First off, make sure that the gear "tip" is below the ground.
// If it's not, there's no force.
@ -164,6 +190,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
// distance from the base to ground. We can get the fraction
// (0-1) of compression from a/(a-b). Note the minus sign -- stuff
// above ground is negative.
float tmp[3];
Math::add3(_cmpr, _pos, tmp);
float b = ground[3] - Math::dot3(tmp, ground);
@ -182,6 +209,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
float cv[3];
body->pointVelocity(_contact, rot, cv);
Math::add3(cv, v, cv);
Math::sub3(cv, glvel, cv);
// Finally, we can start adding up the forces. First the spring
// compression. (note the clamping of _frac to 1):
@ -226,11 +254,18 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
Math::cross3(skid, gup, steer); // skid cross up == steer
if(_rot != 0) {
// Correct for a (small) rotation
Math::mul3(_rot, steer, tmp);
Math::add3(tmp, skid, skid);
Math::unit3(skid, skid);
Math::cross3(skid, gup, steer);
// Correct for a rotation
float srot = Math::sin(_rot);
float crot = Math::cos(_rot);
float tx = steer[0];
float ty = steer[1];
steer[0] = crot*tx + srot*ty;
steer[1] = -srot*tx + crot*ty;
tx = skid[0];
ty = skid[1];
skid[0] = crot*tx + srot*ty;
skid[1] = -srot*tx + crot*ty;
}
float vsteer = Math::dot3(cv, steer);

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

@ -4,6 +4,7 @@
namespace yasim {
class RigidBody;
class State;
// A landing gear has the following parameters:
//
@ -38,9 +39,11 @@ public:
void setRotation(float rotation);
void setExtension(float extension);
void setCastering(bool castering);
void setGlobalGround(double* global_ground, float* global_vel);
void getPosition(float* out);
void getCompression(float* out);
void getGlobalGround(double* global_ground);
float getSpring();
float getDamping();
float getStaticFriction();
@ -54,7 +57,7 @@ public:
// vector, and a ground plane (all specified in local coordinates)
// and make a force and point of application (i.e. ground contact)
// available via getForce().
void calcForce(RigidBody* body, float* v, float* rot, float* ground);
void calcForce(RigidBody* body, State* s, float* v, float* rot);
// Computed values: total force, weight-on-wheels (force normal to
// ground) and compression fraction.
@ -79,6 +82,8 @@ private:
float _contact[3];
float _wow;
float _frac;
double _global_ground[4];
float _global_vel[3];
};
}; // namespace yasim

5
src/FDM/YASim/Makefile.am
View file

@ -14,6 +14,9 @@ SHARED_SOURCE_FILES = \
FGFDM.cpp FGFDM.hpp \
Gear.cpp Gear.hpp \
Glue.cpp Glue.hpp \
Ground.cpp Ground.hpp \
Hook.cpp Hook.hpp \
Launchbar.cpp Launchbar.hpp \
Integrator.cpp Integrator.hpp \
Jet.cpp Jet.hpp \
Math.cpp Math.hpp \
@ -36,7 +39,7 @@ SHARED_SOURCE_FILES = \
noinst_LIBRARIES = libYASim.a
libYASim_a_SOURCES = YASim.cxx YASim.hxx $(SHARED_SOURCE_FILES)
libYASim_a_SOURCES = YASim.cxx YASim.hxx FGGround.cpp FGGround.hpp $(SHARED_SOURCE_FILES)
bin_PROGRAMS = yasim
noinst_PROGRAMS = proptest

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

@ -6,11 +6,14 @@
#include "Propeller.hpp"
#include "PistonEngine.hpp"
#include "Gear.hpp"
#include "Hook.hpp"
#include "Launchbar.hpp"
#include "Surface.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Rotorblade.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
#include "Model.hpp"
namespace yasim {
@ -54,11 +57,17 @@ Model::Model()
_agl = 0;
_crashed = false;
_turb = 0;
_ground_cb = new Ground();
_hook = 0;
_launchbar = 0;
}
Model::~Model()
{
// FIXME: who owns these things? Need a policy
delete _ground_cb;
delete _hook;
delete _launchbar;
}
void Model::getThrust(float* out)
@ -81,8 +90,9 @@ void Model::initIteration()
_gyro[i] = _torque[i] = 0;
// Need a local altitude for the wind calculation
float dummy[3];
float alt = Math::abs(localGround(_s, dummy));
float lground[4];
_s->planeGlobalToLocal(_global_ground, lground);
float alt = Math::abs(lground[3]);
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
@ -109,6 +119,8 @@ void Model::initIteration()
Math::mul3(_integrator.getInterval(), _wind, toff);
_turb->offset(toff);
}
}
// FIXME: This method looks to me like it's doing *integration*, not
@ -205,6 +217,16 @@ int Model::addThruster(Thruster* t)
return _thrusters.add(t);
}
Hook* Model::getHook(void)
{
return _hook;
}
Launchbar* Model::getLaunchbar(void)
{
return _launchbar;
}
int Model::numThrusters()
{
return _thrusters.size();
@ -243,6 +265,27 @@ int Model::addGear(Gear* gear)
return _gears.add(gear);
}
void Model::addHook(Hook* hook)
{
_hook = hook;
}
void Model::addLaunchbar(Launchbar* launchbar)
{
_launchbar = launchbar;
}
void Model::setGroundCallback(Ground* ground_cb)
{
delete _ground_cb;
_ground_cb = ground_cb;
}
Ground* Model::getGroundCallback(void)
{
return _ground_cb;
}
void Model::setGroundEffect(float* pos, float span, float mul)
{
Math::set3(pos, _wingCenter);
@ -250,16 +293,6 @@ void Model::setGroundEffect(float* pos, float span, float mul)
_groundEffect = mul;
}
// The first three elements are a unit vector pointing from the global
// origin to the plane, the final element is the distance from the
// origin (the radius of the earth, in most implementations). So
// (v dot _ground)-_ground[3] gives the distance AGL.
void Model::setGroundPlane(double* planeNormal, double fromOrigin)
{
for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
_ground[3] = fromOrigin;
}
void Model::setAir(float pressure, float temp, float density)
{
_pressure = pressure;
@ -272,6 +305,54 @@ void Model::setWind(float* wind)
Math::set3(wind, _wind);
}
void Model::updateGround(State* s)
{
float dummy[3];
_ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
int i;
// The landing gear
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
// Get the point of ground contact
float pos[3], cmpr[3];
g->getPosition(pos);
g->getCompression(cmpr);
Math::mul3(g->getCompressFraction(), cmpr, cmpr);
Math::add3(cmpr, pos, pos);
// Transform the local coordinates of the contact point to
// global coordinates.
double pt[3];
s->posLocalToGlobal(pos, pt);
// Ask for the ground plane in the global coordinate system
double global_ground[4];
float global_vel[3];
_ground_cb->getGroundPlane(pt, global_ground, global_vel);
g->setGlobalGround(global_ground, global_vel);
}
// The arrester hook
if(_hook) {
double pt[3];
_hook->getTipGlobalPosition(s, pt);
double global_ground[4];
_ground_cb->getGroundPlane(pt, global_ground, dummy);
_hook->setGlobalGround(global_ground);
}
// The launchbar/holdback
if(_launchbar) {
double pt[3];
_launchbar->getTipGlobalPosition(s, pt);
double global_ground[4];
_ground_cb->getGroundPlane(pt, global_ground, dummy);
_launchbar->setGlobalGround(global_ground);
}
}
void Model::calcForces(State* s)
{
// Add in the pre-computed stuff. These values aren't part of the
@ -295,7 +376,7 @@ void Model::calcForces(State* s)
// from the local origin along that vector to the ground plane
// (negative for objects "above" the ground)
float ground[4];
ground[3] = localGround(s, ground);
s->planeGlobalToLocal(_global_ground, ground);
float alt = Math::abs(ground[3]);
// Gravity, convert to a force, then to local coordinates
@ -380,10 +461,29 @@ void Model::calcForces(State* s)
for(i=0; i<_gears.size(); i++) {
float force[3], contact[3];
Gear* g = (Gear*)_gears.get(i);
g->calcForce(&_body, lv, lrot, ground);
g->calcForce(&_body, s, lv, lrot);
g->getForce(force, contact);
_body.addForce(contact, force);
}
// The arrester hook
if(_hook) {
float v[3], rot[3], glvel[3], ground[3];
_hook->calcForce(_ground_cb, &_body, s, lv, lrot);
float force[3], contact[3];
_hook->getForce(force, contact);
_body.addForce(contact, force);
}
// The launchbar/holdback
if(_launchbar) {
float v[3], rot[3], glvel[3], ground[3];
_launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
float force[3], contact[3];
_launchbar->getForce(force, contact);
_body.addForce(contact, force);
}
}
void Model::newState(State* s)
@ -392,17 +492,22 @@ void Model::newState(State* s)
// Some simple collision detection
float min = 1e8;
float ground[4], pos[3], cmpr[3];
ground[3] = localGround(s, ground);
int i;
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
// Get the point of ground contact
float pos[3], cmpr[3];
g->getPosition(pos);
g->getCompression(cmpr);
Math::mul3(g->getCompressFraction(), cmpr, cmpr);
Math::add3(cmpr, pos, pos);
// The plane transformed to local coordinates.
double global_ground[4];
g->getGlobalGround(global_ground);
float ground[4];
s->planeGlobalToLocal(global_ground, ground);
float dist = ground[3] - Math::dot3(pos, ground);
// Find the lowest one
@ -414,28 +519,6 @@ void Model::newState(State* s)
_crashed = true;
}
// Returns a unit "down" vector for the ground in out, and the
// distance from the local origin to the ground as the return value.
// So for a given position V, "dist - (V dot out)" will be the height
// AGL.
float Model::localGround(State* s, float* out)
{
// Get the ground's "down" vector, this can be in floats, because
// we don't need positioning accuracy. The direction has plenty
// of accuracy after truncation.
out[0] = -(float)_ground[0];
out[1] = -(float)_ground[1];
out[2] = -(float)_ground[2];
Math::vmul33(s->orient, out, out);
// The distance from the ground to the Aircraft's origin:
double dist = (s->pos[0]*_ground[0]
+ s->pos[1]*_ground[1]
+ s->pos[2]*_ground[2] - _ground[3]);
return (float)dist;
}
// Calculates the airflow direction at the given point and for the
// specified aircraft velocity.
void Model::localWind(float* pos, State* s, float* out, float alt)
@ -449,8 +532,8 @@ void Model::localWind(float* pos, State* s, float* out, float alt)
Math::tmul33(s->orient, pos, tmp);
for(int i=0; i<3; i++) {
gpos[i] = s->pos[i] + tmp[i];
up[i] = _ground[i];
}
Glue::geodUp(gpos, up);
_turb->getTurbulence(gpos, alt, up, lwind);
Math::add3(_wind, lwind, lwind);
} else {

19
src/FDM/YASim/Model.hpp
View file

@ -17,6 +17,9 @@ class Rotorpart;
class Rotorblade;
class Rotor;
class Gear;
class Ground;
class Hook;
class Launchbar;
class Model : public BodyEnvironment {
public:
@ -44,11 +47,15 @@ public:
int addRotorblade(Rotorblade* rblade);
int addRotor(Rotor* rotor);
int addGear(Gear* gear);
void addHook(Hook* hook);
void addLaunchbar(Launchbar* launchbar);
Surface* getSurface(int handle);
Rotorpart* getRotorpart(int handle);
Rotorblade* getRotorblade(int handle);
Rotor* getRotor(int handle);
Gear* getGear(int handle);
Hook* getHook(void);
Launchbar* getLaunchbar(void);
// Semi-private methods for use by the Airplane solver.
int numThrusters();
@ -57,14 +64,18 @@ public:
void initIteration();
void getThrust(float* out);
void setGroundCallback(Ground* ground_cb);
Ground* getGroundCallback(void);
//
// Per-iteration settables
//
void setGroundPlane(double* planeNormal, double fromOrigin);
void setGroundEffect(float* pos, float span, float mul);
void setWind(float* wind);
void setAir(float pressure, float temp, float density);
void updateGround(State* s);
// BodyEnvironment callbacks
virtual void calcForces(State* s);
virtual void newState(State* s);
@ -73,7 +84,6 @@ private:
void initRotorIteration();
void calcGearForce(Gear* g, float* v, float* rot, float* ground);
float gearFriction(float wgt, float v, Gear* g);
float localGround(State* s, float* out);
void localWind(float* pos, State* s, float* out, float alt);
Integrator _integrator;
@ -87,12 +97,15 @@ private:
Vector _rotorblades;
Vector _rotors;
Vector _gears;
Hook* _hook;
Launchbar* _launchbar;
float _groundEffectSpan;
float _groundEffect;
float _wingCenter[3];
double _ground[4];
Ground* _ground_cb;
double _global_ground[4];
float _pressure;
float _temp;
float _rho;

59
src/FDM/YASim/YASim.cxx
View file

@ -16,6 +16,9 @@
#include "Integrator.hpp"
#include "Glue.hpp"
#include "Gear.hpp"
#include "Hook.hpp"
#include "Launchbar.hpp"
#include "FGGround.hpp"
#include "PropEngine.hpp"
#include "PistonEngine.hpp"
@ -43,6 +46,7 @@ YASim::YASim(double dt)
_dt = dt;
_fdm->getAirplane()->getModel()->setGroundCallback( new FGGround(this) );
_fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
}
@ -185,12 +189,32 @@ void YASim::update(double dt)
return;
}
// ground. Calculate a cartesian coordinate for the ground under
// us, find the (geodetic) up vector normal to the ground, then
// use that to find the final (radius) term of the plane equation.
float v[3] = { get_uBody()*FT2M, get_vBody()*FT2M, get_wBody()*FT2M };
float lat = get_Latitude(); float lon = get_Longitude();
double xyz[3];
sgGeodToCart(lat, lon, 0.0, xyz);
// build the environment cache.
float vr = _fdm->getVehicleRadius();
vr += 2.0*dt*Math::mag3(v);
prepare_ground_cache_m( 0.0, xyz, vr );
// Track time increments.
FGGround* gr
= (FGGround*)_fdm->getAirplane()->getModel()->getGroundCallback();
int i;
for(i=0; i<iterations; i++) {
gr->setTimeOffset(iterations*_dt);
copyToYASim(false);
_fdm->iterate(_dt);
copyFromYASim();
}
// Reset the time increment.
gr->setTimeOffset(0.0);
}
void YASim::copyToYASim(bool copyState)
@ -209,10 +233,6 @@ void YASim::copyToYASim(bool copyState)
wind[1] = get_V_east_airmass() * FT2M * -1.0;
wind[2] = get_V_down_airmass() * FT2M * -1.0;
// Get ground elevation
double ground = fgGetDouble("/position/ground-elev-m");
// cout << "YASIM: ground = " << ground << endl;
float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA;
float temp = fgGetFloat("/environment/temperature-degc") + 273.15;
float dens = fgGetFloat("/environment/density-slugft3")
@ -279,19 +299,16 @@ void YASim::copyToYASim(bool copyState)
Math::tmul33(xyz2ned, wind, wind);
model->setWind(wind);
// ground. Calculate a cartesian coordinate for the ground under
// us, find the (geodetic) up vector normal to the ground, then
// use that to find the final (radius) term of the plane equation.
double xyz[3], gplane[3]; float up[3];
sgGeodToCart(lat, lon, ground, xyz);
Glue::geodUp(lat, lon, up); // FIXME, needless reverse computation...
int i;
for(i=0; i<3; i++) gplane[i] = up[i];
double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
model->setGroundPlane(gplane, rad);
// air
model->setAir(pressure, temp, dens);
// Query a ground plane for each gear/hook/launchbar and
// write that value into the corresponding class.
_fdm->getAirplane()->getModel()->updateGround(&s);
Launchbar* l = model->getLaunchbar();
if (l)
l->setLaunchCmd(0.0<fgGetFloat("/controls/gear/catapult-launch-cmd"));
}
// All the settables:
@ -443,4 +460,16 @@ void YASim::copyFromYASim()
node->setBoolValue("wow", g->getCompressFraction() != 0);
node->setFloatValue("compression-norm", g->getCompressFraction());
}
Hook* h = airplane->getHook();
if(h) {
SGPropertyNode * node = fgGetNode("gear/tailhook", 0, true);
node->setFloatValue("position-norm", h->getCompressFraction());
}
Launchbar* l = airplane->getLaunchbar();
if(l) {
SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true);
node->setFloatValue("position-norm", l->getCompressFraction());
}
}

14
src/FDM/groundcache.cxx
View file

@ -143,14 +143,16 @@ FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia,
{
// Extract data from the leaf which is just copied.
ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices();
ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals();
// Create a new leaf.
ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 0, 0, ia );
ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 0, 0, ia );
// Clones data ...
vtxa->removeUnusedVertices();
// Apply transform. We won't store transforms in our cache.
vtxa->transform( xform );
// Check for magic texture names object names and such ...
vtxa->setUserData( extractGroundProperty( l ) );
vtxa->setCullFace( l->getCullFace() );
// Finally append to cache.
cache_root.addKid((ssgEntity*)vtxa);
}
@ -528,10 +530,18 @@ FGGroundCache::get_agl(double t, const double dpt[3],
sgVec3 isecpoint;
if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
sgPointInTriangle3( isecpoint, tri ) ) {
// Only accept surfaces with the normal pointing upwards.
// For double sided surfaces flip the normal in this case.
float dirDot = sgScalarProductVec3(plane, dir);
if ( dirDot >= 0 && va->getCullFace() == 1 ) {
sgScaleVec4( plane, -1 );
dirDot = -dirDot;
}
// Check for the closest intersection point.
// FIXME: is this the right one?
double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
if ( newSqdist < sqdist ) {
if ( newSqdist < sqdist && dirDot < 0 ) {
sqdist = newSqdist;
ret = true;
// Save the new potential intersection point.