X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FModel.cpp;h=80f7a8dee872f5bfa37608aa9203fce0cdcd5e19;hb=d66903e9ad63b91182ccc25d9bb82f18f8dd98b6;hp=c91fe7df6f2bd72e3ac7f690d27abfc41e3e34b1;hpb=dfffe9c9249cb6ad011fbbb1796733042f97da47;p=flightgear.git diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index c91fe7df6..80f7a8dee 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -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 "Hitch.hpp" #include "Glue.hpp" +#include "Ground.hpp" #include "Model.hpp" namespace yasim { @@ -54,11 +57,28 @@ Model::Model() _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) @@ -79,13 +99,19 @@ void Model::initIteration() 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); + localWind(pos, _s, v, alt); t->setWind(v); t->setAir(_pressure, _temp, _rho); @@ -104,33 +130,31 @@ void Model::initIteration() 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()); + } + + } -// FIXME: This method looks to me like it's doing *integration*, not -// initialization. Integration code should ideally go into -// calcForces. Only very "unstiff" problems can be solved well like -// this (see the engine code for an example); I don't know if rotor -// dynamics qualify or not. -// -Andy +// 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() { - int i; float dt = _integrator.getInterval(); float lrot[3]; + if (!_rotorgear.isInUse()) return; Math::vmul33(_s->orient, _s->rot, lrot); Math::mul3(dt,lrot,lrot); - for(i=0; i<_rotors.size(); i++) { - Rotor* r = (Rotor*)_rotors.get(i); - r->inititeration(dt); - } - for(i=0; i<_rotorparts.size(); i++) { - Rotorpart* rp = (Rotorpart*)_rotorparts.get(i); - rp->inititeration(dt,lrot); - } - for(i=0; i<_rotorblades.size(); i++) { - Rotorblade* rp = (Rotorblade*)_rotorblades.get(i); - rp->inititeration(dt,lrot); - } + _rotorgear.initRotorIteration(lrot,dt); } void Model::iterate() @@ -182,22 +206,24 @@ Surface* Model::getSurface(int handle) return (Surface*)_surfaces.get(handle); } -Rotorpart* Model::getRotorpart(int handle) +Rotorgear* Model::getRotorgear(void) { - return (Rotorpart*)_rotorparts.get(handle); + return &_rotorgear; } -Rotorblade* Model::getRotorblade(int handle) + +int Model::addThruster(Thruster* t) { - return (Rotorblade*)_rotorblades.get(handle); + return _thrusters.add(t); } -Rotor* Model::getRotor(int handle) + +Hook* Model::getHook(void) { - return (Rotor*)_rotors.get(handle); + return _hook; } -int Model::addThruster(Thruster* t) +Launchbar* Model::getLaunchbar(void) { - return _thrusters.add(t); + return _launchbar; } int Model::numThrusters() @@ -220,22 +246,35 @@ int Model::addSurface(Surface* surf) return _surfaces.add(surf); } -int Model::addRotorpart(Rotorpart* rpart) +int Model::addGear(Gear* gear) { - return _rotorparts.add(rpart); + return _gears.add(gear); } -int Model::addRotorblade(Rotorblade* rblade) + +void Model::addHook(Hook* hook) { - return _rotorblades.add(rblade); + _hook = hook; } -int Model::addRotor(Rotor* r) + +void Model::addLaunchbar(Launchbar* launchbar) { - return _rotors.add(r); + _launchbar = launchbar; } -int Model::addGear(Gear* gear) +int Model::addHitch(Hitch* hitch) { - return _gears.add(gear); + 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) @@ -245,17 +284,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) -{ - int i; - for(i=0; i<3; i++) _ground[i] = planeNormal[i]; - _ground[3] = fromOrigin; -} - void Model::setAir(float pressure, float temp, float density) { _pressure = pressure; @@ -268,6 +296,79 @@ 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 @@ -277,7 +378,7 @@ void Model::calcForces(State* s) // step. _body.setGyro(_gyro); _body.addTorque(_torque); - int i; + int i,j; for(i=0; i<_thrusters.size(); i++) { Thruster* t = (Thruster*)_thrusters.get(i); float thrust[3], pos[3]; @@ -286,6 +387,14 @@ void Model::calcForces(State* s) _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); @@ -303,7 +412,7 @@ void Model::calcForces(State* s) // Vsurf = wind - velocity + (rot cross (cg - pos)) float vs[3], pos[3]; sf->getPosition(pos); - localWind(pos, s, vs); + localWind(pos, s, vs, alt); float force[3], torque[3]; sf->calcForce(vs, _rho, force, torque); @@ -312,58 +421,55 @@ void Model::calcForces(State* s) _body.addForce(pos, force); _body.addTorque(torque); } - for(i=0; i<_rotorparts.size(); i++) { - Rotorpart* sf = (Rotorpart*)_rotorparts.get(i); - - // Vsurf = wind - velocity + (rot cross (cg - pos)) - float vs[3], pos[3]; - sf->getPosition(pos); - localWind(pos, s, vs); - - float force[3], torque[3]; - sf->calcForce(vs, _rho, force, torque); - //Math::add3(faero, force, faero); - - sf->getPositionForceAttac(pos); - - _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_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); } - for(i=0; i<_rotorblades.size(); i++) { - Rotorblade* sf = (Rotorblade*)_rotorblades.get(i); - - // Vsurf = wind - velocity + (rot cross (cg - pos)) - float vs[3], pos[3]; - sf->getPosition(pos); - localWind(pos, s, vs); - - float force[3], torque[3]; - sf->calcForce(vs, _rho, force, torque); - //Math::add3(faero, force, faero); - - sf->getPositionForceAttac(pos); - - _body.addForce(pos, force); - _body.addTorque(torque); + if (_rotorgear.isInUse()) + { + float torque[3]; + _rotorgear.calcForces(torque); + _body.addTorque(torque); } - // 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]; - ground[3] = localGround(s, ground); - // Account for ground effect by multiplying the vertical force // component by an amount linear with the fraction of the wingspan // above the ground. - 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); + 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 @@ -375,77 +481,90 @@ 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) { + _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; - 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 - g->getPosition(pos); - g->getCompression(cmpr); - Math::mul3(g->getCompressFraction(), cmpr, cmpr); - Math::add3(cmpr, pos, pos); - float dist = ground[3] - Math::dot3(pos, ground); - - // Find the lowest one - if(dist < min) - min = dist; + 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; } -// 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) +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. - // FIXME: modify turbulence for altitude, attenuating the vertical - // component near the ground. if(_turb) { - double gpos[3]; + 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]; - _turb->getTurbulence(gpos, lwind); + 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); @@ -460,6 +579,15 @@ void Model::localWind(float* pos, State* s, float* out) 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