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