src/FDM/YASim/Model.cpp

   1
   2 #ifdef HAVE_CONFIG_H
   3 #  include "config.h"
   4 #endif
   5
   6 #include "Atmosphere.hpp"
   7 #include "Thruster.hpp"
   8 #include "Math.hpp"
   9 #include "RigidBody.hpp"
  10 #include "Integrator.hpp"
  11 #include "Propeller.hpp"
  12 #include "PistonEngine.hpp"
  13 #include "Gear.hpp"
  14 #include "Hook.hpp"
  15 #include "Launchbar.hpp"
  16 #include "Surface.hpp"
  17 #include "Rotor.hpp"
  18 #include "Rotorpart.hpp"
  19 #include "Hitch.hpp"
  20 #include "Glue.hpp"
  21 #include "Ground.hpp"
  22
  23 #include "Model.hpp"
  24 namespace yasim {
  25
  26 #if 0
  27 void printState(State* s)
  28 {
  29     State tmp = *s;
  30     Math::vmul33(tmp.orient, tmp.v, tmp.v);
  31     Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
  32     Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
  33     Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
  34
  35     printf("\nNEW STATE (LOCAL COORDS)\n");
  36     printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
  37     printf("o:   ");
  38     int i;
  39     for(i=0; i<3; i++) {
  40         if(i != 0) printf("     ");
  41         printf("%6.2f %6.2f %6.2f\n",
  42                tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
  43     }
  44     printf("v:   %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
  45     printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
  46     printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
  47     printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
  48 }
  49 #endif
  50
  51 Model::Model()
  52 {
  53     int i;
  54     for(i=0; i<3; i++) _wind[i] = 0;
  55
  56     _integrator.setBody(&_body);
  57     _integrator.setEnvironment(this);
  58
  59     // Default value of 30 Hz
  60     _integrator.setInterval(1.0f/30.0f);
  61
  62     _agl = 0;
  63     _crashed = false;
  64     _turb = 0;
  65     _ground_cb = new Ground();
  66     _hook = 0;
  67     _launchbar = 0;
  68
  69     _groundEffectSpan = 0;
  70     _groundEffect = 0;
  71     for(i=0; i<3; i++) _wingCenter[i] = 0;
  72
  73     _global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
  74     _global_ground[3] = -100000;
  75
  76 }
  77
  78 Model::~Model()
  79 {
  80     // FIXME: who owns these things?  Need a policy
  81     delete _ground_cb;
  82     delete _hook;
  83     delete _launchbar;
  84     for(int i=0; i<_hitches.size();i++)
  85         delete (Hitch*)_hitches.get(i);
  86
  87 }
  88
  89 void Model::getThrust(float* out)
  90 {
  91     float tmp[3];
  92     out[0] = out[1] = out[2] = 0;
  93     int i;
  94     for(i=0; i<_thrusters.size(); i++) {
  95         Thruster* t = (Thruster*)_thrusters.get(i);
  96         t->getThrust(tmp);
  97         Math::add3(tmp, out, out);
  98     }
  99 }
 100
 101 void Model::initIteration()
 102 {
 103     // Precompute torque and angular momentum for the thrusters
 104     int i;
 105     for(i=0; i<3; i++)
 106         _gyro[i] = _torque[i] = 0;
 107
 108     // Need a local altitude for the wind calculation
 109     float lground[4];
 110     _s->planeGlobalToLocal(_global_ground, lground);
 111     float alt = Math::abs(lground[3]);
 112
 113     for(i=0; i<_thrusters.size(); i++) {
 114         Thruster* t = (Thruster*)_thrusters.get(i);
 115
 116         // Get the wind velocity at the thruster location
 117         float pos[3], v[3];
 118         t->getPosition(pos);
 119         localWind(pos, _s, v, alt);
 120
 121         t->setWind(v);
 122         t->setAir(_pressure, _temp, _rho);
 123         t->integrate(_integrator.getInterval());
 124
 125         t->getTorque(v);
 126         Math::add3(v, _torque, _torque);
 127
 128         t->getGyro(v);
 129         Math::add3(v, _gyro, _gyro);
 130     }
 131
 132     // Displace the turbulence coordinates according to the local wind.
 133     if(_turb) {
 134         float toff[3];
 135         Math::mul3(_integrator.getInterval(), _wind, toff);
 136         _turb->offset(toff);
 137     }
 138
 139     for(i=0; i<_hitches.size(); i++) {
 140         Hitch* h = (Hitch*)_hitches.get(i);
 141         h->integrate(_integrator.getInterval());
 142     }
 143
 144
 145 }
 146
 147 // This function initializes some variables for the rotor calculation
 148 // Furthermore it integrates in "void Rotorpart::inititeration
 149 // (float dt,float *rot)" the "rotor orientation" by omega*dt for the
 150 // 3D-visualization of the heli only. and it compensates the rotation
 151 // of the fuselage. The rotor does not follow the rotation of the fuselage.
 152 // Therefore its rotation must be subtracted from the orientation of the
 153 // rotor.
 154 // Maik
 155 void Model::initRotorIteration()
 156 {
 157     float dt = _integrator.getInterval();
 158     float lrot[3];
 159     if (!_rotorgear.isInUse()) return;
 160     Math::vmul33(_s->orient, _s->rot, lrot);
 161     Math::mul3(dt,lrot,lrot);
 162     _rotorgear.initRotorIteration(lrot,dt);
 163 }
 164
 165 void Model::iterate()
 166 {
 167     initIteration();
 168     initRotorIteration();
 169     _body.recalc(); // FIXME: amortize this, somehow
 170     _integrator.calcNewInterval();
 171 }
 172
 173 bool Model::isCrashed()
 174 {
 175     return _crashed;
 176 }
 177
 178 void Model::setCrashed(bool crashed)
 179 {
 180     _crashed = crashed;
 181 }
 182
 183 float Model::getAGL()
 184 {
 185     return _agl;
 186 }
 187
 188 State* Model::getState()
 189 {
 190     return _s;
 191 }
 192
 193 void Model::setState(State* s)
 194 {
 195     _integrator.setState(s);
 196     _s = _integrator.getState();
 197 }
 198
 199 RigidBody* Model::getBody()
 200 {
 201     return &_body;
 202 }
 203
 204 Integrator* Model::getIntegrator()
 205 {
 206     return &_integrator;
 207 }
 208
 209 Surface* Model::getSurface(int handle)
 210 {
 211     return (Surface*)_surfaces.get(handle);
 212 }
 213
 214 Rotorgear* Model::getRotorgear(void)
 215 {
 216     return &_rotorgear;
 217 }
 218
 219 int Model::addThruster(Thruster* t)
 220 {
 221     return _thrusters.add(t);
 222 }
 223
 224 Hook* Model::getHook(void)
 225 {
 226     return _hook;
 227 }
 228
 229 Launchbar* Model::getLaunchbar(void)
 230 {
 231     return _launchbar;
 232 }
 233
 234 int Model::numThrusters()
 235 {
 236     return _thrusters.size();
 237 }
 238
 239 Thruster* Model::getThruster(int handle)
 240 {
 241     return (Thruster*)_thrusters.get(handle);
 242 }
 243
 244 void Model::setThruster(int handle, Thruster* t)
 245 {
 246     _thrusters.set(handle, t);
 247 }
 248
 249 int Model::addSurface(Surface* surf)
 250 {
 251     return _surfaces.add(surf);
 252 }
 253
 254 int Model::addGear(Gear* gear)
 255 {
 256     return _gears.add(gear);
 257 }
 258
 259 void Model::addHook(Hook* hook)
 260 {
 261     _hook = hook;
 262 }
 263
 264 void Model::addLaunchbar(Launchbar* launchbar)
 265 {
 266     _launchbar = launchbar;
 267 }
 268
 269 int Model::addHitch(Hitch* hitch)
 270 {
 271     return _hitches.add(hitch);
 272 }
 273
 274 void Model::setGroundCallback(Ground* ground_cb)
 275 {
 276     delete _ground_cb;
 277     _ground_cb = ground_cb;
 278 }
 279
 280 Ground* Model::getGroundCallback(void)
 281 {
 282     return _ground_cb;
 283 }
 284
 285 void Model::setGroundEffect(float* pos, float span, float mul)
 286 {
 287     Math::set3(pos, _wingCenter);
 288     _groundEffectSpan = span;
 289     _groundEffect = mul;
 290 }
 291
 292 void Model::setAir(float pressure, float temp, float density)
 293 {
 294     _pressure = pressure;
 295     _temp = temp;
 296     _rho = density;
 297 }
 298
 299 void Model::setWind(float* wind)
 300 {
 301     Math::set3(wind, _wind);
 302 }
 303
 304 void Model::updateGround(State* s)
 305 {
 306     float dummy[3];
 307     _ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
 308
 309     int i;
 310     // The landing gear
 311     for(i=0; i<_gears.size(); i++) {
 312         Gear* g = (Gear*)_gears.get(i);
 313
 314         // Get the point of ground contact
 315         float pos[3], cmpr[3];
 316         g->getPosition(pos);
 317         g->getCompression(cmpr);
 318
 319         Math::mul3(g->getCompressFraction(), cmpr, cmpr);
 320         Math::add3(cmpr, pos, pos);
 321         // Transform the local coordinates of the contact point to
 322         // global coordinates.
 323         double pt[3];
 324         s->posLocalToGlobal(pos, pt);
 325
 326         // Ask for the ground plane in the global coordinate system
 327         double global_ground[4];
 328         float global_vel[3];
 329         const simgear::BVHMaterial* material;
 330         _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material);
 331         g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material);
 332     }
 333
 334     for(i=0; i<_hitches.size(); i++) {
 335         Hitch* h = (Hitch*)_hitches.get(i);
 336
 337         // Get the point of interest
 338         float pos[3];
 339         h->getPosition(pos);
 340
 341         // Transform the local coordinates of the contact point to
 342         // global coordinates.
 343         double pt[3];
 344         s->posLocalToGlobal(pos, pt);
 345
 346         // Ask for the ground plane in the global coordinate system
 347         double global_ground[4];
 348         float global_vel[3];
 349         _ground_cb->getGroundPlane(pt, global_ground, global_vel);
 350         h->setGlobalGround(global_ground, global_vel);
 351     }
 352
 353     for(i=0; i<_rotorgear.getRotors()->size(); i++) {
 354         Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
 355         r->findGroundEffectAltitude(_ground_cb,s);
 356     }
 357
 358     // The arrester hook
 359     if(_hook) {
 360         double pt[3];
 361         _hook->getTipGlobalPosition(s, pt);
 362         double global_ground[4];
 363         _ground_cb->getGroundPlane(pt, global_ground, dummy);
 364         _hook->setGlobalGround(global_ground);
 365     }
 366
 367     // The launchbar/holdback
 368     if(_launchbar) {
 369         double pt[3];
 370         _launchbar->getTipGlobalPosition(s, pt);
 371         double global_ground[4];
 372         _ground_cb->getGroundPlane(pt, global_ground, dummy);
 373         _launchbar->setGlobalGround(global_ground);
 374     }
 375 }
 376
 377 void Model::calcForces(State* s)
 378 {
 379     // Add in the pre-computed stuff.  These values aren't part of the
 380     // Runge-Kutta integration (they don't depend on position or
 381     // velocity), and are therefore constant across the four calls to
 382     // calcForces.  They get computed before we begin the integration
 383     // step.
 384     _body.setGyro(_gyro);
 385     _body.addTorque(_torque);
 386     int i,j;
 387     for(i=0; i<_thrusters.size(); i++) {
 388         Thruster* t = (Thruster*)_thrusters.get(i);
 389         float thrust[3], pos[3];
 390         t->getThrust(thrust);
 391         t->getPosition(pos);
 392         _body.addForce(pos, thrust);
 393     }
 394
 395     // Get a ground plane in local coordinates.  The first three
 396     // elements are the normal vector, the final one is the distance
 397     // from the local origin along that vector to the ground plane
 398     // (negative for objects "above" the ground)
 399     float ground[4];
 400     s->planeGlobalToLocal(_global_ground, ground);
 401     float alt = Math::abs(ground[3]);
 402
 403     // Gravity, convert to a force, then to local coordinates
 404     float grav[3];
 405     Glue::geodUp(s->pos, grav);
 406     Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
 407     Math::vmul33(s->orient, grav, grav);
 408     _body.addForce(grav);
 409
 410     // Do each surface, remembering that the local velocity at each
 411     // point is different due to rotation.
 412     float faero[3];
 413     faero[0] = faero[1] = faero[2] = 0;
 414     for(i=0; i<_surfaces.size(); i++) {
 415         Surface* sf = (Surface*)_surfaces.get(i);
 416
 417         // Vsurf = wind - velocity + (rot cross (cg - pos))
 418         float vs[3], pos[3];
 419         sf->getPosition(pos);
 420         localWind(pos, s, vs, alt);
 421
 422         float force[3], torque[3];
 423         sf->calcForce(vs, _rho, force, torque);
 424         Math::add3(faero, force, faero);
 425
 426         _body.addForce(pos, force);
 427         _body.addTorque(torque);
 428     }
 429     for (j=0; j<_rotorgear.getRotors()->size();j++)
 430     {
 431         Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
 432         float vs[3], pos[3];
 433         r->getPosition(pos);
 434         localWind(pos, s, vs, alt);
 435         r->calcLiftFactor(vs, _rho,s);
 436         float tq=0;
 437         // total torque of rotor (scalar) for calculating new rotor rpm
 438
 439         for(i=0; i<r->_rotorparts.size(); i++) {
 440             float torque_scalar=0;
 441             Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
 442
 443             // Vsurf = wind - velocity + (rot cross (cg - pos))
 444             float vs[3], pos[3];
 445             rp->getPosition(pos);
 446             localWind(pos, s, vs, alt,true);
 447
 448             float force[3], torque[3];
 449             rp->calcForce(vs, _rho, force, torque, &torque_scalar);
 450             tq+=torque_scalar;
 451             rp->getPositionForceAttac(pos);
 452
 453             _body.addForce(pos, force);
 454             _body.addTorque(torque);
 455         }
 456         r->setTorque(tq);
 457     }
 458     if (_rotorgear.isInUse())
 459     {
 460         float torque[3];
 461         _rotorgear.calcForces(torque);
 462         _body.addTorque(torque);
 463     }
 464
 465     // Account for ground effect by multiplying the vertical force
 466     // component by an amount linear with the fraction of the wingspan
 467     // above the ground.
 468     if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
 469     {
 470         float dist = ground[3] - Math::dot3(ground, _wingCenter);
 471         if(dist > 0 && dist < _groundEffectSpan) {
 472         float fz = Math::dot3(faero, ground);
 473             fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
 474             fz *= _groundEffect;
 475         Math::mul3(fz, ground, faero);
 476         _body.addForce(faero);
 477         }
 478     }
 479
 480     // Convert the velocity and rotation vectors to local coordinates
 481     float lrot[3], lv[3];
 482     Math::vmul33(s->orient, s->rot, lrot);
 483     Math::vmul33(s->orient, s->v, lv);
 484
 485     // The landing gear
 486     for(i=0; i<_gears.size(); i++) {
 487         float force[3], contact[3];
 488         Gear* g = (Gear*)_gears.get(i);
 489
 490         g->calcForce(&_body, s, lv, lrot);
 491         g->getForce(force, contact);
 492         _body.addForce(contact, force);
 493     }
 494
 495     // The arrester hook
 496     if(_hook) {
 497         _hook->calcForce(_ground_cb, &_body, s, lv, lrot);
 498         float force[3], contact[3];
 499         _hook->getForce(force, contact);
 500         _body.addForce(contact, force);
 501     }
 502
 503     // The launchbar/holdback
 504     if(_launchbar) {
 505         _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
 506         float forcelb[3], contactlb[3], forcehb[3], contacthb[3];
 507         _launchbar->getForce(forcelb, contactlb, forcehb, contacthb);
 508         _body.addForce(contactlb, forcelb);
 509         _body.addForce(contacthb, forcehb);
 510     }
 511
 512 // The hitches
 513     for(i=0; i<_hitches.size(); i++) {
 514         float force[3], contact[3];
 515         Hitch* h = (Hitch*)_hitches.get(i);
 516         h->calcForce(_ground_cb,&_body, s);
 517         h->getForce(force, contact);
 518         _body.addForce(contact, force);
 519     }
 520 }
 521 void Model::newState(State* s)
 522 {
 523     _s = s;
 524
 525     // Some simple collision detection
 526     float min = 1e8;
 527     int i;
 528     for(i=0; i<_gears.size(); i++) {
 529         Gear* g = (Gear*)_gears.get(i);
 530
 531         if (!g->getSubmergable())
 532         {
 533             // Get the point of ground contact
 534             float pos[3], cmpr[3];
 535             g->getPosition(pos);
 536             g->getCompression(cmpr);
 537             Math::mul3(g->getCompressFraction(), cmpr, cmpr);
 538             Math::add3(cmpr, pos, pos);
 539
 540             // The plane transformed to local coordinates.
 541             double global_ground[4];
 542             g->getGlobalGround(global_ground);
 543             float ground[4];
 544             s->planeGlobalToLocal(global_ground, ground);
 545             float dist = ground[3] - Math::dot3(pos, ground);
 546
 547             // Find the lowest one
 548             if(dist < min)
 549                 min = dist;
 550         }
 551     }
 552     _agl = min;
 553     if(_agl < -1) // Allow for some integration slop
 554         _crashed = true;
 555 }
 556
 557 // Calculates the airflow direction at the given point and for the
 558 // specified aircraft velocity.
 559 void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
 560 {
 561     float tmp[3], lwind[3], lrot[3], lv[3];
 562
 563     // Get a global coordinate for our local position, and calculate
 564     // turbulence.
 565     if(_turb) {
 566         double gpos[3]; float up[3];
 567         Math::tmul33(s->orient, pos, tmp);
 568         for(int i=0; i<3; i++) {
 569             gpos[i] = s->pos[i] + tmp[i];
 570         }
 571         Glue::geodUp(gpos, up);
 572         _turb->getTurbulence(gpos, alt, up, lwind);
 573         Math::add3(_wind, lwind, lwind);
 574     } else {
 575         Math::set3(_wind, lwind);
 576     }
 577
 578     // Convert to local coordinates
 579     Math::vmul33(s->orient, lwind, lwind);
 580     Math::vmul33(s->orient, s->rot, lrot);
 581     Math::vmul33(s->orient, s->v, lv);
 582
 583     _body.pointVelocity(pos, lrot, out); // rotational velocity
 584     Math::mul3(-1, out, out);      //  (negated)
 585     Math::add3(lwind, out, out);    //  + wind
 586     Math::sub3(out, lv, out);       //  - velocity
 587
 588     //add the downwash of the rotors if it is not self a rotor
 589     if (_rotorgear.isInUse()&&!is_rotor)
 590     {
 591         _rotorgear.getDownWash(pos,lv,tmp);
 592         Math::add3(out,tmp, out);    //  + downwash
 593     }
 594
 595
 596 }
 597
 598 }; // namespace yasim