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         s->setOrientation(o);
 435
 436         _model.addSurface(s);
 437         _surfs.add(s);
 438     }
 439     return wgt;
 440 }
 441
 442 // FIXME: should probably add a mass for the gear, too
 443 void Airplane::compileGear(GearRec* gr)
 444 {
 445     Gear* g = gr->gear;
 446
 447     // Make a Surface object for the aerodynamic behavior
 448     Surface* s = new Surface();
 449     gr->surf = s;
 450
 451     // Put the surface at the half-way point on the gear strut, give
 452     // it a drag coefficient equal to a square of the same dimension
 453     // (gear are really draggy) and make it symmetric.  Assume that
 454     // the "length" of the gear is 3x the compression distance
 455     float pos[3], cmp[3];
 456     g->getCompression(cmp);
 457     float length = 3 * Math::mag3(cmp);
 458     g->getPosition(pos);
 459     Math::mul3(0.5, cmp, cmp);
 460     Math::add3(pos, cmp, pos);
 461
 462     s->setPosition(pos);
 463     s->setTotalDrag(length*length);
 464
 465     _model.addGear(g);
 466     _model.addSurface(s);
 467     _surfs.add(s);
 468 }
 469
 470 void Airplane::compileContactPoints()
 471 {
 472     // Figure it will compress by 20cm
 473     float comp[3];
 474     float DIST = 0.2;
 475     comp[0] = 0; comp[1] = 0; comp[2] = DIST;
 476
 477     // Give it a spring constant such that at full compression it will
 478     // hold up 10 times the planes mass.  That's about right.  Yeah.
 479     float mass = _model.getBody()->getTotalMass();
 480     float spring = (1/DIST) * 9.8 * 10 * mass;
 481     float damp = 2 * Math::sqrt(spring * mass);
 482
 483     int i;
 484     for(i=0; i<_contacts.size(); i++) {
 485         float *cp = (float*)_contacts.get(i);
 486
 487         Gear* g = new Gear();
 488         g->setPosition(cp);
 489
 490         g->setCompression(comp);
 491         g->setSpring(spring);
 492         g->setDamping(damp);
 493         g->setBrake(1);
 494
 495         // I made these up
 496         g->setStaticFriction(0.6);
 497         g->setDynamicFriction(0.5);
 498
 499         _model.addGear(g);
 500     }
 501 }
 502
 503 void Airplane::compile()
 504 {
 505     double ground[3];
 506     ground[0] = 0; ground[1] = 0; ground[2] = 1;
 507     _model.setGroundPlane(ground, -100000);
 508
 509     RigidBody* body = _model.getBody();
 510     int firstMass = body->numMasses();
 511
 512     // Generate the point masses for the plane.  Just use unitless
 513     // numbers for a first pass, then go back through and rescale to
 514     // make the weight right.
 515     float aeroWgt = 0;
 516
 517     // The Wing objects
 518     aeroWgt += compileWing(_wing);
 519     aeroWgt += compileWing(_tail);
 520     int i;
 521     for(i=0; i<_vstabs.size(); i++) {
 522         aeroWgt += compileWing((Wing*)_vstabs.get(i));
 523     }
 524
 525     // The fuselage(s)
 526     for(i=0; i<_fuselages.size(); i++) {
 527         aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
 528     }
 529
 530     // Count up the absolute weight we have
 531     float nonAeroWgt = _ballast;
 532     for(i=0; i<_thrusters.size(); i++)
 533         nonAeroWgt += ((ThrustRec*)_thrusters.get(i))->mass;
 534
 535     // Rescale to the specified empty weight
 536     float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
 537     for(i=firstMass; i<body->numMasses(); i++)
 538         body->setMass(i, body->getMass(i)*wscale);
 539
 540     // Add the thruster masses
 541     for(i=0; i<_thrusters.size(); i++) {
 542         ThrustRec* t = (ThrustRec*)_thrusters.get(i);
 543         body->addMass(t->mass, t->cg);
 544     }
 545
 546     // Add the tanks, empty for now.
 547     float totalFuel = 0;
 548     for(i=0; i<_tanks.size(); i++) {
 549         Tank* t = (Tank*)_tanks.get(i);
 550         t->handle = body->addMass(0, t->pos);
 551         totalFuel += t->cap;
 552     }
 553     _cruiseWeight = _emptyWeight + totalFuel*0.5;
 554     _approachWeight = _emptyWeight + totalFuel*0.2;
 555
 556     body->recalc();
 557
 558     // Add surfaces for the landing gear.
 559     for(i=0; i<_gears.size(); i++)
 560         compileGear((GearRec*)_gears.get(i));
 561
 562     // The Thruster objects
 563     for(i=0; i<_thrusters.size(); i++) {
 564         ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
 565         tr->handle = _model.addThruster(tr->thruster);
 566     }
 567
 568     // Ground effect
 569     float gepos[3];
 570     float gespan = _wing->getGroundEffect(gepos);
 571     _model.setGroundEffect(gepos, gespan, .3);
 572
 573     solveGear();
 574     solve();
 575
 576     // Do this after solveGear, because it creates "gear" objects that
 577     // we don't want to affect.
 578     compileContactPoints();
 579 }
 580
 581 void Airplane::solveGear()
 582 {
 583     float cg[3], pos[3];
 584     _model.getBody()->getCG(cg);
 585
 586     // Calculate spring constant weightings for the gear.  Weight by
 587     // the inverse of the distance to the c.g. in the XY plane, which
 588     // should be correct for most gear arrangements.  Add 50cm of
 589     // "buffer" to keep things from blowing up with aircraft with a
 590     // single gear very near the c.g. (AV-8, for example).
 591     float total = 0;
 592     int i;
 593     for(i=0; i<_gears.size(); i++) {
 594         GearRec* gr = (GearRec*)_gears.get(i);
 595         Gear* g = gr->gear;
 596         g->getPosition(pos);
 597         Math::sub3(cg, pos, pos);
 598         gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
 599         total += gr->wgt;
 600     }
 601
 602     // Renormalize so they sum to 1
 603     for(i=0; i<_gears.size(); i++)
 604         ((GearRec*)_gears.get(i))->wgt /= total;
 605
 606     // The force at max compression should be sufficient to stop a
 607     // plane moving downwards at 3x the approach descent rate.  Assume
 608     // a 3 degree approach.
 609     float descentRate = 3*_approachSpeed/19.1;
 610
 611     // Spread the kinetic energy according to the gear weights.  This
 612     // will results in an equal compression fraction (not distance) of
 613     // each gear.
 614     float energy = 0.5*_approachWeight*descentRate*descentRate;
 615
 616     for(i=0; i<_gears.size(); i++) {
 617         GearRec* gr = (GearRec*)_gears.get(i);
 618         float e = energy * gr->wgt;
 619         float comp[3];
 620         gr->gear->getCompression(comp);
 621         float len = Math::mag3(comp);
 622
 623         // Energy in a spring: e = 0.5 * k * len^2
 624         float k = 2 * e / (len*len);
 625
 626         gr->gear->setSpring(k);
 627
 628         // Critically damped (too damped, too!)
 629         gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt));
 630
 631         // These are pretty generic
 632         gr->gear->setStaticFriction(0.8);
 633         gr->gear->setDynamicFriction(0.7);
 634     }
 635 }
 636
 637 void Airplane::initEngines()
 638 {
 639     for(int i=0; i<_thrusters.size(); i++) {
 640         ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
 641         tr->thruster->init();
 642     }
 643 }
 644
 645 void Airplane::stabilizeThrust()
 646 {
 647     int i;
 648     for(i=0; i<_thrusters.size(); i++)
 649         _model.getThruster(i)->stabilize();
 650 }
 651
 652 void Airplane::runCruise()
 653 {
 654     setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
 655     _model.setState(&_cruiseState);
 656     _model.setAir(_cruiseP, _cruiseT);
 657
 658     // The control configuration
 659     _controls.reset();
 660     int i;
 661     for(i=0; i<_cruiseControls.size(); i++) {
 662         Control* c = (Control*)_cruiseControls.get(i);
 663         _controls.setInput(c->control, c->val);
 664     }
 665     _controls.applyControls(1000000); // Huge dt value
 666
 667     // The local wind
 668     float wind[3];
 669     Math::mul3(-1, _cruiseState.v, wind);
 670     Math::vmul33(_cruiseState.orient, wind, wind);
 671
 672     // Cruise is by convention at 50% tank capacity
 673     setFuelFraction(0.5);
 674
 675     // Set up the thruster parameters and iterate until the thrust
 676     // stabilizes.
 677     for(i=0; i<_thrusters.size(); i++) {
 678         Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 679         t->setWind(wind);
 680         t->setAir(_cruiseP, _cruiseT);
 681     }
 682     stabilizeThrust();
 683
 684     updateGearState();
 685
 686     // Precompute thrust in the model, and calculate aerodynamic forces
 687     _model.getBody()->reset();
 688     _model.initIteration();
 689     _model.calcForces(&_cruiseState);
 690 }
 691
 692 void Airplane::runApproach()
 693 {
 694     setupState(_approachAoA, _approachSpeed, &_approachState);
 695     _model.setState(&_approachState);
 696     _model.setAir(_approachP, _approachT);
 697
 698     // The control configuration
 699     _controls.reset();
 700     int i;
 701     for(i=0; i<_approachControls.size(); i++) {
 702         Control* c = (Control*)_approachControls.get(i);
 703         _controls.setInput(c->control, c->val);
 704     }
 705     _controls.applyControls(1000000);
 706
 707     // The local wind
 708     float wind[3];
 709     Math::mul3(-1, _approachState.v, wind);
 710     Math::vmul33(_approachState.orient, wind, wind);
 711
 712     // Approach is by convention at 20% tank capacity
 713     setFuelFraction(0.2);
 714
 715     // Run the thrusters until they get to a stable setting.  FIXME:
 716     // this is lots of wasted work.
 717     for(i=0; i<_thrusters.size(); i++) {
 718         Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 719         t->setWind(wind);
 720         t->setAir(_approachP, _approachT);
 721     }
 722     stabilizeThrust();
 723
 724     updateGearState();
 725
 726     // Precompute thrust in the model, and calculate aerodynamic forces
 727     _model.getBody()->reset();
 728     _model.initIteration();
 729     _model.calcForces(&_approachState);
 730 }
 731
 732 void Airplane::applyDragFactor(float factor)
 733 {
 734     float applied = Math::sqrt(factor);
 735     _dragFactor *= applied;
 736     _wing->setDragScale(_wing->getDragScale() * applied);
 737     _tail->setDragScale(_tail->getDragScale() * applied);
 738     int i;
 739     for(i=0; i<_vstabs.size(); i++) {
 740         Wing* w = (Wing*)_vstabs.get(i);
 741         w->setDragScale(w->getDragScale() * applied);
 742     }
 743     for(i=0; i<_surfs.size(); i++) {
 744         Surface* s = (Surface*)_surfs.get(i);
 745         s->setTotalDrag(s->getTotalDrag() * applied);
 746     }
 747 }
 748
 749 void Airplane::applyLiftRatio(float factor)
 750 {
 751     float applied = Math::sqrt(factor);
 752     _liftRatio *= applied;
 753     _wing->setLiftRatio(_wing->getLiftRatio() * applied);
 754     _tail->setLiftRatio(_tail->getLiftRatio() * applied);
 755     int i;
 756     for(i=0; i<_vstabs.size(); i++) {
 757         Wing* w = (Wing*)_vstabs.get(i);
 758         w->setLiftRatio(w->getLiftRatio() * applied);
 759     }
 760 }
 761
 762 float Airplane::clamp(float val, float min, float max)
 763 {
 764     if(val < min) return min;
 765     if(val > max) return max;
 766     return val;
 767 }
 768
 769 float Airplane::normFactor(float f)
 770 {
 771     if(f < 0) f = -f;
 772     if(f < 1) f = 1/f;
 773     return f;
 774 }
 775
 776 void Airplane::solve()
 777 {
 778     static const float ARCMIN = 0.0002909;
 779
 780     float tmp[3];
 781     _solutionIterations = 0;
 782     _failureMsg = 0;
 783     while(1) {
 784         if(_solutionIterations++ > 10000) {
 785             _failureMsg = "Solution failed to converge after 10000 iterations";
 786             return;
 787         }
 788
 789         // Run an iteration at cruise, and extract the needed numbers:
 790         runCruise();
 791
 792         _model.getThrust(tmp);
 793         float thrust = tmp[0];
 794
 795         _model.getBody()->getAccel(tmp);
 796         float xforce = _cruiseWeight * tmp[0];
 797         float clift0 = _cruiseWeight * tmp[2];
 798
 799         _model.getBody()->getAngularAccel(tmp);
 800         float pitch0 = tmp[1];
 801
 802         // Run an approach iteration, and do likewise
 803         runApproach();
 804
 805         _model.getBody()->getAccel(tmp);
 806         float alift = _approachWeight * tmp[2];
 807
 808         // Modify the cruise AoA a bit to get a derivative
 809         _cruiseAoA += ARCMIN;
 810         runCruise();
 811         _cruiseAoA -= ARCMIN;
 812
 813         _model.getBody()->getAccel(tmp);
 814         float clift1 = _cruiseWeight * tmp[2];
 815
 816         // Do the same with the tail incidence
 817         _tail->setIncidence(_tailIncidence + ARCMIN);
 818         runCruise();
 819         _tail->setIncidence(_tailIncidence);
 820
 821         _model.getBody()->getAngularAccel(tmp);
 822         float pitch1 = tmp[1];
 823
 824         // Now calculate:
 825         float awgt = 9.8 * _approachWeight;
 826
 827         float dragFactor = thrust / (thrust-xforce);
 828         float liftFactor = awgt / (awgt+alift);
 829         float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
 830         float tailDelta = -pitch0 * (ARCMIN/(pitch1-pitch0));
 831
 832         // Sanity:
 833         if(dragFactor <= 0) {
 834             _failureMsg = "Zero or negative drag adjustment.";
 835             return;
 836         } else if(liftFactor <= 0) {
 837             _failureMsg = "Zero or negative lift adjustment.";
 838             return;
 839         }
 840
 841         // And apply:
 842         applyDragFactor(dragFactor);
 843         applyLiftRatio(liftFactor);
 844
 845         // DON'T do the following until the above are sane
 846         if(normFactor(dragFactor) > 1.1
 847            || normFactor(liftFactor) > 1.1)
 848         {
 849             continue;
 850         }
 851
 852         // OK, now we can adjust the minor variables
 853         _cruiseAoA += 0.5*aoaDelta;
 854         _tailIncidence += 0.5*tailDelta;
 855
 856         _cruiseAoA = clamp(_cruiseAoA, -.174, .174);
 857         _tailIncidence = clamp(_tailIncidence, -.174, .174);
 858
 859         if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
 860            aoaDelta < .000017   && tailDelta < .000017)
 861         {
 862             break;
 863         }
 864     }
 865
 866     if(_dragFactor < 1e-06 || _dragFactor > 1e6) {
 867         _failureMsg = "Drag factor beyond reasonable bounds.";
 868         return;
 869     } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
 870         _failureMsg = "Lift ratio beyond reasonable bounds.";
 871         return;
 872     } else if(Math::abs(_cruiseAoA) >= .174) {
 873         _failureMsg = "Cruise AoA > 10 degrees";
 874         return;
 875     } else if(Math::abs(_tailIncidence) >= .174) {
 876         _failureMsg = "Tail incidence > 10 degrees";
 877         return;
 878     }
 879 }
 880 }; // namespace yasim