src/FDM/YASim/Model.cpp

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