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