src/FDM/YASim/Airplane.cpp

   1 #include "Atmosphere.hpp"
   2 #include "ControlMap.hpp"
   3 #include "Gear.hpp"
   4 #include "Math.hpp"
   5 #include "Glue.hpp"
   6 #include "RigidBody.hpp"
   7 #include "Surface.hpp"
   8 #include "Thruster.hpp"
   9
  10 #include "Airplane.hpp"
  11 namespace yasim {
  12
  13 Airplane::Airplane()
  14 {
  15     _emptyWeight = 0;
  16     _pilotPos[0] = _pilotPos[1] = _pilotPos[2] = 0;
  17     _wing = 0;
  18     _tail = 0;
  19     _ballast = 0;
  20     _cruiseP = 0;
  21     _cruiseT = 0;
  22     _cruiseSpeed = 0;
  23     _cruiseWeight = 0;
  24     _approachP = 0;
  25     _approachT = 0;
  26     _approachSpeed = 0;
  27     _approachAoA = 0;
  28     _approachWeight = 0;
  29
  30     _dragFactor = 1;
  31     _liftRatio = 1;
  32     _cruiseAoA = 0;
  33     _tailIncidence = 0;
  34 }
  35
  36 Airplane::~Airplane()
  37 {
  38     int i;
  39     for(i=0; i<_fuselages.size(); i++)
  40         delete (Fuselage*)_fuselages.get(i);
  41     for(i=0; i<_tanks.size(); i++)
  42         delete (Tank*)_tanks.get(i);
  43     for(i=0; i<_thrusters.size(); i++)
  44         delete (ThrustRec*)_thrusters.get(i);
  45     for(i=0; i<_gears.size(); i++)
  46         delete (GearRec*)_gears.get(i);
  47     for(i=0; i<_surfs.size(); i++)
  48         delete (Surface*)_surfs.get(i);
  49     for(i=0; i<_contacts.size(); i++)
  50         delete[] (float*)_contacts.get(i);
  51 }
  52
  53 void Airplane::iterate(float dt)
  54 {
  55     // The gear might have moved.  Change their aerodynamics.
  56     updateGearState();
  57
  58     _model.iterate();
  59
  60     // FIXME: Consume fuel
  61 }
  62
  63 ControlMap* Airplane::getControlMap()
  64 {
  65     return &_controls;
  66 }
  67
  68 Model* Airplane::getModel()
  69 {
  70     return &_model;
  71 }
  72
  73 void Airplane::getPilotAccel(float* out)
  74 {
  75     State* s = _model.getState();
  76
  77     // Gravity
  78     Glue::geodUp(s->pos, out);
  79     Math::mul3(-9.8, out, out);
  80
  81     // The regular acceleration
  82     float tmp[3];
  83     Math::mul3(-1, s->acc, tmp);
  84     Math::add3(tmp, out, out);
  85
  86     // Convert to aircraft coordinates
  87     Math::vmul33(s->orient, out, out);
  88
  89     // FIXME: rotational & centripetal acceleration needed
  90 }
  91
  92 void Airplane::setPilotPos(float* pos)
  93 {
  94     int i;
  95     for(i=0; i<3; i++) _pilotPos[i] = pos[i];
  96 }
  97
  98 void Airplane::getPilotPos(float* out)
  99 {
 100     int i;
 101     for(i=0; i<3; i++) out[i] = _pilotPos[i];
 102 }
 103
 104 int Airplane::numGear()
 105 {
 106     return _gears.size();
 107 }
 108
 109 Gear* Airplane::getGear(int g)
 110 {
 111     return ((GearRec*)_gears.get(g))->gear;
 112 }
 113
 114 void Airplane::updateGearState()
 115 {
 116     for(int i=0; i<_gears.size(); i++) {
 117         GearRec* gr = (GearRec*)_gears.get(i);
 118         float ext = gr->gear->getExtension();
 119
 120         gr->surf->setXDrag(ext);
 121         gr->surf->setYDrag(ext);
 122         gr->surf->setZDrag(ext);
 123     }
 124 }
 125
 126 void Airplane::setApproach(float speed, float altitude)
 127 {
 128     // The zero AoA will become a calculated stall AoA in compile()
 129     setApproach(speed, altitude, 0);
 130 }
 131
 132 void Airplane::setApproach(float speed, float altitude, float aoa)
 133 {
 134     _approachSpeed = speed;
 135     _approachP = Atmosphere::getStdPressure(altitude);
 136     _approachT = Atmosphere::getStdTemperature(altitude);
 137     _approachAoA = aoa;
 138 }
 139
 140 void Airplane::setCruise(float speed, float altitude)
 141 {
 142     _cruiseSpeed = speed;
 143     _cruiseP = Atmosphere::getStdPressure(altitude);
 144     _cruiseT = Atmosphere::getStdTemperature(altitude);
 145     _cruiseAoA = 0;
 146     _tailIncidence = 0;
 147 }
 148
 149 void Airplane::addApproachControl(int control, float val)
 150 {
 151     Control* c = new Control();
 152     c->control = control;
 153     c->val = val;
 154     _approachControls.add(c);
 155 }
 156
 157 void Airplane::addCruiseControl(int control, float val)
 158 {
 159     Control* c = new Control();
 160     c->control = control;
 161     c->val = val;
 162     _cruiseControls.add(c);
 163 }
 164
 165 int Airplane::numTanks()
 166 {
 167     return _tanks.size();
 168 }
 169
 170 float Airplane::getFuel(int tank)
 171 {
 172     return ((Tank*)_tanks.get(tank))->fill;
 173 }
 174
 175 float Airplane::getFuelDensity(int tank)
 176 {
 177     return ((Tank*)_tanks.get(tank))->density;
 178 }
 179
 180 void Airplane::setWeight(float weight)
 181 {
 182     _emptyWeight = weight;
 183 }
 184
 185 void Airplane::setWing(Wing* wing)
 186 {
 187     _wing = wing;
 188 }
 189
 190 void Airplane::setTail(Wing* tail)
 191 {
 192     _tail = tail;
 193 }
 194
 195 void Airplane::addVStab(Wing* vstab)
 196 {
 197     _vstabs.add(vstab);
 198 }
 199
 200 void Airplane::addFuselage(float* front, float* back, float width,
 201                            float taper, float mid)
 202 {
 203     Fuselage* f = new Fuselage();
 204     int i;
 205     for(i=0; i<3; i++) {
 206         f->front[i] = front[i];
 207         f->back[i]  = back[i];
 208     }
 209     f->width = width;
 210     f->taper = taper;
 211     f->mid = mid;
 212     _fuselages.add(f);
 213 }
 214
 215 int Airplane::addTank(float* pos, float cap, float density)
 216 {
 217     Tank* t = new Tank();
 218     int i;
 219     for(i=0; i<3; i++) t->pos[i] = pos[i];
 220     t->cap = cap;
 221     t->fill = cap;
 222     t->density = density;
 223     t->handle = 0xffffffff;
 224     return _tanks.add(t);
 225 }
 226
 227 void Airplane::addGear(Gear* gear)
 228 {
 229     GearRec* g = new GearRec();
 230     g->gear = gear;
 231     g->surf = 0;
 232     _gears.add(g);
 233 }
 234
 235 void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
 236 {
 237     ThrustRec* t = new ThrustRec();
 238     t->thruster = thruster;
 239     t->mass = mass;
 240     int i;
 241     for(i=0; i<3; i++) t->cg[i] = cg[i];
 242     _thrusters.add(t);
 243 }
 244
 245 void Airplane::addBallast(float* pos, float mass)
 246 {
 247     _model.getBody()->addMass(mass, pos);
 248     _ballast += mass;
 249 }
 250
 251 int Airplane::addWeight(float* pos, float size)
 252 {
 253     WeightRec* wr = new WeightRec();
 254     wr->handle = _model.getBody()->addMass(0, pos);
 255
 256     wr->surf = new Surface();
 257     wr->surf->setPosition(pos);
 258     wr->surf->setTotalDrag(size*size);
 259     _model.addSurface(wr->surf);
 260     _surfs.add(wr->surf);
 261
 262     return _weights.add(wr);
 263 }
 264
 265 void Airplane::setWeight(int handle, float mass)
 266 {
 267     WeightRec* wr = (WeightRec*)_weights.get(handle);
 268
 269     _model.getBody()->setMass(wr->handle, mass);
 270
 271     // Kill the aerodynamic drag if the mass is exactly zero.  This is
 272     // how we simulate droppable stores.
 273     if(mass == 0) {
 274         wr->surf->setXDrag(0);
 275         wr->surf->setYDrag(0);
 276         wr->surf->setZDrag(0);
 277     } else {
 278         wr->surf->setXDrag(1);
 279         wr->surf->setYDrag(1);
 280         wr->surf->setZDrag(1);
 281     }
 282 }
 283
 284 void Airplane::setFuelFraction(float frac)
 285 {
 286     int i;
 287     for(i=0; i<_tanks.size(); i++) {
 288         Tank* t = (Tank*)_tanks.get(i);
 289         _model.getBody()->setMass(t->handle, t->cap * frac);
 290     }
 291 }
 292
 293 float Airplane::getDragCoefficient()
 294 {
 295     return _dragFactor;
 296 }
 297
 298 float Airplane::getLiftRatio()
 299 {
 300     return _liftRatio;
 301 }
 302
 303 float Airplane::getCruiseAoA()
 304 {
 305     return _cruiseAoA;
 306 }
 307
 308 float Airplane::getTailIncidence()
 309 {
 310     return _tailIncidence;
 311 }
 312
 313 char* Airplane::getFailureMsg()
 314 {
 315     return _failureMsg;
 316 }
 317
 318 int Airplane::getSolutionIterations()
 319 {
 320     return _solutionIterations;
 321 }
 322
 323 void Airplane::setupState(float aoa, float speed, State* s)
 324 {
 325     float cosAoA = Math::cos(aoa);
 326     float sinAoA = Math::sin(aoa);
 327     s->orient[0] =  cosAoA; s->orient[1] = 0; s->orient[2] = sinAoA;
 328     s->orient[3] =       0; s->orient[4] = 1; s->orient[5] =      0;
 329     s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
 330
 331     s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
 332
 333     int i;
 334     for(i=0; i<3; i++)
 335         s->pos[i] = s->rot[i] = s->acc[i] = s->racc[i] = 0;
 336
 337     // Put us 1m above the origin, or else the gravity computation in
 338     // Model goes nuts
 339     s->pos[2] = 1;
 340 }
 341
 342 void Airplane::addContactPoint(float* pos)
 343 {
 344     float* cp = new float[3];
 345     cp[0] = pos[0];
 346     cp[1] = pos[1];
 347     cp[2] = pos[2];
 348     _contacts.add(cp);
 349 }
 350
 351 float Airplane::compileWing(Wing* w)
 352 {
 353     // The tip of the wing is a contact point
 354     float tip[3];
 355     w->getTip(tip);
 356     addContactPoint(tip);
 357     if(w->isMirrored()) {
 358         tip[1] *= -1;
 359         addContactPoint(tip);
 360     }
 361
 362     // Make sure it's initialized.  The surfaces will pop out with
 363     // total drag coefficients equal to their areas, which is what we
 364     // want.
 365     w->compile();
 366
 367     float wgt = 0;
 368     int i;
 369     for(i=0; i<w->numSurfaces(); i++) {
 370         Surface* s = (Surface*)w->getSurface(i);
 371
 372         float td = s->getTotalDrag();
 373         s->setTotalDrag(td);
 374
 375         _model.addSurface(s);
 376
 377         float mass = w->getSurfaceWeight(i);
 378         mass = mass * Math::sqrt(mass);
 379         float pos[3];
 380         s->getPosition(pos);
 381         _model.getBody()->addMass(mass, pos);
 382         wgt += mass;
 383     }
 384     return wgt;
 385 }
 386
 387 float Airplane::compileFuselage(Fuselage* f)
 388 {
 389     // The front and back are contact points
 390     addContactPoint(f->front);
 391     addContactPoint(f->back);
 392
 393     float wgt = 0;
 394     float fwd[3];
 395     Math::sub3(f->front, f->back, fwd);
 396     float len = Math::mag3(fwd);
 397     float wid = f->width;
 398     int segs = (int)Math::ceil(len/wid);
 399     float segWgt = len*wid/segs;
 400     int j;
 401     for(j=0; j<segs; j++) {
 402         float frac = (j+0.5) / segs;
 403
 404         float scale = 1;
 405         if(frac < f->mid)
 406             scale = f->taper+(1-f->taper) * (frac / f->mid);
 407         else
 408             scale = f->taper+(1-f->taper) * (frac - f->mid) / (1 - f->mid);
 409
 410         // Where are we?
 411         float pos[3];
 412         Math::mul3(frac, fwd, pos);
 413         Math::add3(f->back, pos, pos);
 414
 415         // _Mass_ weighting goes as surface area^(3/2)
 416         float mass = scale*segWgt * Math::sqrt(scale*segWgt);
 417         _model.getBody()->addMass(mass, pos);
 418         wgt += mass;
 419
 420         // Make a Surface too
 421         Surface* s = new Surface();
 422         s->setPosition(pos);
 423         float sideDrag = len/wid;
 424         s->setYDrag(sideDrag);
 425         s->setZDrag(sideDrag);
 426         s->setTotalDrag(scale*segWgt);
 427
 428         // FIXME: fails for fuselages aligned along the Y axis
 429         float o[9];
 430         float *x=o, *y=o+3, *z=o+6; // nicknames for the axes
 431         Math::unit3(fwd, x);
 432         y[0] = 0; y[1] = 1; y[2] = 0;
 433         Math::cross3(x, y, z);
 434         Math::unit3(z, z);
 435         Math::cross3(z, x, y);
 436         s->setOrientation(o);
 437
 438         _model.addSurface(s);
 439         _surfs.add(s);
 440     }
 441     return wgt;
 442 }
 443
 444 // FIXME: should probably add a mass for the gear, too
 445 void Airplane::compileGear(GearRec* gr)
 446 {
 447     Gear* g = gr->gear;
 448
 449     // Make a Surface object for the aerodynamic behavior
 450     Surface* s = new Surface();
 451     gr->surf = s;
 452
 453     // Put the surface at the half-way point on the gear strut, give
 454     // it a drag coefficient equal to a square of the same dimension
 455     // (gear are really draggy) and make it symmetric.  Assume that
 456     // the "length" of the gear is 3x the compression distance
 457     float pos[3], cmp[3];
 458     g->getCompression(cmp);
 459     float length = 3 * Math::mag3(cmp);
 460     g->getPosition(pos);
 461     Math::mul3(0.5, cmp, cmp);
 462     Math::add3(pos, cmp, pos);
 463
 464     s->setPosition(pos);
 465     s->setTotalDrag(length*length);
 466
 467     _model.addGear(g);
 468     _model.addSurface(s);
 469     _surfs.add(s);
 470 }
 471
 472 void Airplane::compileContactPoints()
 473 {
 474     // Figure it will compress by 20cm
 475     float comp[3];
 476     float DIST = 0.2;
 477     comp[0] = 0; comp[1] = 0; comp[2] = DIST;
 478
 479     // Give it a spring constant such that at full compression it will
 480     // hold up 10 times the planes mass.  That's about right.  Yeah.
 481     float mass = _model.getBody()->getTotalMass();
 482     float spring = (1/DIST) * 9.8 * 10 * mass;
 483     float damp = 2 * Math::sqrt(spring * mass);
 484
 485     int i;
 486     for(i=0; i<_contacts.size(); i++) {
 487         float *cp = (float*)_contacts.get(i);
 488
 489         Gear* g = new Gear();
 490         g->setPosition(cp);
 491
 492         g->setCompression(comp);
 493         g->setSpring(spring);
 494         g->setDamping(damp);
 495         g->setBrake(1);
 496
 497         // I made these up
 498         g->setStaticFriction(0.6);
 499         g->setDynamicFriction(0.5);
 500
 501         _model.addGear(g);
 502     }
 503 }
 504
 505 void Airplane::compile()
 506 {
 507     double ground[3];
 508     ground[0] = 0; ground[1] = 0; ground[2] = 1;
 509     _model.setGroundPlane(ground, -100000);
 510
 511     RigidBody* body = _model.getBody();
 512     int firstMass = body->numMasses();
 513
 514     // Generate the point masses for the plane.  Just use unitless
 515     // numbers for a first pass, then go back through and rescale to
 516     // make the weight right.
 517     float aeroWgt = 0;
 518
 519     // The Wing objects
 520     aeroWgt += compileWing(_wing);
 521     aeroWgt += compileWing(_tail);
 522     int i;
 523     for(i=0; i<_vstabs.size(); i++) {
 524         aeroWgt += compileWing((Wing*)_vstabs.get(i));
 525     }
 526
 527     // The fuselage(s)
 528     for(i=0; i<_fuselages.size(); i++) {
 529         aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
 530     }
 531
 532     // Count up the absolute weight we have
 533     float nonAeroWgt = _ballast;
 534     for(i=0; i<_thrusters.size(); i++)
 535         nonAeroWgt += ((ThrustRec*)_thrusters.get(i))->mass;
 536
 537     // Rescale to the specified empty weight
 538     float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
 539     for(i=firstMass; i<body->numMasses(); i++)
 540         body->setMass(i, body->getMass(i)*wscale);
 541
 542     // Add the thruster masses
 543     for(i=0; i<_thrusters.size(); i++) {
 544         ThrustRec* t = (ThrustRec*)_thrusters.get(i);
 545         body->addMass(t->mass, t->cg);
 546     }
 547
 548     // Add the tanks, empty for now.
 549     float totalFuel = 0;
 550     for(i=0; i<_tanks.size(); i++) {
 551         Tank* t = (Tank*)_tanks.get(i);
 552         t->handle = body->addMass(0, t->pos);
 553         totalFuel += t->cap;
 554     }
 555     _cruiseWeight = _emptyWeight + totalFuel*0.5;
 556     _approachWeight = _emptyWeight + totalFuel*0.2;
 557
 558     body->recalc();
 559
 560     // Add surfaces for the landing gear.
 561     for(i=0; i<_gears.size(); i++)
 562         compileGear((GearRec*)_gears.get(i));
 563
 564     // The Thruster objects
 565     for(i=0; i<_thrusters.size(); i++) {
 566         ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
 567         tr->handle = _model.addThruster(tr->thruster);
 568     }
 569
 570     // Ground effect
 571     float gepos[3];
 572     float gespan = _wing->getGroundEffect(gepos);
 573     _model.setGroundEffect(gepos, gespan, .3);
 574
 575     solveGear();
 576     solve();
 577
 578     // Do this after solveGear, because it creates "gear" objects that
 579     // we don't want to affect.
 580     compileContactPoints();
 581 }
 582
 583 void Airplane::solveGear()
 584 {
 585     float cg[3], pos[3];
 586     _model.getBody()->getCG(cg);
 587
 588     // Calculate spring constant weightings for the gear.  Weight by
 589     // the inverse of the distance to the c.g. in the XY plane, which
 590     // should be correct for most gear arrangements.  Add 50cm of
 591     // "buffer" to keep things from blowing up with aircraft with a
 592     // single gear very near the c.g. (AV-8, for example).
 593     float total = 0;
 594     int i;
 595     for(i=0; i<_gears.size(); i++) {
 596         GearRec* gr = (GearRec*)_gears.get(i);
 597         Gear* g = gr->gear;
 598         g->getPosition(pos);
 599         Math::sub3(cg, pos, pos);
 600         gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
 601         total += gr->wgt;
 602     }
 603
 604     // Renormalize so they sum to 1
 605     for(i=0; i<_gears.size(); i++)
 606         ((GearRec*)_gears.get(i))->wgt /= total;
 607
 608     // The force at max compression should be sufficient to stop a
 609     // plane moving downwards at 3x the approach descent rate.  Assume
 610     // a 3 degree approach.
 611     float descentRate = 3*_approachSpeed/19.1;
 612
 613     // Spread the kinetic energy according to the gear weights.  This
 614     // will results in an equal compression fraction (not distance) of
 615     // each gear.
 616     float energy = 0.5*_approachWeight*descentRate*descentRate;
 617
 618     for(i=0; i<_gears.size(); i++) {
 619         GearRec* gr = (GearRec*)_gears.get(i);
 620         float e = energy * gr->wgt;
 621         float comp[3];
 622         gr->gear->getCompression(comp);
 623         float len = Math::mag3(comp);
 624
 625         // Energy in a spring: e = 0.5 * k * len^2
 626         float k = 2 * e / (len*len);
 627
 628         gr->gear->setSpring(k);
 629
 630         // Critically damped (too damped, too!)
 631         gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt));
 632
 633         // These are pretty generic
 634         gr->gear->setStaticFriction(0.8);
 635         gr->gear->setDynamicFriction(0.7);
 636     }
 637 }
 638
 639 void Airplane::initEngines()
 640 {
 641     for(int i=0; i<_thrusters.size(); i++) {
 642         ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
 643         tr->thruster->init();
 644     }
 645 }
 646
 647 void Airplane::stabilizeThrust()
 648 {
 649     int i;
 650     for(i=0; i<_thrusters.size(); i++)
 651         _model.getThruster(i)->stabilize();
 652 }
 653
 654 void Airplane::runCruise()
 655 {
 656     setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
 657     _model.setState(&_cruiseState);
 658     _model.setAir(_cruiseP, _cruiseT);
 659
 660     // The control configuration
 661     _controls.reset();
 662     int i;
 663     for(i=0; i<_cruiseControls.size(); i++) {
 664         Control* c = (Control*)_cruiseControls.get(i);
 665         _controls.setInput(c->control, c->val);
 666     }
 667     _controls.applyControls(1000000); // Huge dt value
 668
 669     // The local wind
 670     float wind[3];
 671     Math::mul3(-1, _cruiseState.v, wind);
 672     Math::vmul33(_cruiseState.orient, wind, wind);
 673
 674     // Cruise is by convention at 50% tank capacity
 675     setFuelFraction(0.5);
 676
 677     // Set up the thruster parameters and iterate until the thrust
 678     // stabilizes.
 679     for(i=0; i<_thrusters.size(); i++) {
 680         Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 681         t->setWind(wind);
 682         t->setAir(_cruiseP, _cruiseT);
 683     }
 684     stabilizeThrust();
 685
 686     updateGearState();
 687
 688     // Precompute thrust in the model, and calculate aerodynamic forces
 689     _model.getBody()->reset();
 690     _model.initIteration();
 691     _model.calcForces(&_cruiseState);
 692 }
 693
 694 void Airplane::runApproach()
 695 {
 696     setupState(_approachAoA, _approachSpeed, &_approachState);
 697     _model.setState(&_approachState);
 698     _model.setAir(_approachP, _approachT);
 699
 700     // The control configuration
 701     _controls.reset();
 702     int i;
 703     for(i=0; i<_approachControls.size(); i++) {
 704         Control* c = (Control*)_approachControls.get(i);
 705         _controls.setInput(c->control, c->val);
 706     }
 707     _controls.applyControls(1000000);
 708
 709     // The local wind
 710     float wind[3];
 711     Math::mul3(-1, _approachState.v, wind);
 712     Math::vmul33(_approachState.orient, wind, wind);
 713
 714     // Approach is by convention at 20% tank capacity
 715     setFuelFraction(0.2);
 716
 717     // Run the thrusters until they get to a stable setting.  FIXME:
 718     // this is lots of wasted work.
 719     for(i=0; i<_thrusters.size(); i++) {
 720         Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 721         t->setWind(wind);
 722         t->setAir(_approachP, _approachT);
 723     }
 724     stabilizeThrust();
 725
 726     updateGearState();
 727
 728     // Precompute thrust in the model, and calculate aerodynamic forces
 729     _model.getBody()->reset();
 730     _model.initIteration();
 731     _model.calcForces(&_approachState);
 732 }
 733
 734 void Airplane::applyDragFactor(float factor)
 735 {
 736     float applied = Math::sqrt(factor);
 737     _dragFactor *= applied;
 738     _wing->setDragScale(_wing->getDragScale() * applied);
 739     _tail->setDragScale(_tail->getDragScale() * applied);
 740     int i;
 741     for(i=0; i<_vstabs.size(); i++) {
 742         Wing* w = (Wing*)_vstabs.get(i);
 743         w->setDragScale(w->getDragScale() * applied);
 744     }
 745     for(i=0; i<_surfs.size(); i++) {
 746         Surface* s = (Surface*)_surfs.get(i);
 747         s->setTotalDrag(s->getTotalDrag() * applied);
 748     }
 749 }
 750
 751 void Airplane::applyLiftRatio(float factor)
 752 {
 753     float applied = Math::sqrt(factor);
 754     _liftRatio *= applied;
 755     _wing->setLiftRatio(_wing->getLiftRatio() * applied);
 756     _tail->setLiftRatio(_tail->getLiftRatio() * applied);
 757     int i;
 758     for(i=0; i<_vstabs.size(); i++) {
 759         Wing* w = (Wing*)_vstabs.get(i);
 760         w->setLiftRatio(w->getLiftRatio() * applied);
 761     }
 762 }
 763
 764 float Airplane::clamp(float val, float min, float max)
 765 {
 766     if(val < min) return min;
 767     if(val > max) return max;
 768     return val;
 769 }
 770
 771 float Airplane::normFactor(float f)
 772 {
 773     if(f < 0) f = -f;
 774     if(f < 1) f = 1/f;
 775     return f;
 776 }
 777
 778 void Airplane::solve()
 779 {
 780     static const float ARCMIN = 0.0002909;
 781
 782     float tmp[3];
 783     _solutionIterations = 0;
 784     _failureMsg = 0;
 785     while(1) {
 786         if(_solutionIterations++ > 10000) {
 787             _failureMsg = "Solution failed to converge after 10000 iterations";
 788             return;
 789         }
 790
 791         // Run an iteration at cruise, and extract the needed numbers:
 792         runCruise();
 793
 794         _model.getThrust(tmp);
 795         float thrust = tmp[0];
 796
 797         _model.getBody()->getAccel(tmp);
 798         float xforce = _cruiseWeight * tmp[0];
 799         float clift0 = _cruiseWeight * tmp[2];
 800
 801         _model.getBody()->getAngularAccel(tmp);
 802         float pitch0 = tmp[1];
 803
 804         // Run an approach iteration, and do likewise
 805         runApproach();
 806
 807         _model.getBody()->getAccel(tmp);
 808         float alift = _approachWeight * tmp[2];
 809
 810         // Modify the cruise AoA a bit to get a derivative
 811         _cruiseAoA += ARCMIN;
 812         runCruise();
 813         _cruiseAoA -= ARCMIN;
 814
 815         _model.getBody()->getAccel(tmp);
 816         float clift1 = _cruiseWeight * tmp[2];
 817
 818         // Do the same with the tail incidence
 819         _tail->setIncidence(_tailIncidence + ARCMIN);
 820         runCruise();
 821         _tail->setIncidence(_tailIncidence);
 822
 823         _model.getBody()->getAngularAccel(tmp);
 824         float pitch1 = tmp[1];
 825
 826         // Now calculate:
 827         float awgt = 9.8 * _approachWeight;
 828
 829         float dragFactor = thrust / (thrust-xforce);
 830         float liftFactor = awgt / (awgt+alift);
 831         float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
 832         float tailDelta = -pitch0 * (ARCMIN/(pitch1-pitch0));
 833
 834         // Sanity:
 835         if(dragFactor <= 0) {
 836             _failureMsg = "Zero or negative drag adjustment.";
 837             return;
 838         } else if(liftFactor <= 0) {
 839             _failureMsg = "Zero or negative lift adjustment.";
 840             return;
 841         }
 842
 843         // And apply:
 844         applyDragFactor(dragFactor);
 845         applyLiftRatio(liftFactor);
 846
 847         // DON'T do the following until the above are sane
 848         if(normFactor(dragFactor) > 1.1
 849            || normFactor(liftFactor) > 1.1)
 850         {
 851             continue;
 852         }
 853
 854         // OK, now we can adjust the minor variables
 855         _cruiseAoA += 0.5*aoaDelta;
 856         _tailIncidence += 0.5*tailDelta;
 857
 858         _cruiseAoA = clamp(_cruiseAoA, -.174, .174);
 859         _tailIncidence = clamp(_tailIncidence, -.174, .174);
 860
 861         if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
 862            aoaDelta < .000017   && tailDelta < .000017)
 863         {
 864             break;
 865         }
 866     }
 867
 868     if(_dragFactor < 1e-06 || _dragFactor > 1e6) {
 869         _failureMsg = "Drag factor beyond reasonable bounds.";
 870         return;
 871     } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
 872         _failureMsg = "Lift ratio beyond reasonable bounds.";
 873         return;
 874     } else if(Math::abs(_cruiseAoA) >= .174) {
 875         _failureMsg = "Cruise AoA > 10 degrees";
 876         return;
 877     } else if(Math::abs(_tailIncidence) >= .174) {
 878         _failureMsg = "Tail incidence > 10 degrees";
 879         return;
 880     }
 881 }
 882 }; // namespace yasim