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