fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/FDM/YASim/Model.cpp
frohlich 55a978f2a8 Update groundcache api: 
Make leaner interfaces to the groundcache.
Remove legacy interfaces.
Update users of them.
Add new query routines for 'nearest point' and 'body with given id'.

Modified Files:
	src/FDM/flight.cxx src/FDM/flight.hxx src/FDM/groundcache.cxx
	src/FDM/groundcache.hxx src/FDM/JSBSim/JSBSim.cxx
	src/FDM/JSBSim/JSBSim.hxx src/FDM/YASim/FGGround.cpp
	src/FDM/YASim/FGGround.hpp src/FDM/YASim/Gear.cpp
	src/FDM/YASim/Gear.hpp src/FDM/YASim/Ground.cpp
	src/FDM/YASim/Ground.hpp src/FDM/YASim/Model.cpp
2009-03-11 23:14:33 +01:00

593 lines
15 KiB
C++
Raw Blame History

#include "Atmosphere.hpp"
#include "Thruster.hpp"
#include "Math.hpp"
#include "RigidBody.hpp"
#include "Integrator.hpp"
#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 "Hitch.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
#include "Model.hpp"
namespace yasim {
#if 0
void printState(State* s)
{
State tmp = *s;
Math::vmul33(tmp.orient, tmp.v, tmp.v);
Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
printf("\nNEW STATE (LOCAL COORDS)\n");
printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
printf("o: ");
int i;
for(i=0; i<3; i++) {
if(i != 0) printf(" ");
printf("%6.2f %6.2f %6.2f\n",
tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
}
printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
}
#endif
Model::Model()
{
int i;
for(i=0; i<3; i++) _wind[i] = 0;
_integrator.setBody(&_body);
_integrator.setEnvironment(this);
// Default value of 30 Hz
_integrator.setInterval(1.0f/30.0f);
_agl = 0;
_crashed = false;
_turb = 0;
_ground_cb = new Ground();
_hook = 0;
_launchbar = 0;
_groundEffectSpan = 0;
_groundEffect = 0;
for(i=0; i<3; i++) _wingCenter[i] = 0;
_global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
_global_ground[3] = -100000;
}
Model::~Model()
{
// FIXME: who owns these things? Need a policy
delete _ground_cb;
delete _hook;
delete _launchbar;
for(int i=0; i<_hitches.size();i++)
delete (Hitch*)_hitches.get(i);
}
void Model::getThrust(float* out)
{
float tmp[3];
out[0] = out[1] = out[2] = 0;
int i;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
t->getThrust(tmp);
Math::add3(tmp, out, out);
}
}
void Model::initIteration()
{
// Precompute torque and angular momentum for the thrusters
int i;
for(i=0; i<3; i++)
_gyro[i] = _torque[i] = 0;
// Need a local altitude for the wind calculation
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);
// Get the wind velocity at the thruster location
float pos[3], v[3];
t->getPosition(pos);
localWind(pos, _s, v, alt);
t->setWind(v);
t->setAir(_pressure, _temp, _rho);
t->integrate(_integrator.getInterval());
t->getTorque(v);
Math::add3(v, _torque, _torque);
t->getGyro(v);
Math::add3(v, _gyro, _gyro);
}
// Displace the turbulence coordinates according to the local wind.
if(_turb) {
float toff[3];
Math::mul3(_integrator.getInterval(), _wind, toff);
_turb->offset(toff);
}
for(i=0; i<_hitches.size(); i++) {
Hitch* h = (Hitch*)_hitches.get(i);
h->integrate(_integrator.getInterval());
}
}
// This function initializes some variables for the rotor calculation
// Furthermore it integrates in "void Rotorpart::inititeration
// (float dt,float *rot)" the "rotor orientation" by omega*dt for the
// 3D-visualization of the heli only. and it compensates the rotation
// of the fuselage. The rotor does not follow the rotation of the fuselage.
// Therefore its rotation must be subtracted from the orientation of the
// rotor.
// Maik
void Model::initRotorIteration()
{
float dt = _integrator.getInterval();
float lrot[3];
if (!_rotorgear.isInUse()) return;
Math::vmul33(_s->orient, _s->rot, lrot);
Math::mul3(dt,lrot,lrot);
_rotorgear.initRotorIteration(lrot,dt);
}
void Model::iterate()
{
initIteration();
initRotorIteration();
_body.recalc(); // FIXME: amortize this, somehow
_integrator.calcNewInterval();
}
bool Model::isCrashed()
{
return _crashed;
}
void Model::setCrashed(bool crashed)
{
_crashed = crashed;
}
float Model::getAGL()
{
return _agl;
}
State* Model::getState()
{
return _s;
}
void Model::setState(State* s)
{
_integrator.setState(s);
_s = _integrator.getState();
}
RigidBody* Model::getBody()
{
return &_body;
}
Integrator* Model::getIntegrator()
{
return &_integrator;
}
Surface* Model::getSurface(int handle)
{
return (Surface*)_surfaces.get(handle);
}
Rotorgear* Model::getRotorgear(void)
{
return &_rotorgear;
}
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();
}
Thruster* Model::getThruster(int handle)
{
return (Thruster*)_thrusters.get(handle);
}
void Model::setThruster(int handle, Thruster* t)
{
_thrusters.set(handle, t);
}
int Model::addSurface(Surface* surf)
{
return _surfaces.add(surf);
}
int Model::addGear(Gear* gear)
{
return _gears.add(gear);
}
void Model::addHook(Hook* hook)
{
_hook = hook;
}
void Model::addLaunchbar(Launchbar* launchbar)
{
_launchbar = launchbar;
}
int Model::addHitch(Hitch* hitch)
{
return _hitches.add(hitch);
}
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);
_groundEffectSpan = span;
_groundEffect = mul;
}
void Model::setAir(float pressure, float temp, float density)
{
_pressure = pressure;
_temp = temp;
_rho = density;
}
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];
const SGMaterial* 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++) {
Hitch* h = (Hitch*)_hitches.get(i);
// Get the point of interest
float pos[3];
h->getPosition(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);
h->setGlobalGround(global_ground, global_vel);
}
for(i=0; i<_rotorgear.getRotors()->size(); i++) {
Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
r->findGroundEffectAltitude(_ground_cb,s);
}
// 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
// Runge-Kutta integration (they don't depend on position or
// velocity), and are therefore constant across the four calls to
// calcForces. They get computed before we begin the integration
// step.
_body.setGyro(_gyro);
_body.addTorque(_torque);
int i,j;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
float thrust[3], pos[3];
t->getThrust(thrust);
t->getPosition(pos);
_body.addForce(pos, thrust);
}
// Get a ground plane in local coordinates. The first three
// elements are the normal vector, the final one is the distance
// from the local origin along that vector to the ground plane
// (negative for objects "above" the ground)
float ground[4];
s->planeGlobalToLocal(_global_ground, ground);
float alt = Math::abs(ground[3]);
// Gravity, convert to a force, then to local coordinates
float grav[3];
Glue::geodUp(s->pos, grav);
Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
Math::vmul33(s->orient, grav, grav);
_body.addForce(grav);
// Do each surface, remembering that the local velocity at each
// point is different due to rotation.
float faero[3];
faero[0] = faero[1] = faero[2] = 0;
for(i=0; i<_surfaces.size(); i++) {
Surface* sf = (Surface*)_surfaces.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
localWind(pos, s, vs, alt);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
Math::add3(faero, force, faero);
_body.addForce(pos, force);
_body.addTorque(torque);
}
for (j=0; j<_rotorgear.getRotors()->size();j++)
{
Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
float vs[3], pos[3];
r->getPosition(pos);
localWind(pos, s, vs, alt);
r->calcLiftFactor(vs, _rho,s);
float tq=0;
// total torque of rotor (scalar) for calculating new rotor rpm
for(i=0; i<r->_rotorparts.size(); i++) {
float torque_scalar=0;
Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
rp->getPosition(pos);
localWind(pos, s, vs, alt,true);
float force[3], torque[3];
rp->calcForce(vs, _rho, force, torque, &torque_scalar);
tq+=torque_scalar;
rp->getPositionForceAttac(pos);
_body.addForce(pos, force);
_body.addTorque(torque);
}
r->setTorque(tq);
}
if (_rotorgear.isInUse())
{
float torque[3];
_rotorgear.calcForces(torque);
_body.addTorque(torque);
}
// Account for ground effect by multiplying the vertical force
// component by an amount linear with the fraction of the wingspan
// above the ground.
if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
{
float dist = ground[3] - Math::dot3(ground, _wingCenter);
if(dist > 0 && dist < _groundEffectSpan) {
float fz = Math::dot3(faero, ground);
fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
fz *= _groundEffect;
Math::mul3(fz, ground, faero);
_body.addForce(faero);
}
}
// Convert the velocity and rotation vectors to local coordinates
float lrot[3], lv[3];
Math::vmul33(s->orient, s->rot, lrot);
Math::vmul33(s->orient, s->v, lv);
// The landing gear
for(i=0; i<_gears.size(); i++) {
float force[3], contact[3];
Gear* g = (Gear*)_gears.get(i);
g->calcForce(&_body, s, lv, lrot);
g->getForce(force, contact);
_body.addForce(contact, force);
}
// The arrester hook
if(_hook) {
_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) {
_launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
float forcelb[3], contactlb[3], forcehb[3], contacthb[3];
_launchbar->getForce(forcelb, contactlb, forcehb, contacthb);
_body.addForce(contactlb, forcelb);
_body.addForce(contacthb, forcehb);
}
// The hitches
for(i=0; i<_hitches.size(); i++) {
float force[3], contact[3];
Hitch* h = (Hitch*)_hitches.get(i);
h->calcForce(_ground_cb,&_body, s);
h->getForce(force, contact);
_body.addForce(contact, force);
}
}
void Model::newState(State* s)
{
_s = s;
// Some simple collision detection
float min = 1e8;
int i;
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
if (!g->getSubmergable())
{
// 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
if(dist < min)
min = dist;
}
}
_agl = min;
if(_agl < -1) // Allow for some integration slop
_crashed = true;
}
// 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, bool is_rotor)
{
float tmp[3], lwind[3], lrot[3], lv[3];
// Get a global coordinate for our local position, and calculate
// turbulence.
if(_turb) {
double gpos[3]; float up[3];
Math::tmul33(s->orient, pos, tmp);
for(int i=0; i<3; i++) {
gpos[i] = s->pos[i] + tmp[i];
}
Glue::geodUp(gpos, up);
_turb->getTurbulence(gpos, alt, up, lwind);
Math::add3(_wind, lwind, lwind);
} else {
Math::set3(_wind, lwind);
}
// Convert to local coordinates
Math::vmul33(s->orient, lwind, lwind);
Math::vmul33(s->orient, s->rot, lrot);
Math::vmul33(s->orient, s->v, lv);
_body.pointVelocity(pos, lrot, out); // rotational velocity
Math::mul3(-1, out, out); // (negated)
Math::add3(lwind, out, out); // + wind
Math::sub3(out, lv, out); // - velocity
//add the downwash of the rotors if it is not self a rotor
if (_rotorgear.isInUse()&&!is_rotor)
{
_rotorgear.getDownWash(pos,lv,tmp);
Math::add3(out,tmp, out); // + downwash
}
}
}; // namespace yasim
View git blame Copy permalink