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