-SUBDIRS = Balloon JSBSim LaRCsim UIUCModel
+SUBDIRS = Balloon JSBSim LaRCsim UIUCModel YASim
noinst_LIBRARIES = libFlight.a
--- /dev/null
+#include "Atmosphere.hpp"
+#include "ControlMap.hpp"
+#include "Gear.hpp"
+#include "Math.hpp"
+#include "Glue.hpp"
+#include "RigidBody.hpp"
+#include "Surface.hpp"
+#include "Thruster.hpp"
+
+#include "Airplane.hpp"
+namespace yasim {
+
+// FIXME: hook gear extension into the force calculation somehow...
+
+Airplane::Airplane()
+{
+ _emptyWeight = 0;
+ _pilotPos[0] = _pilotPos[1] = _pilotPos[2] = 0;
+ _wing = 0;
+ _tail = 0;
+ _ballast = 0;
+ _cruiseRho = 0;
+ _cruiseSpeed = 0;
+ _cruiseWeight = 0;
+ _approachRho = 0;
+ _approachSpeed = 0;
+ _approachAoA = 0;
+ _approachWeight = 0;
+
+ _dragFactor = 1;
+ _liftRatio = 1;
+ _cruiseAoA = 0;
+ _tailIncidence = 0;
+}
+
+Airplane::~Airplane()
+{
+ for(int i=0; i<_fuselages.size(); i++)
+ delete (Fuselage*)_fuselages.get(i);
+ for(int i=0; i<_tanks.size(); i++)
+ delete (Tank*)_tanks.get(i);
+ for(int i=0; i<_thrusters.size(); i++)
+ delete (ThrustRec*)_thrusters.get(i);
+ for(int i=0; i<_gears.size(); i++)
+ delete (GearRec*)_gears.get(i);
+ for(int i=0; i<_surfs.size(); i++)
+ delete (Surface*)_surfs.get(i);
+}
+
+void Airplane::iterate(float dt)
+{
+ _model.iterate();
+
+ // Consume fuel
+ // FIXME
+}
+
+ControlMap* Airplane::getControlMap()
+{
+ return &_controls;
+}
+
+Model* Airplane::getModel()
+{
+ return &_model;
+}
+
+void Airplane::getPilotAccel(float* out)
+{
+ State* s = _model.getState();
+
+ // Gravity
+ Glue::geodUp(s->pos, out);
+ Math::mul3(-9.8, out, out);
+
+ // The regular acceleration
+ float tmp[3];
+ Math::mul3(-1, s->acc, tmp);
+ Math::add3(tmp, out, out);
+
+ // Convert to aircraft coordinates
+ Math::vmul33(s->orient, out, out);
+
+ // FIXME: rotational & centripetal acceleration needed
+}
+
+void Airplane::setPilotPos(float* pos)
+{
+ for(int i=0; i<3; i++) _pilotPos[i] = pos[i];
+}
+
+void Airplane::getPilotPos(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _pilotPos[i];
+}
+
+int Airplane::numGear()
+{
+ return _gears.size();
+}
+
+Gear* Airplane::getGear(int g)
+{
+ return ((GearRec*)_gears.get(g))->gear;
+}
+
+void Airplane::setGearState(bool down, float dt)
+{
+ for(int i=0; i<_gears.size(); i++) {
+ GearRec* gr = (GearRec*)_gears.get(i);
+ if(gr->time == 0) {
+ // Non-extensible
+ gr->gear->setExtension(1);
+ gr->surf->setXDrag(1);
+ gr->surf->setYDrag(1);
+ gr->surf->setZDrag(1);
+ continue;
+ }
+
+ float diff = dt / gr->time;
+ if(!down) diff = -diff;
+ float ext = gr->gear->getExtension() + diff;
+ if(ext < 0) ext = 0;
+ if(ext > 1) ext = 1;
+
+ gr->gear->setExtension(ext);
+ gr->surf->setXDrag(ext);
+ gr->surf->setYDrag(ext);
+ gr->surf->setZDrag(ext);
+ }
+}
+
+void Airplane::setApproach(float speed, float altitude)
+{
+ // The zero AoA will become a calculated stall AoA in compile()
+ setApproach(speed, altitude, 0);
+}
+
+void Airplane::setApproach(float speed, float altitude, float aoa)
+{
+ _approachSpeed = speed;
+ _approachRho = Atmosphere::getStdDensity(altitude);
+ _approachAoA = aoa;
+}
+
+void Airplane::setCruise(float speed, float altitude)
+{
+ _cruiseSpeed = speed;
+ _cruiseRho = Atmosphere::getStdDensity(altitude);
+ _cruiseAoA = 0;
+ _tailIncidence = 0;
+}
+
+void Airplane::addApproachControl(int control, float val)
+{
+ Control* c = new Control();
+ c->control = control;
+ c->val = val;
+ _approachControls.add(c);
+}
+
+void Airplane::addCruiseControl(int control, float val)
+{
+ Control* c = new Control();
+ c->control = control;
+ c->val = val;
+ _cruiseControls.add(c);
+}
+
+int Airplane::numTanks()
+{
+ return _tanks.size();
+}
+
+float Airplane::getFuel(int tank)
+{
+ return ((Tank*)_tanks.get(tank))->fill;
+}
+
+float Airplane::getFuelDensity(int tank)
+{
+ return ((Tank*)_tanks.get(tank))->density;
+}
+
+void Airplane::setWeight(float weight)
+{
+ _emptyWeight = weight;
+}
+
+void Airplane::setWing(Wing* wing)
+{
+ _wing = wing;
+}
+
+void Airplane::setTail(Wing* tail)
+{
+ _tail = tail;
+}
+
+void Airplane::addVStab(Wing* vstab)
+{
+ _vstabs.add(vstab);
+}
+
+void Airplane::addFuselage(float* front, float* back, float width)
+{
+ Fuselage* f = new Fuselage();
+ for(int i=0; i<3; i++) {
+ f->front[i] = front[i];
+ f->back[i] = back[i];
+ }
+ f->width = width;
+ _fuselages.add(f);
+}
+
+int Airplane::addTank(float* pos, float cap, float density)
+{
+ Tank* t = new Tank();
+ for(int i=0; i<3; i++) t->pos[i] = pos[i];
+ t->cap = cap;
+ t->fill = cap;
+ t->density = density;
+ t->handle = 0xffffffff;
+ return _tanks.add(t);
+}
+
+void Airplane::addGear(Gear* gear, float transitionTime)
+{
+ GearRec* g = new GearRec();
+ g->gear = gear;
+ g->surf = 0;
+ g->time = transitionTime;
+ _gears.add(g);
+}
+
+void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
+{
+ ThrustRec* t = new ThrustRec();
+ t->thruster = thruster;
+ t->mass = mass;
+ for(int i=0; i<3; i++) t->cg[i] = cg[i];
+ _thrusters.add(t);
+}
+
+void Airplane::addBallast(float* pos, float mass)
+{
+ _model.getBody()->addMass(mass, pos);
+ _ballast += mass;
+}
+
+int Airplane::addWeight(float* pos, float size)
+{
+ WeightRec* wr = new WeightRec();
+ wr->handle = _model.getBody()->addMass(0, pos);
+
+ wr->surf = new Surface();
+ wr->surf->setTotalDrag(size*size);
+ _model.addSurface(wr->surf);
+ _surfs.add(wr->surf);
+
+ return _weights.add(wr);
+}
+
+void Airplane::setWeight(int handle, float mass)
+{
+ WeightRec* wr = (WeightRec*)_weights.get(handle);
+
+ _model.getBody()->setMass(wr->handle, mass);
+
+ // Kill the aerodynamic drag if the mass is exactly zero. This is
+ // how we simulate droppable stores.
+ if(mass == 0) {
+ wr->surf->setXDrag(0);
+ wr->surf->setYDrag(0);
+ wr->surf->setZDrag(0);
+ } else {
+ wr->surf->setXDrag(1);
+ wr->surf->setYDrag(1);
+ wr->surf->setZDrag(1);
+ }
+}
+
+void Airplane::setFuelFraction(float frac)
+{
+ for(int i=0; i<_tanks.size(); i++) {
+ Tank* t = (Tank*)_tanks.get(i);
+ _model.getBody()->setMass(t->handle, t->cap * frac);
+ }
+}
+
+float Airplane::getDragCoefficient()
+{
+ return _dragFactor;
+}
+
+float Airplane::getLiftRatio()
+{
+ return _liftRatio;
+}
+
+float Airplane::getCruiseAoA()
+{
+ return _cruiseAoA;
+}
+
+float Airplane::getTailIncidence()
+{
+ return _tailIncidence;
+}
+
+char* Airplane::getFailureMsg()
+{
+ return _failureMsg;
+}
+
+int Airplane::getSolutionIterations()
+{
+ return _solutionIterations;
+}
+
+void Airplane::setupState(float aoa, float speed, State* s)
+{
+ float cosAoA = Math::cos(aoa);
+ float sinAoA = Math::sin(aoa);
+ s->orient[0] = cosAoA; s->orient[1] = 0; s->orient[2] = sinAoA;
+ s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0;
+ s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
+
+ s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
+
+ for(int i=0; i<3; i++)
+ s->pos[i] = s->rot[i] = s->acc[i] = s->racc[i] = 0;
+
+ // Put us 1m above the origin, or else the gravity computation in
+ // Model goes nuts
+ s->pos[2] = 1;
+}
+
+float Airplane::compileWing(Wing* w)
+{
+ // Make sure it's initialized. The surfaces will pop out with
+ // total drag coefficients equal to their areas, which is what we
+ // want.
+ w->compile();
+
+ float wgt = 0;
+ for(int i=0; i<w->numSurfaces(); i++) {
+ Surface* s = (Surface*)w->getSurface(i);
+ _model.addSurface(s);
+
+ float pos[3];
+ s->getPosition(pos);
+ _model.getBody()->addMass(w->getSurfaceWeight(i), pos);
+ wgt += w->getSurfaceWeight(i);
+ }
+ return wgt;
+}
+
+float Airplane::compileFuselage(Fuselage* f)
+{
+ float wgt = 0;
+ float fwd[3];
+ Math::sub3(f->front, f->back, fwd);
+ float len = Math::mag3(fwd);
+ float wid = f->width;
+ int segs = (int)Math::ceil(len/wid);
+ float segWgt = wid*wid/segs;
+ for(int j=0; j<segs; j++) {
+ float frac = (j+0.5) / segs;
+ float pos[3];
+ Math::mul3(frac, fwd, pos);
+ Math::add3(f->back, pos, pos);
+ _model.getBody()->addMass(segWgt, pos);
+ wgt += segWgt;
+
+ // Make a Surface too
+ Surface* s = new Surface();
+ s->setPosition(pos);
+ float sideDrag = len/wid;
+ s->setYDrag(sideDrag);
+ s->setZDrag(sideDrag);
+ s->setTotalDrag(segWgt);
+
+ // FIXME: fails for fuselages aligned along the Y axis
+ float o[9];
+ float *x=o, *y=o+3, *z=o+6; // nicknames for the axes
+ Math::unit3(fwd, x);
+ y[0] = 0; y[1] = 1; y[2] = 0;
+ Math::cross3(x, y, z);
+ s->setOrientation(o);
+
+ _model.addSurface(s);
+ _surfs.add(s);
+ }
+ return wgt;
+}
+
+// FIXME: should probably add a mass for the gear, too
+void Airplane::compileGear(GearRec* gr)
+{
+ Gear* g = gr->gear;
+
+ // Make a Surface object for the aerodynamic behavior
+ Surface* s = new Surface();
+ gr->surf = s;
+
+ // Put the surface at the half-way point on the gear strut, give
+ // it a drag coefficient equal to a square of the same dimension
+ // (gear are really draggy) and make it symmetric.
+ float pos[3], cmp[3];
+ g->getCompression(cmp);
+ float length = Math::mag3(cmp);
+ g->getPosition(pos);
+ Math::mul3(0.5, cmp, cmp);
+ Math::add3(pos, cmp, pos);
+
+ s->setPosition(pos);
+ s->setTotalDrag(length*length);
+
+ _model.addGear(g);
+ _model.addSurface(s);
+ _surfs.add(s);
+}
+
+void Airplane::compile()
+{
+ double ground[3];
+ ground[0] = 0; ground[1] = 0; ground[2] = 1;
+ _model.setGroundPlane(ground, -100000);
+
+ RigidBody* body = _model.getBody();
+ int firstMass = body->numMasses();
+
+ // Generate the point masses for the plane. Just use unitless
+ // numbers for a first pass, then go back through and rescale to
+ // make the weight right.
+ float aeroWgt = 0;
+
+ // The Wing objects
+ aeroWgt += compileWing(_wing);
+ aeroWgt += compileWing(_tail);
+ for(int i=0; i<_vstabs.size(); i++) {
+ aeroWgt += compileWing((Wing*)_vstabs.get(i));
+ }
+
+ // The fuselage(s)
+ for(int i=0; i<_fuselages.size(); i++) {
+ aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
+ }
+
+ // Count up the absolute weight we have
+ float nonAeroWgt = _ballast;
+ for(int i=0; i<_thrusters.size(); i++)
+ nonAeroWgt += ((ThrustRec*)_thrusters.get(i))->mass;
+
+ // Rescale to the specified empty weight
+ float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
+ for(int i=firstMass; i<body->numMasses(); i++)
+ body->setMass(i, body->getMass(i)*wscale);
+
+ // Add the thruster masses
+ for(int i=0; i<_thrusters.size(); i++) {
+ ThrustRec* t = (ThrustRec*)_thrusters.get(i);
+ body->addMass(t->mass, t->cg);
+ }
+
+ // Add the tanks, empty for now.
+ float totalFuel = 0;
+ for(int i=0; i<_tanks.size(); i++) {
+ Tank* t = (Tank*)_tanks.get(i);
+ t->handle = body->addMass(0, t->pos);
+ totalFuel += t->cap;
+ }
+ _cruiseWeight = _emptyWeight + totalFuel*0.5;
+ _approachWeight = _emptyWeight + totalFuel*0.2;
+
+ body->recalc();
+
+ // Add surfaces for the landing gear.
+ for(int i=0; i<_gears.size(); i++)
+ compileGear((GearRec*)_gears.get(i));
+
+ // The Thruster objects
+ for(int i=0; i<_thrusters.size(); i++) {
+ ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
+ tr->handle = _model.addThruster(tr->thruster);
+ }
+
+ solveGear();
+ solve();
+
+ // Drop the gear (use a really big dt)
+ setGearState(true, 1000000);
+}
+
+void Airplane::solveGear()
+{
+ float cg[3], pos[3];
+ _model.getBody()->getCG(cg);
+
+ // Calculate spring constant weightings for the gear. Weight by
+ // the inverse of the distance to the c.g. in the XY plane, which
+ // should be correct for most gear arrangements. Add 50cm of
+ // "buffer" to keep things from blowing up with aircraft with a
+ // single gear very near the c.g. (AV-8, for example).
+ float total = 0;
+ for(int i=0; i<_gears.size(); i++) {
+ GearRec* gr = (GearRec*)_gears.get(i);
+ Gear* g = gr->gear;
+ g->getPosition(pos);
+ Math::sub3(cg, pos, pos);
+ gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
+ total += gr->wgt;
+ }
+
+ // Renormalize so they sum to 1
+ for(int i=0; i<_gears.size(); i++)
+ ((GearRec*)_gears.get(i))->wgt /= total;
+
+ // The force at max compression should be sufficient to stop a
+ // plane moving downwards at 3x the approach descent rate. Assume
+ // a 3 degree approach.
+ float descentRate = 3*_approachSpeed/19.1;
+
+ // Spread the kinetic energy according to the gear weights. This
+ // will results in an equal compression fraction (not distance) of
+ // each gear.
+ float energy = 0.5*_approachWeight*descentRate*descentRate;
+
+ for(int i=0; i<_gears.size(); i++) {
+ GearRec* gr = (GearRec*)_gears.get(i);
+ float e = energy * gr->wgt;
+ float comp[3];
+ gr->gear->getCompression(comp);
+ float len = Math::mag3(comp);
+
+ // Energy in a spring: e = 0.5 * k * len^2
+ float k = 2 * e / (len*len);
+
+ gr->gear->setSpring(k);
+
+ // Critically damped (too damped, too!)
+ gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt));
+
+ // These are pretty generic
+ gr->gear->setStaticFriction(0.8);
+ gr->gear->setDynamicFriction(0.7);
+ }
+}
+
+void Airplane::stabilizeThrust()
+{
+ float thrust[3], tmp[3];
+ float last = 0;
+ while(1) {
+ thrust[0] = thrust[1] = thrust[2] = 0;
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = _model.getThruster(i);
+ t->integrate(0.033);
+ t->getThrust(tmp);
+ Math::add3(thrust, tmp, thrust);
+ }
+
+ float t = Math::mag3(thrust);
+ float ratio = (t+0.1)/(last+0.1);
+ if(ratio < 1.00001 && ratio > 0.99999)
+ break;
+
+ last = t;
+ }
+}
+
+void Airplane::runCruise()
+{
+ setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
+ _model.setState(&_cruiseState);
+ _model.setAirDensity(_cruiseRho);
+
+ // The control configuration
+ _controls.reset();
+ for(int i=0; i<_cruiseControls.size(); i++) {
+ Control* c = (Control*)_cruiseControls.get(i);
+ _controls.setInput(c->control, c->val);
+ }
+ _controls.applyControls();
+
+ // The local wind
+ float wind[3];
+ Math::mul3(-1, _cruiseState.v, wind);
+ Math::vmul33(_cruiseState.orient, wind, wind);
+
+ // Gear are up (if they're non-retractable, this is a noop)
+ setGearState(false, 100000);
+
+ // Cruise is by convention at 50% tank capacity
+ setFuelFraction(0.5);
+
+ // Set up the thruster parameters and iterate until the thrust
+ // stabilizes.
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
+ t->setWind(wind);
+ t->setDensity(_cruiseRho);
+ }
+ stabilizeThrust();
+
+ // Precompute thrust in the model, and calculate aerodynamic forces
+ _model.getBody()->reset();
+ _model.initIteration();
+ _model.calcForces(&_cruiseState);
+}
+
+void Airplane::runApproach()
+{
+ setupState(_approachAoA, _approachSpeed, &_approachState);
+ _model.setState(&_approachState);
+ _model.setAirDensity(_approachRho);
+
+ // The control configuration
+ _controls.reset();
+ for(int i=0; i<_approachControls.size(); i++) {
+ Control* c = (Control*)_approachControls.get(i);
+ _controls.setInput(c->control, c->val);
+ }
+ _controls.applyControls();
+
+ // The local wind
+ float wind[3];
+ Math::mul3(-1, _approachState.v, wind);
+ Math::vmul33(_approachState.orient, wind, wind);
+
+ // Approach is by convention at 20% tank capacity
+ setFuelFraction(0.2);
+
+ // Gear are down
+ setGearState(true, 100000);
+
+ // Run the thrusters until they get to a stable setting. FIXME:
+ // this is lots of wasted work.
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
+ t->setWind(wind);
+ t->setDensity(_approachRho);
+ }
+ stabilizeThrust();
+
+ // Precompute thrust in the model, and calculate aerodynamic forces
+ _model.getBody()->reset();
+ _model.initIteration();
+ _model.calcForces(&_approachState);
+}
+
+void Airplane::applyDragFactor(float factor)
+{
+ float applied = Math::sqrt(factor);
+ _dragFactor *= applied;
+ _wing->setDragScale(_wing->getDragScale() * applied);
+ _tail->setDragScale(_tail->getDragScale() * applied);
+ for(int i=0; i<_vstabs.size(); i++) {
+ Wing* w = (Wing*)_vstabs.get(i);
+ w->setDragScale(w->getDragScale() * applied);
+ }
+ for(int i=0; i<_surfs.size(); i++) {
+ Surface* s = (Surface*)_surfs.get(i);
+ s->setTotalDrag(s->getTotalDrag() * applied);
+ }
+}
+
+void Airplane::applyLiftRatio(float factor)
+{
+ float applied = Math::sqrt(factor);
+ _liftRatio *= applied;
+ _wing->setLiftRatio(_wing->getLiftRatio() * applied);
+ _tail->setLiftRatio(_tail->getLiftRatio() * applied);
+ for(int i=0; i<_vstabs.size(); i++) {
+ Wing* w = (Wing*)_vstabs.get(i);
+ w->setLiftRatio(w->getLiftRatio() * applied);
+ }
+}
+
+float Airplane::clamp(float val, float min, float max)
+{
+ if(val < min) return min;
+ if(val > max) return max;
+ return val;
+}
+
+float Airplane::normFactor(float f)
+{
+ if(f < 0) f = -f;
+ if(f < 1) f = 1/f;
+ return f;
+}
+
+void Airplane::solve()
+{
+ static const float ARCMIN = 0.0002909;
+
+ float tmp[3];
+ _solutionIterations = 0;
+ _failureMsg = 0;
+ while(1) {
+ if(_solutionIterations++ > 10000) {
+ _failureMsg = "Solution failed to converge after 10000 iterations";
+ return;
+ }
+
+ // Run an iteration at cruise, and extract the needed numbers:
+ runCruise();
+
+ _model.getThrust(tmp);
+ float thrust = tmp[0];
+
+ _model.getBody()->getAccel(tmp);
+ float xforce = _cruiseWeight * tmp[0];
+ float clift0 = _cruiseWeight * tmp[2];
+
+ _model.getBody()->getAngularAccel(tmp);
+ float pitch0 = tmp[1];
+
+ // Run an approach iteration, and do likewise
+ runApproach();
+
+ _model.getBody()->getAccel(tmp);
+ float alift = _approachWeight * tmp[2];
+
+ // Modify the cruise AoA a bit to get a derivative
+ _cruiseAoA += ARCMIN;
+ runCruise();
+ _cruiseAoA -= ARCMIN;
+
+ _model.getBody()->getAccel(tmp);
+ float clift1 = _cruiseWeight * tmp[2];
+
+ // Do the same with the tail incidence
+ _tail->setIncidence(_tailIncidence + ARCMIN);
+ runCruise();
+ _tail->setIncidence(_tailIncidence);
+
+ _model.getBody()->getAngularAccel(tmp);
+ float pitch1 = tmp[1];
+
+ // Now calculate:
+ float awgt = 9.8 * _approachWeight;
+
+ float dragFactor = thrust / (thrust-xforce);
+ float liftFactor = awgt / (awgt+alift);
+ float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
+ float tailDelta = -pitch0 * (ARCMIN/(pitch1-pitch0));
+
+ // Sanity:
+ if(dragFactor <= 0) {
+ _failureMsg = "Zero or negative drag adjustment.";
+ return;
+ } else if(liftFactor <= 0) {
+ _failureMsg = "Zero or negative lift adjustment.";
+ return;
+ }
+
+ // And apply:
+ applyDragFactor(dragFactor);
+ applyLiftRatio(liftFactor);
+
+ // DON'T do the following until the above are sane
+ if(normFactor(dragFactor) > 1.1
+ || normFactor(liftFactor) > 1.1)
+ {
+ continue;
+ }
+
+ // OK, now we can adjust the minor variables
+ _cruiseAoA += 0.5*aoaDelta;
+ _tailIncidence += 0.5*tailDelta;
+
+ _cruiseAoA = clamp(_cruiseAoA, -.174, .174);
+ _tailIncidence = clamp(_tailIncidence, -.174, .174);
+
+ if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
+ aoaDelta < .000017 && tailDelta < .000017)
+ {
+ break;
+ }
+ }
+
+ if(_dragFactor < 1e-06 || _dragFactor > 1e6) {
+ _failureMsg = "Drag factor beyond reasonable bounds.";
+ return;
+ } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
+ _failureMsg = "Lift ratio beyond reasonable bounds.";
+ return;
+ } else if(Math::abs(_cruiseAoA) >= .174) {
+ _failureMsg = "Cruise AoA > 10 degrees";
+ return;
+ } else if(Math::abs(_tailIncidence) >= .174) {
+ _failureMsg = "Tail incidence > 10 degrees";
+ return;
+ }
+}
+}; // namespace yasim
--- /dev/null
+#ifndef _AIRPLANE_HPP
+#define _AIRPLANE_HPP
+
+#include "ControlMap.hpp"
+#include "Model.hpp"
+#include "Wing.hpp"
+#include "util/Vector.hpp"
+
+namespace yasim {
+
+class Gear;
+class Thruster;
+
+class Airplane {
+public:
+ Airplane();
+ ~Airplane();
+
+ void iterate(float dt);
+
+ ControlMap* getControlMap();
+ Model* getModel();
+
+ void setPilotPos(float* pos);
+ void getPilotPos(float* out);
+
+ void getPilotAccel(float* out);
+
+ void setWeight(float weight);
+
+ void setWing(Wing* wing);
+ void setTail(Wing* tail);
+ void addVStab(Wing* vstab);
+
+ void addFuselage(float* front, float* back, float width);
+ int addTank(float* pos, float cap, float fuelDensity);
+ void addGear(Gear* g, float transitionTime);
+ void addThruster(Thruster* t, float mass, float* cg);
+ void addBallast(float* pos, float mass);
+
+ int addWeight(float* pos, float size);
+ void setWeight(int handle, float mass);
+
+ void setApproach(float speed, float altitude);
+ void setApproach(float speed, float altitude, float aoa);
+ void setCruise(float speed, float altitude);
+
+ void addApproachControl(int control, float val);
+ void addCruiseControl(int control, float val);
+
+ int numGear();
+ Gear* getGear(int g);
+ void setGearState(bool down, float dt);
+
+ int numTanks();
+ void setFuelFraction(float frac); // 0-1, total amount of fuel
+ float getFuel(int tank); // in kg!
+ float getFuelDensity(int tank); // kg/m^3
+
+ void compile(); // generate point masses & such, then solve
+
+ // Solution output values
+ int getSolutionIterations();
+ float getDragCoefficient();
+ float getLiftRatio();
+ float getCruiseAoA();
+ float getTailIncidence();
+ char* getFailureMsg();
+
+private:
+ struct Tank { float pos[3]; float cap; float fill;
+ float density; int handle; };
+ struct Fuselage { float front[3], back[3], width; };
+ struct GearRec { Gear* gear; Surface* surf; float wgt; float time; };
+ struct ThrustRec { Thruster* thruster;
+ int handle; float cg[3]; float mass; };
+ struct Control { int control; float val; };
+ struct WeightRec { int handle; Surface* surf; };
+
+ void runCruise();
+ void runApproach();
+ void setupState(float aoa, float speed, State* s);
+ void solveGear();
+ void solve();
+ float compileWing(Wing* w);
+ float compileFuselage(Fuselage* f);
+ void compileGear(GearRec* gr);
+ void stabilizeThrust();
+ void applyDragFactor(float factor);
+ void applyLiftRatio(float factor);
+ float clamp(float val, float min, float max);
+ float normFactor(float f);
+
+ Model _model;
+ ControlMap _controls;
+
+ float _emptyWeight;
+ float _pilotPos[3];
+
+ Wing* _wing;
+ Wing* _tail;
+
+ Vector _fuselages;
+ Vector _vstabs;
+ Vector _tanks;
+ Vector _thrusters;
+ float _ballast;
+
+ Vector _gears;
+ Vector _weights;
+ Vector _surfs; // NON-wing Surfaces
+
+ Vector _cruiseControls;
+ State _cruiseState;
+ float _cruiseRho;
+ float _cruiseSpeed;
+ float _cruiseWeight;
+
+ Vector _approachControls;
+ State _approachState;
+ float _approachRho;
+ float _approachSpeed;
+ float _approachAoA;
+ float _approachWeight;
+
+ int _solutionIterations;
+ float _dragFactor;
+ float _liftRatio;
+ float _cruiseAoA;
+ float _tailIncidence;
+ char* _failureMsg;
+};
+
+}; // namespace yasim
+#endif // _AIRPLANE_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Atmosphere.hpp"
+namespace yasim {
+
+// Copied from McCormick, who got it from "The ARDC Model Atmosphere"
+// Note that there's an error in the text in the first entry,
+// McCormick lists 299.16/101325/1.22500, but those don't agree with
+// R=287. I chose to correct the temperature to 288.20, since 79F is
+// pretty hot for a "standard" atmosphere.
+// meters kelvin kPa kg/m^3
+float Atmosphere::data[][4] = {{ 0, 288.20, 101325, 1.22500 },
+ { 900, 282.31, 90971, 1.12260 },
+ { 1800, 276.46, 81494, 1.02690 },
+ { 2700, 270.62, 72835, 0.93765 },
+ { 3600, 264.77, 64939, 0.85445 },
+ { 4500, 258.93, 57752, 0.77704 },
+ { 5400, 253.09, 51226, 0.70513 },
+ { 6300, 247.25, 45311, 0.63845 },
+ { 7200, 241.41, 39963, 0.57671 },
+ { 8100, 235.58, 35140, 0.51967 },
+ { 9000, 229.74, 30800, 0.46706 },
+ { 9900, 223.91, 26906, 0.41864 },
+ { 10800, 218.08, 23422, 0.37417 },
+ { 11700, 216.66, 20335, 0.32699 },
+ { 12600, 216.66, 17654, 0.28388 },
+ { 13500, 216.66, 15327, 0.24646 },
+ { 14400, 216.66, 13308, 0.21399 },
+ { 15300, 216.66, 11555, 0.18580 },
+ { 16200, 216.66, 10033, 0.16133 },
+ { 17100, 216.66, 8712, 0.14009 },
+ { 18000, 216.66, 7565, 0.12165 },
+ { 18900, 216.66, 6570, 0.10564 }};
+
+float Atmosphere::getStdTemperature(float alt)
+{
+ return getRecord(alt, 1);
+}
+
+float Atmosphere::getStdPressure(float alt)
+{
+ return getRecord(alt, 2);
+}
+
+float Atmosphere::getStdDensity(float alt)
+{
+ return getRecord(alt, 3);
+}
+
+float Atmosphere::calcVEAS(float spd, float pressure, float temp)
+{
+ return 0; //FIXME
+}
+
+float Atmosphere::calcVCAS(float spd, float pressure, float temp)
+{
+ // Stolen shamelessly from JSBSim. Constants that appear:
+ // 2/5 == gamma-1
+ // 5/12 == 1/(gamma+1)
+ // 4/5 == 2*(gamma-1)
+ // 14/5 == 2*gamma
+ // 28/5 == 4*gamma
+ // 144/25 == (gamma+1)^2
+
+ float m2 = calcMach(spd, temp);
+ m2 = m2*m2; // mach^2
+
+ float cp; // pressure coefficient
+ if(m2 < 1) {
+ // (1+(mach^2)/5)^(gamma/(gamma-1))
+ cp = Math::pow(1+0.2*m2, 3.5);
+ } else {
+ float tmp0 = ((144/25.) * m2) / (28/5.*m2 - 4/5.);
+ float tmp1 = ((14/5.) * m2 - (2/5.)) * (5/12.);
+ cp = Math::pow(tmp0, 3.5) * tmp1;
+ }
+
+ // Conditions at sea level
+ float p0 = getStdPressure(0);
+ float rho0 = getStdDensity(0);
+
+ float tmp = Math::pow((pressure/p0)*(cp-1) + 1, (2/7.));
+ return Math::sqrt((7*p0/rho0)*(tmp-1));
+}
+
+float Atmosphere::calcDensity(float pressure, float temp)
+{
+ // P = rho*R*T, R == 287 kPa*m^3 per kg*kelvin for air
+ return pressure / (287 * temp);
+}
+
+float Atmosphere::calcMach(float spd, float temp)
+{
+ return spd / Math::sqrt(1.4 * 287 * temp);
+}
+
+float Atmosphere::getRecord(float alt, int recNum)
+{
+ int hi = (sizeof(data) / (4*sizeof(float))) - 1;
+ int lo = 0;
+
+ // safety valve, clamp to the edges of the table
+ if(alt < data[0][0]) hi=1;
+ else if(alt > data[hi][0]) lo = hi-1;
+
+ // binary search
+ while(1) {
+ if(hi-lo == 1) break;
+ int mid = (hi+lo)>>1;
+ if(alt < data[mid][0]) hi = mid;
+ else lo = mid;
+ }
+
+ // interpolate
+ float frac = (alt - data[lo][0])/(data[hi][0] - data[lo][0]);
+ float a = data[lo][recNum];
+ float b = data[hi][recNum];
+ return a + frac * (b-a);
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _ATMOSPHERE_HPP
+#define _ATMOSPHERE_HPP
+
+namespace yasim {
+
+class Atmosphere {
+public:
+ static float getStdTemperature(float alt);
+ static float getStdPressure(float alt);
+ static float getStdDensity(float alt);
+
+ static float calcVCAS(float spd, float pressure, float temp);
+ static float calcVEAS(float spd, float pressure, float temp);
+ static float calcMach(float spd, float temp);
+ static float calcDensity(float pressure, float temp);
+
+private:
+ static float getRecord(float alt, int idx);
+ static float data[][4];
+};
+
+}; // namespace yasim
+#endif // _ATMOSPHERE_HPP
--- /dev/null
+#ifndef _BODYENVIRONMENT_HPP
+#define _BODYENVIRONMENT_HPP
+
+#include "RigidBody.hpp"
+
+namespace yasim {
+
+// The values for position and orientation within the global reference
+// frame, along with their first and second time derivatives. The
+// orientation is stored as a matrix for simplicity. Note that
+// because it is orthonormal, its inverse is simply its transpose.
+// You can get local->global transformations by calling Math::tmul33()
+// and using the same matrix.
+struct State {
+ double pos[3]; // position
+ float orient[9]; // global->local xform matrix
+ float v[3]; // velocity
+ float rot[3]; // rotational velocity
+ float acc[3]; // acceleration
+ float racc[3]; // rotational acceleration
+
+ // Simple initialization
+ State() {
+ for(int i=0; i<3; i++) {
+ pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
+ for(int j=0; j<3; j++)
+ orient[3*i+j] = i==j ? 1 : 0;
+ }
+ }
+};
+
+//
+// Objects implementing this interface are responsible for calculating
+// external forces on a RigidBody object. These will then be used by
+// an Integrator to decide on a new solution to the state equations,
+// which will be reported to the BodyEnvironment for further action.
+//
+class BodyEnvironment
+{
+public:
+ // This method inspects the "environment" in which a RigidBody
+ // exists, calculates forces and torques on the body, and sets
+ // them via addForce()/addTorque(). Because this method is called
+ // multiple times ("trials") as part of a Runge-Kutta integration,
+ // this is NOT the place to make decisions about anything but the
+ // forces on the object. Note that the acc and racc fields of the
+ // passed-in State object are undefined! (They are calculed BY
+ // this method).
+ virtual void calcForces(State* state) = 0;
+
+ // Called when the RK4 integrator has determined a "real" new
+ // point on the curve of life. Any side-effect producing checks
+ // of body state vs. the environment can happen here (crashes,
+ // etc...).
+ virtual void newState(State* state) = 0;
+};
+
+}; // namespace yasim
+#endif // _BODYENVIRONMENT_HPP
--- /dev/null
+#include "Jet.hpp"
+#include "Thruster.hpp"
+#include "Gear.hpp"
+#include "Wing.hpp"
+#include "Math.hpp"
+
+#include "ControlMap.hpp"
+namespace yasim {
+
+ControlMap::~ControlMap()
+{
+ for(int i=0; i<_inputs.size(); i++) {
+ Vector* v = (Vector*)_inputs.get(i);
+ for(int j=0; j<v->size(); j++)
+ delete (MapRec*)v->get(j);
+ delete v;
+ }
+
+ for(int i=0; i<_outputs.size(); i++) {
+ OutRec* o = (OutRec*)_outputs.get(i);
+ delete[] o->values;
+ delete o;
+ }
+}
+
+int ControlMap::newInput()
+{
+ Vector* v = new Vector();
+ return _inputs.add(v);
+}
+
+void ControlMap::addMapping(int input, int type, void* object, int options)
+{
+ // See if the output object already exists
+ OutRec* out = 0;
+ for(int i=0; i<_outputs.size(); i++) {
+ OutRec* o = (OutRec*)_outputs.get(i);
+ if(o->object == object && o->type == type) {
+ out = o;
+ break;
+ }
+ }
+
+ // Create one if it doesn't
+ if(out == 0) {
+ out = new OutRec();
+ out->type = type;
+ out->object = object;
+ out->n = 0;
+ out->values = 0;
+ _outputs.add(out);
+ }
+
+ // Make space for the new input value
+ int idx = out->n++;
+ delete[] out->values;
+ out->values = new float[out->n];
+
+ // Add the new option tag
+ out->options.add((void*)options);
+
+ // Make a new input record
+ MapRec* map = new MapRec();
+ map->out = out;
+ map->idx = idx;
+
+ // And add it to the approproate vector.
+ Vector* maps = (Vector*)_inputs.get(input);
+ maps->add(map);
+}
+
+void ControlMap::reset()
+{
+ // Set all the values to zero
+ for(int i=0; i<_outputs.size(); i++) {
+ OutRec* o = (OutRec*)_outputs.get(i);
+ for(int j=0; j<o->n; j++)
+ o->values[j] = 0;
+ }
+}
+
+void ControlMap::setInput(int input, float value)
+{
+ Vector* maps = (Vector*)_inputs.get(input);
+ for(int i=0; i<maps->size(); i++) {
+ MapRec* map = (MapRec*)maps->get(i);
+ map->out->values[map->idx] = value;
+ }
+}
+
+void ControlMap::applyControls()
+{
+ for(int outrec=0; outrec<_outputs.size(); outrec++) {
+ OutRec* o = (OutRec*)_outputs.get(outrec);
+
+ // Generate a summed value. Note the check for "split"
+ // control axes like ailerons.
+ float lval = 0, rval = 0;
+ for(int i=0; i<o->n; i++) {
+ float val = o->values[i];
+ int opt = (int)o->options.get(i);
+ if(opt & OPT_SQUARE)
+ val = val * Math::abs(val);
+ if(opt & OPT_INVERT)
+ val = -val;
+ lval += val;
+ if(opt & OPT_SPLIT)
+ rval -= val;
+ else
+ rval += val;
+ }
+
+ void* obj = o->object;
+ switch(o->type) {
+ case THROTTLE: ((Thruster*)obj)->setThrottle(lval); break;
+ case MIXTURE: ((Thruster*)obj)->setMixture(lval); break;
+ case PROP: ((Thruster*)obj)->setPropAdvance(lval); break;
+ case REHEAT: ((Jet*)obj)->setReheat(lval); break;
+ case BRAKE: ((Gear*)obj)->setBrake(lval); break;
+ case STEER: ((Gear*)obj)->setRotation(lval); break;
+ case EXTEND: ((Gear*)obj)->setExtension(lval); break;
+ case SLAT: ((Wing*)obj)->setSlat(lval); break;
+ case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break;
+ case FLAP1: ((Wing*)obj)->setFlap1(lval, rval); break;
+ case SPOILER: ((Wing*)obj)->setSpoiler(lval, rval); break;
+ }
+ }
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _CONTROL_MAP_HPP
+#define _CONTROL_MAP_HPP
+
+#include "util/Vector.hpp"
+
+namespace yasim {
+
+class ControlMap {
+public:
+ ~ControlMap();
+
+ enum OutputType { THROTTLE, MIXTURE, REHEAT, PROP,
+ BRAKE, STEER, EXTEND,
+ INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER };
+
+ static const int OPT_SPLIT = 0x01;
+ static const int OPT_INVERT = 0x02;
+ static const int OPT_SQUARE = 0x04;
+
+ // Returns a new, not-yet-used "input handle" for addMapping and
+ // setInput. This typically corresponds to one user axis.
+ int newInput();
+
+ // Adds a mapping to between input handle and a particular setting
+ // on an output object. The value of output MUST match the type
+ // of object!
+ void addMapping(int input, int output, void* object, int options=0);
+
+ // Resets our accumulated input values. Call before any
+ // setInput() invokations.
+ void reset();
+
+ // Sets the specified input (as returned by newInput) to the
+ // specified value.
+ void setInput(int input, float value);
+
+ // Calculates and applies the settings received since the last reset().
+ void applyControls();
+
+private:
+ struct OutRec { int type; void* object; int n;
+ float* values; Vector options; };
+ struct MapRec { OutRec* out; int idx; };
+
+ // A list of (sub)Vectors containing a bunch of MapRec objects for
+ // each input handle.
+ Vector _inputs;
+
+ // An unordered list of output settings.
+ Vector _outputs;
+};
+
+}; // namespace yasim
+#endif // _CONTROL_MAP_HPP
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <Main/fg_props.hxx>
+
+#include "Jet.hpp"
+#include "Gear.hpp"
+#include "Atmosphere.hpp"
+#include "PropEngine.hpp"
+#include "Propeller.hpp"
+#include "PistonEngine.hpp"
+
+#include "FGFDM.hpp"
+namespace yasim {
+
+// Some conversion factors
+static const float KTS2MPS = 0.514444444444;
+static const float FT2M = 0.3048;
+static const float DEG2RAD = 0.0174532925199;
+static const float RPM2RAD = 0.10471975512;
+static const float LBS2N = 4.44822;
+static const float LBS2KG = 0.45359237;
+static const float CM2GALS = 264.172037284;
+static const float HP2W = 745.700;
+
+// Stubs, so that this can be compiled without the FlightGear
+// binary. What's the best way to handle this?
+
+// float fgGetFloat(char* name, float def) { return 0; }
+// void fgSetFloat(char* name, float val) {}
+
+FGFDM::FGFDM()
+{
+ _nextEngine = 0;
+}
+
+FGFDM::~FGFDM()
+{
+ for(int i=0; i<_axes.size(); i++) {
+ AxisRec* a = (AxisRec*)_axes.get(i);
+ delete[] a->name;
+ delete a;
+ }
+ for(int i=0; i<_pistons.size(); i++) {
+ EngRec* er = (EngRec*)_pistons.get(i);
+ delete[] er->prefix;
+ delete (PropEngine*)er->eng;
+ delete er;
+ }
+ for(int i=0; i<_jets.size(); i++) {
+ EngRec* er = (EngRec*)_pistons.get(i);
+ delete[] er->prefix;
+ delete (Jet*)er->eng;
+ delete er;
+ }
+ for(int i=0; i<_weights.size(); i++) {
+ WeightRec* wr = (WeightRec*)_weights.get(i);
+ delete[] wr->prop;
+ delete wr;
+ }
+
+}
+
+void FGFDM::iterate(float dt)
+{
+ getExternalInput(dt);
+ _airplane.iterate(dt);
+ setOutputProperties();
+}
+
+Airplane* FGFDM::getAirplane()
+{
+ return &_airplane;
+}
+
+void FGFDM::init()
+{
+ // We don't want to use these ties (we set the values ourselves,
+ // and this works only for the first piston engine anyway).
+ fgUntie("/engines/engine[0]/rpm");
+ fgUntie("/engines/engine[0]/egt-degf");
+ fgUntie("/engines/engine[0]/cht-degf");
+ fgUntie("/engines/engine[0]/oil-temperature-degf");
+ fgUntie("/engines/engine[0]/mp-osi");
+ fgUntie("/engines/engine[0]/fuel-flow-gph");
+ fgUntie("/engines/engine[0]/running");
+ fgUntie("/engines/engine[0]/cranking");
+ fgUntie("/consumables/fuel/tank[0]/level-gal_us");
+ fgUntie("/consumables/fuel/tank[1]/level-gal_us");
+
+ // Set these to sane values. We don't support engine start yet.
+ fgSetBool("/engines/engine[0]/running", true);
+ fgSetBool("/engines/engine[0]/cranking", false);
+
+ // Allows the user to start with something other than full fuel
+ _airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1));
+
+ // This has a nasty habit of being false at startup. That's not
+ // good.
+ fgSetBool("/controls/gear-down", true);
+}
+
+// Not the worlds safest parser. But it's short & sweet.
+void FGFDM::startElement(const char* name, const XMLAttributes &atts)
+{
+ XMLAttributes* a = (XMLAttributes*)&atts;
+ float v[3];
+ char buf[64];
+
+ if(eq(name, "airplane")) {
+ _airplane.setWeight(attrf(a, "mass") * LBS2KG);
+ } else if(eq(name, "approach")) {
+ float spd = attrf(a, "speed") * KTS2MPS;
+ float alt = attrf(a, "alt", 0) * FT2M;
+ float aoa = attrf(a, "aoa", 0) * DEG2RAD;
+ _airplane.setApproach(spd, alt, aoa);
+ _cruiseCurr = false;
+ } else if(eq(name, "cruise")) {
+ float spd = attrf(a, "speed") * KTS2MPS;
+ float alt = attrf(a, "alt") * FT2M;
+ _airplane.setCruise(spd, alt);
+ _cruiseCurr = true;
+ } else if(eq(name, "cockpit")) {
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ _airplane.setPilotPos(v);
+ } else if(eq(name, "wing")) {
+ _airplane.setWing(parseWing(a, name));
+ } else if(eq(name, "hstab")) {
+ _airplane.setTail(parseWing(a, name));
+ } else if(eq(name, "vstab")) {
+ _airplane.addVStab(parseWing(a, name));
+ } else if(eq(name, "propeller")) {
+ parsePropeller(a);
+ } else if(eq(name, "jet")) {
+ Jet* j = new Jet();
+ _currObj = j;
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ float mass = attrf(a, "mass") * LBS2KG;
+ j->setDryThrust(attrf(a, "thrust") * LBS2N);
+ j->setPosition(v);
+ _airplane.addThruster(j, mass, v);
+ sprintf(buf, "/engines/engine[%d]", _nextEngine++);
+ EngRec* er = new EngRec();
+ er->eng = j;
+ er->prefix = dup(buf);
+ _jets.add(er);
+ } else if(eq(name, "gear")) {
+ Gear* g = new Gear();
+ _currObj = g;
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ g->setPosition(v);
+ v[0] = 0;
+ v[1] = 0;
+ v[2] = attrf(a, "compression", 1);
+ g->setCompression(v);
+ g->setStaticFriction(attrf(a, "sfric", 0.8));
+ g->setDynamicFriction(attrf(a, "dfric", 0.7));
+ float transitionTime = attrf(a, "retract-time", 0);
+ _airplane.addGear(g, transitionTime);
+ } else if(eq(name, "fuselage")) {
+ float b[3];
+ v[0] = attrf(a, "ax");
+ v[1] = attrf(a, "ay");
+ v[2] = attrf(a, "az");
+ b[0] = attrf(a, "bx");
+ b[1] = attrf(a, "by");
+ b[2] = attrf(a, "bz");
+ _airplane.addFuselage(v, b, attrf(a, "width"));
+ } else if(eq(name, "tank")) {
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ float density = 6.0; // gasoline, in lbs/gal
+ if(a->hasAttribute("jet")) density = 6.72;
+ density *= LBS2KG/CM2GALS;
+ _airplane.addTank(v, attrf(a, "capacity") * LBS2KG, density);
+ } else if(eq(name, "ballast")) {
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ _airplane.addBallast(v, attrf(a, "mass") * LBS2KG);
+ } else if(eq(name, "weight")) {
+ parseWeight(a);
+ } else if(eq(name, "stall")) {
+ Wing* w = (Wing*)_currObj;
+ w->setStall(attrf(a, "aoa") * DEG2RAD);
+ w->setStallWidth(attrf(a, "width", 2) * DEG2RAD);
+ w->setStallPeak(attrf(a, "peak", 1.5));
+ } else if(eq(name, "flap0")) {
+ ((Wing*)_currObj)->setFlap0(attrf(a, "start"), attrf(a, "end"),
+ attrf(a, "lift"), attrf(a, "drag"));
+ } else if(eq(name, "flap1")) {
+ ((Wing*)_currObj)->setFlap1(attrf(a, "start"), attrf(a, "end"),
+ attrf(a, "lift"), attrf(a, "drag"));
+ } else if(eq(name, "slat")) {
+ ((Wing*)_currObj)->setSlat(attrf(a, "start"), attrf(a, "end"),
+ attrf(a, "aoa"), attrf(a, "drag"));
+ } else if(eq(name, "spoiler")) {
+ ((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"),
+ attrf(a, "lift"), attrf(a, "drag"));
+ } else if(eq(name, "actionpt")) {
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ ((Thruster*)_currObj)->setPosition(v);
+ } else if(eq(name, "dir")) {
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+ ((Thruster*)_currObj)->setDirection(v);
+ } else if(eq(name, "control")) {
+ const char* axis = a->getValue("axis");
+ if(a->hasAttribute("output")) {
+ // assert: output type must match _currObj type!
+ const char* output = a->getValue("output");
+ int opt = 0;
+ opt |= a->hasAttribute("split") ? ControlMap::OPT_SPLIT : 0;
+ opt |= a->hasAttribute("invert") ? ControlMap::OPT_INVERT : 0;
+ opt |= a->hasAttribute("square") ? ControlMap::OPT_SQUARE : 0;
+ _airplane.getControlMap()->addMapping(parseAxis(axis),
+ parseOutput(output),
+ _currObj,
+ opt);
+ } else {
+ // assert: must be under a "cruise" or "approach" tag
+ float value = attrf(a, "value", 0);
+ if(_cruiseCurr)
+ _airplane.addCruiseControl(parseAxis(axis), value);
+ else
+ _airplane.addApproachControl(parseAxis(axis), value);
+ }
+ } else {
+ *(int*)0=0; // unexpected tag, boom
+ }
+}
+
+void FGFDM::getExternalInput(float dt)
+{
+ // The control axes
+ ControlMap* cm = _airplane.getControlMap();
+ cm->reset();
+ for(int i=0; i<_axes.size(); i++) {
+ AxisRec* a = (AxisRec*)_axes.get(i);
+ float val = fgGetFloat(a->name, 0);
+ cm->setInput(a->handle, val);
+ }
+ cm->applyControls();
+
+ // Weights
+ for(int i=0; i<_weights.size(); i++) {
+ WeightRec* wr = (WeightRec*)_weights.get(i);
+ _airplane.setWeight(wr->handle, fgGetFloat(wr->prop));
+ }
+
+ // Gear state
+ _airplane.setGearState(fgGetBool("/controls/gear-down"), dt);
+}
+
+void FGFDM::setOutputProperties()
+{
+ char buf[256];
+ for(int i=0; i<_airplane.numTanks(); i++) {
+ sprintf(buf, "/consumables/fuel/tank[%d]/level-gal_us", i);
+ fgSetFloat(buf,
+ CM2GALS*_airplane.getFuel(i)/_airplane.getFuelDensity(i));
+ }
+
+ for(int i=0; i<_pistons.size(); i++) {
+ EngRec* er = (EngRec*)_pistons.get(i);
+ PropEngine* p = (PropEngine*)er->eng;
+
+ sprintf(buf, "%s/rpm", er->prefix);
+ fgSetFloat(buf, p->getOmega() * (30/3.15149265358979));
+
+ sprintf(buf, "%s/fuel-flow-gph", er->prefix);
+ fgSetFloat(buf, p->getFuelFlow() * (3600*2.2/5)); // FIXME, wrong
+ }
+
+ for(int i=0; i<_jets.size(); i++) {
+ EngRec* er = (EngRec*)_jets.get(i);
+ Jet* j = (Jet*)er->eng;
+
+ sprintf(buf, "%s/fuel-flow-gph", er->prefix);
+ fgSetFloat(buf, j->getFuelFlow() * (3600*2.2/6)); // FIXME, wrong
+ }
+}
+
+Wing* FGFDM::parseWing(XMLAttributes* a, const char* type)
+{
+ Wing* w = new Wing();
+
+ float defDihed = 0;
+ if(eq(type, "vstab"))
+ defDihed = 90;
+ else
+ w->setMirror(true);
+
+ float pos[3];
+ pos[0] = attrf(a, "x");
+ pos[1] = attrf(a, "y");
+ pos[2] = attrf(a, "z");
+ w->setBase(pos);
+
+ w->setLength(attrf(a, "length"));
+ w->setChord(attrf(a, "chord"));
+ w->setSweep(attrf(a, "sweep", 0) * DEG2RAD);
+ w->setTaper(attrf(a, "taper", 1));
+ w->setDihedral(attrf(a, "dihedral", defDihed) * DEG2RAD);
+ w->setCamber(attrf(a, "camber", 0));
+ w->setIncidence(attrf(a, "incidence", 0) * DEG2RAD);
+
+ float effect = attrf(a, "effectiveness", 1);
+ w->setDragScale(w->getDragScale()*effect);
+
+ _currObj = w;
+ return w;
+}
+
+void FGFDM::parsePropeller(XMLAttributes* a)
+{
+ float cg[3];
+ cg[0] = attrf(a, "x");
+ cg[1] = attrf(a, "y");
+ cg[2] = attrf(a, "z");
+ float mass = attrf(a, "mass") * LBS2KG;
+ float moment = attrf(a, "moment");
+ float radius = attrf(a, "radius");
+ float speed = attrf(a, "cruise-speed") * KTS2MPS;
+ float omega = attrf(a, "cruise-rpm") * RPM2RAD;
+ float rho = Atmosphere::getStdDensity(attrf(a, "alt") * FT2M);
+ float power = attrf(a, "takeoff-power") * HP2W;
+ float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD;
+
+ // FIXME: this is a hack. Find a better way to ask the engine how
+ // much power it can produce under cruise conditions.
+ float cruisePower = (power * (rho/Atmosphere::getStdDensity(0))
+ * (omega/omega0));
+
+ Propeller* prop = new Propeller(radius, speed, omega, rho, cruisePower,
+ omega0, power);
+ PistonEngine* eng = new PistonEngine(power, omega0);
+ PropEngine* thruster = new PropEngine(prop, eng, moment);
+ _airplane.addThruster(thruster, mass, cg);
+
+ char buf[64];
+ sprintf(buf, "/engines/engine[%d]", _nextEngine++);
+ EngRec* er = new EngRec();
+ er->eng = thruster;
+ er->prefix = dup(buf);
+ _pistons.add(er);
+
+ _currObj = thruster;
+}
+
+// Turns a string axis name into an integer for use by the
+// ControlMap. Creates a new axis if this one hasn't been defined
+// yet.
+int FGFDM::parseAxis(const char* name)
+{
+ for(int i=0; i<_axes.size(); i++) {
+ AxisRec* a = (AxisRec*)_axes.get(i);
+ if(eq(a->name, name))
+ return a->handle;
+ }
+
+ // Not there, make a new one.
+ AxisRec* a = new AxisRec();
+ a->name = dup(name);
+ a->handle = _airplane.getControlMap()->newInput();
+ _axes.add(a);
+ return a->handle;
+}
+
+int FGFDM::parseOutput(const char* name)
+{
+ if(eq(name, "THROTTLE")) return ControlMap::THROTTLE;
+ if(eq(name, "MIXTURE")) return ControlMap::MIXTURE;
+ if(eq(name, "REHEAT")) return ControlMap::REHEAT;
+ if(eq(name, "PROP")) return ControlMap::PROP;
+ if(eq(name, "BRAKE")) return ControlMap::BRAKE;
+ if(eq(name, "STEER")) return ControlMap::STEER;
+ if(eq(name, "EXTEND")) return ControlMap::EXTEND;
+ if(eq(name, "INCIDENCE")) return ControlMap::INCIDENCE;
+ if(eq(name, "FLAP0")) return ControlMap::FLAP0;
+ if(eq(name, "FLAP1")) return ControlMap::FLAP1;
+ if(eq(name, "SLAT")) return ControlMap::SLAT;
+ if(eq(name, "SPOILER")) return ControlMap::SPOILER;
+ // error here...
+ return -1;
+}
+
+void FGFDM::parseWeight(XMLAttributes* a)
+{
+ WeightRec* wr = new WeightRec();
+
+ float v[3];
+ v[0] = attrf(a, "x");
+ v[1] = attrf(a, "y");
+ v[2] = attrf(a, "z");
+
+ wr->prop = dup(a->getValue("mass-prop"));
+ wr->size = attrf(a, "size", 0);
+ wr->handle = _airplane.addWeight(v, wr->size);
+
+ _weights.add(wr);
+}
+
+bool FGFDM::eq(const char* a, const char* b)
+{
+ // Figure it out for yourself. :)
+ while(*a && *b && *a++ == *b++);
+ return !(*a || *b);
+}
+
+char* FGFDM::dup(const char* s)
+{
+ int len=0;
+ while(s[len++]);
+ char* s2 = new char[len+1];
+ char* p = s2;
+ while((*p++ = *s++));
+ s2[len] = 0;
+ return s2;
+}
+
+int FGFDM::attri(XMLAttributes* atts, char* attr)
+{
+ if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
+ return attri(atts, attr, 0);
+}
+
+int FGFDM::attri(XMLAttributes* atts, char* attr, int def)
+{
+ const char* val = atts->getValue(attr);
+ if(val == 0) return def;
+ else return atol(val);
+}
+
+float FGFDM::attrf(XMLAttributes* atts, char* attr)
+{
+ if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
+ return attrf(atts, attr, 0);
+}
+
+float FGFDM::attrf(XMLAttributes* atts, char* attr, float def)
+{
+ const char* val = atts->getValue(attr);
+ if(val == 0) return def;
+ else return (float)atof(val);
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _FGFDM_HPP
+#define _FGFDM_HPP
+
+#include <simgear/easyxml.hxx>
+
+#include "Airplane.hpp"
+#include "util/Vector.hpp"
+
+namespace yasim {
+
+class Wing;
+
+// This class forms the "glue" to the FlightGear codebase. It handles
+// parsing of XML airplane files, interfacing to the properties
+// system, and providing data for the use of the FGInterface object.
+class FGFDM : public XMLVisitor {
+public:
+ FGFDM();
+ ~FGFDM();
+ void init();
+ void iterate(float dt);
+
+ Airplane* getAirplane();
+
+ // XML parsing callback from XMLVisitor
+ virtual void startElement(const char* name, const XMLAttributes &atts);
+
+private:
+ struct AxisRec { char* name; int handle; };
+ struct EngRec { char* prefix; void* eng; };
+ struct WeightRec { char* prop; float size; int handle; };
+
+ void getExternalInput(float dt);
+ void setOutputProperties();
+
+ Wing* parseWing(XMLAttributes* a, const char* name);
+ int parseAxis(const char* name);
+ int parseOutput(const char* name);
+ void parseWeight(XMLAttributes* a);
+ void parsePropeller(XMLAttributes* a);
+ bool eq(const char* a, const char* b);
+ char* dup(const char* s);
+ int attri(XMLAttributes* atts, char* attr);
+ int attri(XMLAttributes* atts, char* attr, int def);
+ float attrf(XMLAttributes* atts, char* attr);
+ float attrf(XMLAttributes* atts, char* attr, float def);
+
+ // The core Airplane object we manage.
+ Airplane _airplane;
+
+ // The list of "axes" that we expect to find as input. These are
+ // typically property names.
+ Vector _axes;
+
+ // Settable weights
+ Vector _weights;
+
+ // Engine types. Contains an EngRec structure.
+ Vector _jets;
+ Vector _pistons;
+
+ // Parsing temporaries
+ void* _currObj;
+ bool _cruiseCurr;
+ int _nextEngine;
+};
+
+}; // namespace yasim
+#endif // _FGFDM_HPP
--- /dev/null
+#include "Math.hpp"
+#include "RigidBody.hpp"
+
+#include "Gear.hpp"
+namespace yasim {
+
+Gear::Gear()
+{
+ for(int i=0; i<3; i++)
+ _pos[i] = _cmpr[i] = 0;
+ _spring = 1;
+ _damp = 0;
+ _sfric = 0.8;
+ _dfric = 0.7;
+ _brake = 0;
+ _rot = 0;
+ _extension = 1;
+}
+
+void Gear::setPosition(float* position)
+{
+ for(int i=0; i<3; i++) _pos[i] = position[i];
+}
+
+void Gear::setCompression(float* compression)
+{
+ for(int i=0; i<3; i++) _cmpr[i] = compression[i];
+}
+
+void Gear::setSpring(float spring)
+{
+ _spring = spring;
+}
+
+void Gear::setDamping(float damping)
+{
+ _damp = damping;
+}
+
+void Gear::setStaticFriction(float sfric)
+{
+ _sfric = sfric;
+}
+
+void Gear::setDynamicFriction(float dfric)
+{
+ _dfric = dfric;
+}
+
+void Gear::setBrake(float brake)
+{
+ _brake = brake;
+}
+
+void Gear::setRotation(float rotation)
+{
+ _rot = rotation;
+}
+
+void Gear::setExtension(float extension)
+{
+ _extension = extension;
+}
+
+void Gear::getPosition(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _pos[i];
+}
+
+void Gear::getCompression(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _cmpr[i];
+}
+
+float Gear::getSpring()
+{
+ return _spring;
+}
+
+float Gear::getDamping()
+{
+ return _damp;
+}
+
+float Gear::getStaticFriction()
+{
+ return _sfric;
+}
+
+float Gear::getDynamicFriction()
+{
+ return _dfric;
+}
+
+float Gear::getBrake()
+{
+ return _brake;
+}
+
+float Gear::getRotation()
+{
+ return _rot;
+}
+
+float Gear::getExtension()
+{
+ return _extension;
+}
+
+void Gear::getForce(float* force, float* contact)
+{
+ Math::set3(_force, force);
+ Math::set3(_contact, contact);
+}
+
+float Gear::getWoW()
+{
+ return _wow;
+}
+
+float Gear::getCompressFraction()
+{
+ return _frac;
+}
+
+void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
+{
+ // Init the return values
+ for(int i=0; i<3; i++) _force[i] = _contact[i] = 0;
+
+ // Don't bother if it's not down
+ if(_extension < 1)
+ return;
+
+ float tmp[3];
+
+ // First off, make sure that the gear "tip" is below the ground.
+ // If it's not, there's no force.
+ float a = ground[3] - Math::dot3(_pos, ground);
+ if(a > 0)
+ return;
+
+ // Now a is the distance from the tip to ground, so make b the
+ // distance from the base to ground. We can get the fraction
+ // (0-1) of compression from a/(a-b). Note the minus sign -- stuff
+ // above ground is negative.
+ Math::add3(_cmpr, _pos, tmp);
+ float b = ground[3] - Math::dot3(tmp, ground);
+
+ // Calculate the point of ground _contact.
+ _frac = a/(a-b);
+ if(b < 0) _frac = 1;
+ for(int i=0; i<3; i++)
+ _contact[i] = _pos[i] + _frac*_cmpr[i];
+
+ // Turn _cmpr into a unit vector and a magnitude
+ float cmpr[3];
+ float clen = Math::mag3(_cmpr);
+ Math::mul3(1/clen, _cmpr, cmpr);
+
+ // Now get the velocity of the point of contact
+ float cv[3];
+ body->pointVelocity(_contact, rot, cv);
+ Math::add3(cv, v, cv);
+
+ // Finally, we can start adding up the forces. First the
+ // spring compression (note the clamping of _frac to 1):
+ _frac = (_frac > 1) ? 1 : _frac;
+ float fmag = _frac*clen*_spring;
+ Math::mul3(fmag, cmpr, _force);
+
+ // Then the damping. Use the only the velocity into the ground
+ // (projection along "ground") projected along the compression
+ // axis. So Vdamp = ground*(ground dot cv) dot cmpr
+ Math::mul3(Math::dot3(ground, cv), ground, tmp);
+ float dv = Math::dot3(cmpr, tmp);
+ float damp = _damp * dv;
+ if(damp > fmag) damp = fmag; // can't pull the plane down!
+ if(damp < -fmag) damp = -fmag; // sanity
+ Math::mul3(-damp, cmpr, tmp);
+ Math::add3(_force, tmp, _force);
+
+ _wow = fmag - damp;
+
+ // Wheels are funky. Split the velocity along the ground plane
+ // into rolling and skidding components. Assuming small angles,
+ // we generate "forward" and "left" unit vectors (the compression
+ // goes "up") for the gear, make a "steer" direction from these,
+ // and then project it onto the ground plane. Project the
+ // velocity onto the ground plane too, and extract the "steer"
+ // component. The remainder is the skid velocity.
+
+ float gup[3]; // "up" unit vector from the ground
+ Math::set3(ground, gup);
+ Math::mul3(-1, gup, gup);
+
+ float xhat[] = {1,0,0};
+ float steer[3], skid[3];
+ Math::cross3(gup, xhat, skid); // up cross xhat =~ skid
+ Math::unit3(skid, skid); // == skid
+
+ Math::cross3(skid, gup, steer); // skid cross up == steer
+
+ if(_rot != 0) {
+ // Correct for a (small) rotation
+ Math::mul3(_rot, steer, tmp);
+ Math::add3(tmp, skid, skid);
+ Math::unit3(skid, skid);
+ Math::cross3(skid, gup, steer);
+ }
+
+ float vsteer = Math::dot3(cv, steer);
+ float vskid = Math::dot3(cv, skid);
+ float wgt = Math::dot3(_force, gup); // force into the ground
+
+ float fsteer = _brake * calcFriction(wgt, vsteer);
+ float fskid = calcFriction(wgt, vskid);
+ if(vsteer > 0) fsteer = -fsteer;
+ if(vskid > 0) fskid = -fskid;
+
+ // Phoo! All done. Add it up and get out of here.
+ Math::mul3(fsteer, steer, tmp);
+ Math::add3(tmp, _force, _force);
+
+ Math::mul3(fskid, skid, tmp);
+ Math::add3(tmp, _force, _force);
+}
+
+float Gear::calcFriction(float wgt, float v)
+{
+ // How slow is stopped? 50 cm/second?
+ const float STOP = 0.5;
+ const float iSTOP = 1/STOP;
+ v = Math::abs(v);
+ if(v < STOP) return v*iSTOP * wgt * _sfric;
+ else return wgt * _dfric;
+}
+
+}; // namespace yasim
+
--- /dev/null
+#ifndef _GEAR_HPP
+#define _GEAR_HPP
+
+namespace yasim {
+
+class RigidBody;
+
+// A landing gear has the following parameters:
+//
+// position: a point in the aircraft's local coordinate system where the
+// fully-extended wheel will be found.
+// compression: a vector from position where a fully-compressed wheel
+// will be, also in aircraft-local coordinates.
+// spring coefficient: force coefficient along the compression axis, in
+// Newtons per meter.
+// damping coefficient: force per compression speed, in Ns/m
+// static coefficient of friction: force along the ground plane exerted
+// by a motionless wheel. A unitless scalar.
+// dynamic coefficient of friction: force along the ground plane exerted
+// by a sliding/skidding wheel.
+// braking fraction: fraction of the dynamic friction that will be
+// actually exerted by a rolling wheel
+// rotation: the angle from "forward" by which the wheel is rotated
+// around its compression axis. In radians.
+//
+class Gear {
+public:
+ Gear();
+
+ // Externally set values
+ void setPosition(float* position);
+ void setCompression(float* compression);
+ void setSpring(float spring);
+ void setDamping(float damping);
+ void setStaticFriction(float sfric);
+ void setDynamicFriction(float dfric);
+ void setBrake(float brake);
+ void setRotation(float rotation);
+ void setExtension(float extension);
+
+ void getPosition(float* out);
+ void getCompression(float* out);
+ float getSpring();
+ float getDamping();
+ float getStaticFriction();
+ float getDynamicFriction();
+ float getBrake();
+ float getRotation();
+ float getExtension();
+
+ // Takes a velocity of the aircraft relative to ground, a rotation
+ // vector, and a ground plane (all specified in local coordinates)
+ // and make a force and point of application (i.e. ground contact)
+ // available via getForce().
+ void calcForce(RigidBody* body, float* v, float* rot, float* ground);
+
+ // Computed values: total force, weight-on-wheels (force normal to
+ // ground) and compression fraction.
+ void getForce(float* force, float* contact);
+ float getWoW();
+ float getCompressFraction();
+
+private:
+ float calcFriction(float wgt, float v);
+
+ float _pos[3];
+ float _cmpr[3];
+ float _spring;
+ float _damp;
+ float _sfric;
+ float _dfric;
+ float _brake;
+ float _rot;
+ float _extension;
+ float _force[3];
+ float _contact[3];
+ float _wow;
+ float _frac;
+};
+
+}; // namespace yasim
+#endif // _GEAR_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Glue.hpp"
+namespace yasim {
+
+void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
+{
+ // Convert the velocity to the aircraft frame.
+ float v[3];
+ Math::vmul33(s->orient, s->v, v);
+
+ // By convention, positive alpha is an up pitch, and a positive
+ // beta is yawed to the right.
+ *alpha = -Math::atan2(v[2], v[0]);
+ *beta = Math::atan2(v[1], v[0]);
+}
+
+void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
+{
+ // This one is easy, the projection of the rotation vector around
+ // the "up" axis.
+ float up[3];
+ geodUp(s->pos, up);
+ *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
+
+ // A bit harder: the X component of the rotation vector expressed
+ // in airframe coordinates.
+ float lr[3];
+ Math::vmul33(s->orient, s->rot, lr);
+ *roll = lr[0];
+
+ // Hardest: the component of rotation along the direction formed
+ // by the cross product of (and thus perpendicular to) the
+ // aircraft's forward vector (i.e. the first three elements of the
+ // orientation matrix) and the "up" axis.
+ float pitchAxis[3];
+ Math::cross3(s->orient, up, pitchAxis);
+ Math::unit3(pitchAxis, pitchAxis);
+ *pitch = Math::dot3(pitchAxis, s->rot);
+}
+
+void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
+{
+ double x=xyz[0], y=xyz[1], z=xyz[2];
+
+ // Cylindrical radius from the polar axis
+ double rcyl = Math::sqrt(x*x + y*y);
+
+ // In geocentric coordinates, these are just the angles in
+ // cartesian space.
+ *lon = Math::atan2(y, x);
+ *lat = Math::atan2(z, rcyl);
+
+ // To get XYZ coordinate of "ground", we "squash" the cylindric
+ // radius into a coordinate system where the earth is a sphere,
+ // find the fraction of the xyz vector that is above ground.
+ double rsquash = SQUASH * rcyl;
+ double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z);
+ double len = Math::sqrt(x*x + y*y + z*z);
+
+ *alt = len * (1-frac);
+}
+
+void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
+{
+ // Generate a unit vector
+ double rcyl = Math::cos(lat);
+ double x = rcyl*Math::cos(lon);
+ double y = rcyl*Math::sin(lon);
+ double z = Math::sin(lat);
+
+ // Convert to "squashed" space, renormalize the unit vector,
+ // multiply by the polar radius, and back convert to get us the
+ // point of intersection of the unit vector with the surface.
+ // Then just add the altitude.
+ double rtmp = rcyl*SQUASH;
+ double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z);
+ double ztmp = z*renorm;
+ rtmp *= renorm*STRETCH;
+ double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp);
+ len += alt;
+
+ out[0] = x*len;
+ out[1] = y*len;
+ out[2] = z*len;
+}
+
+double Glue::geod2geocLat(double lat)
+{
+ double r = Math::cos(lat)*STRETCH*STRETCH;
+ double z = Math::sin(lat);
+ return Math::atan2(z, r);
+}
+
+double Glue::geoc2geodLat(double lat)
+{
+ double r = Math::cos(lat)*SQUASH*SQUASH;
+ double z = Math::sin(lat);
+ return Math::atan2(z, r);
+}
+
+void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
+{
+ xyz2geoc(xyz, lat, lon, alt);
+ *lat = geoc2geodLat(*lat);
+}
+
+void Glue::geod2xyz(double lat, double lon, double alt, double* out)
+{
+ lat = geod2geocLat(lat);
+ geoc2xyz(lat, lon, alt, out);
+}
+
+void Glue::xyz2nedMat(double lat, double lon, float* out)
+{
+ // Shorthand for our output vectors:
+ float *north = out, *east = out+3, *down = out+6;
+
+ float slat = Math::sin(lat);
+ float clat = Math::cos(lat);
+ float slon = Math::sin(lon);
+ float clon = Math::cos(lon);
+
+ north[0] = -clon * slat;
+ north[1] = -slon * slat;
+ north[2] = clat;
+
+ east[0] = -slon;
+ east[1] = clon;
+ east[2] = 0;
+
+ down[0] = -clon * clat;
+ down[1] = -slon * clat;
+ down[2] = -slat;
+}
+
+void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
+{
+ // To translate a point in aircraft space to the output "NED"
+ // frame, first negate the Y and Z axes (ugh), then roll around
+ // the X axis, then pitch around Y, then point to the correct
+ // heading about Z. Expressed as a matrix multiplication, then,
+ // the transformation from aircraft to local is HPRN. And our
+ // desired output is the inverse (i.e. transpose) of that. Since
+ // all rotations are 2D, they have a simpler form than a generic
+ // rotation and are done out longhand below for efficiency.
+
+ // Init to the identity matrix
+ for(int i=0; i<3; i++)
+ for(int j=0; j<3; j++)
+ out[3*i+j] = (i==j) ? 1 : 0;
+
+ // Negate Y and Z
+ out[4] = out[8] = -1;
+
+ float s = Math::sin(roll);
+ float c = Math::cos(roll);
+ for(int col=0; col<3; col++) {
+ float y=out[col+3], z=out[col+6];
+ out[col+3] = c*y - s*z;
+ out[col+6] = s*y + c*z;
+ }
+
+ s = Math::sin(pitch);
+ c = Math::cos(pitch);
+ for(int col=0; col<3; col++) {
+ float x=out[col], z=out[col+6];
+ out[col] = c*x + s*z;
+ out[col+6] = c*z - s*x;
+ }
+
+ s = Math::sin(hdg);
+ c = Math::cos(hdg);
+ for(int col=0; col<3; col++) {
+ float x=out[col], y=out[col+3];
+ out[col] = c*x - s*y;
+ out[col+3] = s*x + c*y;
+ }
+
+ // Invert:
+ Math::trans33(out, out);
+}
+
+void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
+{
+ // The airplane's "pointing" direction in NED coordinates is vx,
+ // and it's y (left-right) axis is vy.
+ float vx[3], vy[3];
+ vx[0]=o[0], vx[1]=o[1], vx[2]=o[2];
+ vy[0]=o[3], vy[1]=o[4], vy[2]=o[5];
+
+ // The heading is simply the rotation of the projection onto the
+ // XY plane
+ *hdg = Math::atan2(vx[1], vx[0]);
+
+ // The pitch is the angle between the XY plane and vx, remember
+ // that rotations toward positive Z are _negative_
+ float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]);
+ *pitch = -Math::atan2(vx[2], projmag);
+
+ // Roll is a bit harder. Construct an "unrolled" orientation,
+ // where the X axis is the same as the "rolled" one, and the Y
+ // axis is parallel to the XY plane. These two can give you an
+ // "unrolled" Z axis as their cross product. Now, take the "vy"
+ // axis, which points out the left wing. The projections of this
+ // along the "unrolled" YZ plane will give you the roll angle via
+ // atan().
+ float* ux = vx;
+ float uy[3], uz[3];
+
+ uz[0] = 0; uz[1] = 0; uz[2] = 1;
+ Math::cross3(uz, ux, uy);
+ Math::unit3(uy, uy);
+ Math::cross3(ux, uy, uz);
+
+ float py = -Math::dot3(vy, uy);
+ float pz = -Math::dot3(vy, uz);
+ *roll = Math::atan2(pz, py);
+}
+
+void Glue::geodUp(double* pos, float* out)
+{
+ double lat, lon, alt;
+ xyz2geod(pos, &lat, &lon, &alt);
+
+ float slat = Math::sin(lat);
+ float clat = Math::cos(lat);
+ float slon = Math::sin(lon);
+ float clon = Math::cos(lon);
+ out[0] = clon * clat;
+ out[1] = slon * clat;
+ out[2] = slat;
+}
+
+}; // namespace yasim
+
--- /dev/null
+#ifndef _GLUE_HPP
+#define _GLUE_HPP
+
+#include "BodyEnvironment.hpp"
+
+namespace yasim {
+
+// The XYZ coordinate system has Z as the earth's axis, the Y axis
+// pointing out the equator at zero longitude, and the X axis pointing
+// out the middle of the western hemisphere.
+class Glue {
+public:
+ static void calcAlphaBeta(State* s, float* alpha, float* beta);
+
+ // Calculates the instantaneous rotation velocities about each
+ // axis.
+ static void calcEulerRates(State* s,
+ float* roll, float* pitch, float* hdg);
+
+ static void xyz2geoc(double* xyz,
+ double* lat, double* lon, double* alt);
+ static void geoc2xyz(double lat, double lon, double alt,
+ double* out);
+ static void xyz2geod(double* xyz,
+ double* lat, double* lon, double* alt);
+ static void geod2xyz(double lat, double lon, double alt,
+ double* out);
+
+ static double geod2geocLat(double lat);
+ static double geoc2geodLat(double lat);
+
+ // Returns a global to "local" (north, east, down) matrix. Note
+ // that the latitude passed in is geoDETic.
+ static void xyz2nedMat(double lat, double lon, float* out);
+
+ // Conversion between a euler triplet and a matrix that transforms
+ // "local" (north/east/down) coordinates to the aircraft frame.
+ static void euler2orient(float roll, float pitch, float hdg,
+ float* out);
+ static void orient2euler(float* o,
+ float* roll, float* pitch, float* hdg);
+
+ // Returns a geodetic (i.e. gravitational, "level", etc...) "up"
+ // vector for the specified xyz position.
+ static void geodUp(double* pos, float* out);
+
+private:
+
+ // WGS84 numbers
+ static const double EQURAD = 6378137; // equatorial radius
+ static const double STRETCH = 1.003352810665; // equ./polar radius
+
+ // Derived from the above
+ static const double SQUASH = 0.99665839311; // 1/STRETCH
+ static const double POLRAD = 6356823.77346; // EQURAD*SQUASH
+ static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD
+};
+
+}; // namespace yasim
+#endif // _GLUE_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Integrator.hpp"
+namespace yasim {
+
+void Integrator::setBody(RigidBody* body)
+{
+ _body = body;
+}
+
+void Integrator::setEnvironment(BodyEnvironment* env)
+{
+ _env = env;
+}
+
+void Integrator::setInterval(float dt)
+{
+ _dt = dt;
+}
+
+float Integrator::getInterval()
+{
+ return _dt;
+}
+
+void Integrator::setState(State* s)
+{
+ _s = *s;
+}
+
+State* Integrator::getState()
+{
+ return &_s;
+}
+
+// Transforms a "local" vector to a "global" vector (not coordinate!)
+// using the specified orientation.
+void Integrator::l2gVector(float* orient, float* v, float* out)
+{
+ Math::tmul33(_s.orient, v, out);
+}
+
+// Updates a position vector for a body c.g. motion with velocity v
+// over time dt, from orientation o0 to o1. Because the position
+// references the local coordinate origin, but the velocity is that of
+// the c.g., this gets a bit complicated.
+void Integrator::extrapolatePosition(double* pos, float* v, float dt,
+ float* o1, float* o2)
+{
+ // Remember that it's the c.g. that's moving, so account for
+ // changes in orientation. The motion of the coordinate
+ // frame will be l2gOLD(cg) + deltaCG - l2gNEW(cg)
+ float cg[3], tmp[3];
+
+ _body->getCG(cg);
+ l2gVector(o1, cg, cg); // cg = l2gOLD(cg) ("cg0")
+ Math::set3(v, tmp); // tmp = vel
+ Math::mul3(dt, tmp, tmp); // = vel*dt ("deltaCG")
+ Math::add3(cg, tmp, tmp); // = cg0 + deltaCG
+ _body->getCG(cg);
+ l2gVector(o2, cg, cg); // cg = l2gNEW(cg) ("cg1")
+ Math::sub3(tmp, cg, tmp); // tmp = cg0 + deltaCG - cg1
+
+ pos[0] += tmp[0]; // p1 = p0 + (cg0+deltaCG-cg1)
+ pos[1] += tmp[1]; // (positions are doubles, so we
+ pos[2] += tmp[2]; // can't use Math::add3)
+}
+
+#if 0
+// A straight euler integration, for reference. Don't use.
+void Integrator::calcNewInterval()
+{
+ float tmp[3];
+ State s = _s;
+
+ float dt = _dt / 4;
+
+ orthonormalize(_s.orient);
+ for(int i=0; i<4; i++) {
+ _body->reset();
+ _env->calcForces(&s);
+
+ _body->getAccel(s.acc);
+ l2gVector(_s.orient, s.acc, s.acc);
+
+ _body->getAngularAccel(s.racc);
+ l2gVector(_s.orient, s.racc, s.racc);
+
+ float rotmat[9];
+ rotMatrix(s.rot, dt, rotmat);
+ Math::mmul33(_s.orient, rotmat, s.orient);
+
+ extrapolatePosition(s.pos, s.v, dt, _s.orient, s.orient);
+
+ Math::mul3(dt, s.acc, tmp);
+ Math::add3(tmp, s.v, s.v);
+
+ Math::mul3(dt, s.racc, tmp);
+ Math::add3(tmp, s.rot, s.rot);
+
+ _s = s;
+ }
+
+ _env->newState(&_s);
+}
+#endif
+
+void Integrator::calcNewInterval()
+{
+ // In principle, these could be changed for something other than
+ // a 4th order integration. I doubt if anyone cares.
+ const static int NITER=4;
+ static float TIMESTEP[] = { 1.0, 0.5, 0.5, 1.0 };
+ static float WEIGHTS[] = { 6.0, 3.0, 3.0, 6.0 };
+
+ // Scratch pads:
+ double pos[NITER][3]; float vel[NITER][3]; float acc[NITER][3];
+ float ori[NITER][9]; float rot[NITER][3]; float rac[NITER][3];
+
+ float *currVel = _s.v;
+ float *currAcc = _s.acc;
+ float *currRot = _s.rot;
+ float *currRac = _s.racc;
+
+ // First off, sanify the initial orientation
+ orthonormalize(_s.orient);
+
+ for(int i=0; i<NITER; i++) {
+ //
+ // extrapolate forward based on current values of the
+ // derivatives and the ORIGINAL values of the
+ // position/orientation.
+ //
+ float dt = _dt * TIMESTEP[i];
+ float tmp[3];
+
+ // "add" rotation to orientation (generate a rotation matrix)
+ float rotmat[9];
+ rotMatrix(currRot, dt, rotmat);
+ Math::mmul33(_s.orient, rotmat, ori[i]);
+
+ // add velocity to (original!) position
+ for(int j=0; j<3; j++) pos[i][j] = _s.pos[j];
+ extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
+
+ // add acceleration to (original!) velocity
+ Math::set3(currAcc, tmp);
+ Math::mul3(dt, tmp, tmp);
+ Math::add3(_s.v, tmp, vel[i]);
+
+ // add rotational acceleration to rotation
+ Math::set3(currRac, tmp);
+ Math::mul3(dt, tmp, tmp);
+ Math::add3(_s.rot, tmp, rot[i]);
+
+ //
+ // Tell the environment to generate new forces on the body,
+ // extract the accelerations, and convert to vectors in the
+ // global frame.
+ //
+ _body->reset();
+
+ // FIXME. Copying into a state object is clumsy! The
+ // per-coordinate arrays should be changed into a single array
+ // of State objects. Ick.
+ State stmp;
+ for(int j=0; j<3; j++) {
+ stmp.pos[j] = pos[i][j];
+ stmp.v[j] = vel[i][j];
+ stmp.rot[j] = rot[i][j];
+ }
+ for(int j=0; j<9; j++)
+ stmp.orient[j] = ori[i][j];
+ _env->calcForces(&stmp);
+
+ _body->getAccel(acc[i]);
+ _body->getAngularAccel(rac[i]);
+ l2gVector(_s.orient, acc[i], acc[i]);
+ l2gVector(_s.orient, rac[i], rac[i]);
+
+ //
+ // Save the resulting derivatives for the next iteration
+ //
+ currVel = vel[i]; currAcc = acc[i];
+ currRot = rot[i]; currRac = rac[i];
+ }
+
+ // Average the resulting derivatives together according to their
+ // weights. Yes, we're "averaging" rotations, which isn't
+ // stricly correct -- rotations live in a non-cartesian space.
+ // But the space is "locally" cartesian.
+ State derivs;
+ float tot = 0;
+ for(int i=0; i<NITER; i++) {
+ float wgt = WEIGHTS[i];
+ tot += wgt;
+ for(int j=0; j<3; j++) {
+ derivs.v[j] += wgt*vel[i][j]; derivs.rot[j] += wgt*rot[i][j];
+ derivs.acc[j] += wgt*acc[i][j]; derivs.racc[j] += wgt*rac[i][j];
+ }
+ }
+ float itot = 1/tot;
+ for(int i=0; i<3; i++) {
+ derivs.v[i] *= itot; derivs.rot[i] *= itot;
+ derivs.acc[i] *= itot; derivs.racc[i] *= itot;
+ }
+
+ // And finally extrapolate once more, using the averaged
+ // derivatives, to the final position and orientation. This code
+ // is essentially identical to the position extrapolation step
+ // inside the loop.
+
+ // save the starting orientation
+ float orient0[9];
+ for(int i=0; i<9; i++) orient0[i] = _s.orient[i];
+
+ float rotmat[9];
+ rotMatrix(derivs.rot, _dt, rotmat);
+ Math::mmul33(orient0, rotmat, _s.orient);
+
+ extrapolatePosition(_s.pos, derivs.v, _dt, orient0, _s.orient);
+
+ float tmp[3];
+ Math::mul3(_dt, derivs.acc, tmp);
+ Math::add3(_s.v, tmp, _s.v);
+
+ Math::mul3(_dt, derivs.racc, tmp);
+ Math::add3(_s.rot, tmp, _s.rot);
+
+ for(int i=0; i<3; i++) {
+ _s.acc[i] = derivs.acc[i];
+ _s.racc[i] = derivs.racc[i];
+ }
+
+ // Tell the environment about our decision
+ _env->newState(&_s);
+}
+
+// Generates a matrix that rotates about axis r through an angle equal
+// to (|r| * dt). That is, a rotation effected by rotating with rate
+// r for dt "seconds" (or whatever time unit is in use).
+// Implementation shamelessly cribbed from the OpenGL specification.
+//
+// NOTE: we're actually returning the _transpose_ of the rotation
+// matrix! This is becuase we store orientations as global-to-local
+// transformations. Thus, we want to rotate the ROWS of the old
+// matrix to get the new one.
+void Integrator::rotMatrix(float* r, float dt, float* out)
+{
+ // Normalize the vector, and extract the angle through which we'll
+ // rotate.
+ float mag = Math::mag3(r);
+ float angle = dt*mag;
+
+ // Tiny rotations result in degenerate (zero-length) rotation
+ // vectors, so clamp to an identity matrix. 1e-06 radians
+ // per 1/30th of a second (typical dt unit) corresponds to one
+ // revolution per 2.4 days, or significantly less than the
+ // coriolis rotation. And it's still preserves half the floating
+ // point precision of a radian-per-iteration rotation.
+ if(angle < 1e-06) {
+ out[0] = 1; out[1] = 0; out[2] = 0;
+ out[3] = 0; out[4] = 1; out[5] = 0;
+ out[6] = 0; out[7] = 0; out[8] = 1;
+ return;
+ }
+
+ float runit[3];
+ Math::mul3(1/mag, r, runit);
+
+ float s = Math::sin(angle);
+ float c = Math::cos(angle);
+ float c1 = 1-c;
+ float c1rx = c1*runit[0];
+ float c1ry = c1*runit[1];
+ float xx = c1rx*runit[0];
+ float xy = c1rx*runit[1];
+ float xz = c1rx*runit[2];
+ float yy = c1ry*runit[1];
+ float yz = c1ry*runit[2];
+ float zz = c1*runit[2]*runit[2];
+ float xs = runit[0]*s;
+ float ys = runit[1]*s;
+ float zs = runit[2]*s;
+
+ out[0] = xx+c ; out[3] = xy-zs; out[6] = xz+ys;
+ out[1] = xy+zs; out[4] = yy+c ; out[7] = yz-xs;
+ out[2] = xz-ys; out[5] = yz+xs; out[8] = zz+c ;
+}
+
+// Does a Gram-Schmidt orthonormalization on the rows of a
+// global-to-local orientation matrix. The order of normalization
+// here is x, z, y. This is because of the convention of "x" being
+// the forward vector and "z" being up in the body frame. These two
+// vectors are the most important to keep correct.
+void Integrator::orthonormalize(float* m)
+{
+ // The 1st, 2nd and 3rd rows of the matrix store the global frame
+ // equivalents of the x, y, and z unit vectors in the local frame.
+ float *x = m, *y = m+3, *z = m+6;
+
+ Math::unit3(x, x); // x = x/|x|
+
+ float v[3];
+ Math::mul3(Math::dot3(x, z), z, v); // v = z(x dot z)
+ Math::sub3(z, v, z); // z = z - z*(x dot z)
+ Math::unit3(z, z); // z = z/|z|
+
+ Math::cross3(z, x, y); // y = z cross x
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _INTEGRATOR_HPP
+#define _INTEGRATOR_HPP
+
+#include "BodyEnvironment.hpp"
+#include "RigidBody.hpp"
+
+namespace yasim {
+
+//
+// These objects are responsible for extracting force data from a
+// BodyEnvironment object, using a RigidBody object to calculate
+// accelerations, and then tying that all together into a new
+// "solution" of position/orientation/etc... for the body. The method
+// used is a fourth-order Runge-Kutta integration.
+//
+class Integrator
+{
+public:
+ // Sets the RigidBody that will be integrated.
+ void setBody(RigidBody* body);
+
+ // Sets the BodyEnvironment object used to calculate the second
+ // derivatives.
+ void setEnvironment(BodyEnvironment* env);
+
+ // Sets the time interval between steps. Units can be anything,
+ // but they must match the other values (if you specify speed in
+ // m/s, then angular acceleration had better be in 1/s^2 and the
+ // time interval should be in seconds, etc...)
+ void setInterval(float dt);
+ float getInterval();
+
+ // The current state, i.e. initial conditions for the next
+ // integration iteration. Note that the acceleration parameters
+ // in the State object are ignored.
+ State* getState();
+ void setState(State* s);
+
+ // Do a 4th order Runge-Kutta integration over one time interval.
+ // This is the top level of the simulation.
+ void calcNewInterval();
+
+private:
+ void orthonormalize(float* m);
+ void rotMatrix(float* r, float dt, float* out);
+ void l2gVector(float* orient, float* v, float* out);
+ void extrapolatePosition(double* pos, float* v, float dt,
+ float* o1, float* o2);
+
+ BodyEnvironment* _env;
+ RigidBody* _body;
+ float _dt;
+
+ State _s;
+};
+
+}; // namespace yasim
+#endif // _INTEGRATOR_HPP
--- /dev/null
+#include "Atmosphere.hpp"
+#include "Math.hpp"
+#include "Jet.hpp"
+namespace yasim {
+
+Jet::Jet()
+{
+ _rho0 = Atmosphere::getStdDensity(0);
+ _thrust = 0;
+ _reheat = 0;
+}
+
+Thruster* Jet::clone()
+{
+ Jet* j = new Jet();
+ j->_thrust = _thrust;
+ j->_rho0 = _rho0;
+ return j;
+}
+
+void Jet::setDryThrust(float thrust)
+{
+ _thrust = thrust;
+}
+
+void Jet::setReheat(float reheat)
+{
+ _reheat = reheat;
+}
+
+void Jet::getThrust(float* out)
+{
+ float t = _thrust * _throttle * (_rho / _rho0);
+ Math::mul3(t, _dir, out);
+}
+
+void Jet::getTorque(float* out)
+{
+ out[0] = out[1] = out[2] = 0;
+ return;
+}
+
+void Jet::getGyro(float* out)
+{
+ out[0] = out[1] = out[2] = 0;
+ return;
+}
+
+float Jet::getFuelFlow()
+{
+ return 0;
+}
+
+void Jet::integrate(float dt)
+{
+ return;
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _JET_HPP
+#define _JET_HPP
+
+#include "Thruster.hpp"
+
+namespace yasim {
+
+// Incredibly dumb placeholder for a Jet implementation. But it does
+// what's important, which is provide thrust.
+class Jet : public Thruster {
+public:
+ Jet();
+
+ virtual Thruster* clone();
+
+ void setDryThrust(float thrust);
+ void setReheat(float reheat);
+
+ virtual void getThrust(float* out);
+ virtual void getTorque(float* out);
+ virtual void getGyro(float* out);
+ virtual float getFuelFlow();
+ virtual void integrate(float dt);
+
+private:
+ float _thrust;
+ float _rho0;
+ float _reheat;
+};
+
+}; // namespace yasim
+#endif // _JET_HPP
--- /dev/null
+noinst_LIBRARIES = libYASim.a
+
+libYASim_a_SOURCES = YASim.cxx Airplane.cpp Atmosphere.cpp ControlMap.cpp \
+ FGFDM.cpp Gear.cpp Glue.cpp Integrator.cpp Jet.cpp \
+ Math.cpp Model.cpp PistonEngine.cpp Propeller.cpp \
+ PropEngine.cpp RigidBody.cpp Surface.cpp \
+ Thruster.cpp Wing.cpp
+
+INCLUDES += -I$(top_srcdir) -I$(top_srcdir)/src
--- /dev/null
+#include <math.h>
+
+#include "Math.hpp"
+namespace yasim {
+
+float Math::abs(float f)
+{
+ return ::fabs(f);
+}
+
+float Math::sqrt(float f)
+{
+ return ::sqrt(f);
+}
+
+float Math::pow(float base, float exp)
+{
+ return ::pow(base, exp);
+}
+
+float Math::ceil(float f)
+{
+ return ::ceil(f);
+}
+
+float Math::cos(float f)
+{
+ return ::cos(f);
+}
+
+float Math::sin(float f)
+{
+ return ::sin(f);
+}
+
+float Math::tan(float f)
+{
+ return ::tan(f);
+}
+
+float Math::atan2(float y, float x)
+{
+ return ::atan2(y, x);
+}
+
+double Math::abs(double f)
+{
+ return ::fabs(f);
+}
+
+double Math::sqrt(double f)
+{
+ return ::sqrt(f);
+}
+
+double Math::pow(double base, double exp)
+{
+ return ::pow(base, exp);
+}
+
+double Math::ceil(double f)
+{
+ return ::ceil(f);
+}
+
+double Math::cos(double f)
+{
+ return ::cos(f);
+}
+
+double Math::sin(double f)
+{
+ return ::sin(f);
+}
+
+double Math::tan(double f)
+{
+ return ::tan(f);
+}
+
+double Math::atan2(double y, double x)
+{
+ return ::atan2(y, x);
+}
+
+void Math::set3(float* v, float* out)
+{
+ out[0] = v[0];
+ out[1] = v[1];
+ out[2] = v[2];
+}
+
+float Math::dot3(float* a, float* b)
+{
+ return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
+}
+
+void Math::mul3(float scalar, float* v, float* out)
+{
+ out[0] = scalar * v[0];
+ out[1] = scalar * v[1];
+ out[2] = scalar * v[2];
+}
+
+void Math::add3(float* a, float* b, float* out)
+{
+ out[0] = a[0] + b[0];
+ out[1] = a[1] + b[1];
+ out[2] = a[2] + b[2];
+}
+
+void Math::sub3(float* a, float* b, float* out)
+{
+ out[0] = a[0] - b[0];
+ out[1] = a[1] - b[1];
+ out[2] = a[2] - b[2];
+}
+
+float Math::mag3(float* v)
+{
+ return sqrt(dot3(v, v));
+}
+
+
+void Math::unit3(float* v, float* out)
+{
+ float imag = 1/mag3(v);
+ mul3(imag, v, out);
+}
+
+void Math::cross3(float* a, float* b, float* out)
+{
+ float ax=a[0], ay=a[1], az=a[2];
+ float bx=b[0], by=b[1], bz=b[2];
+ out[0] = ay*bz - by*az;
+ out[1] = az*bx - bz*ax;
+ out[2] = ax*by - bx*ay;
+}
+
+void Math::mmul33(float* a, float* b, float* out)
+{
+ float tmp[9];
+ tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
+ tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
+ tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
+
+ tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
+ tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
+ tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
+
+ tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
+ tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
+ tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
+
+ for(int i=0; i<9; i++)
+ out[i] = tmp[i];
+}
+
+void Math::vmul33(float* m, float* v, float* out)
+{
+ float x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[1] + z*m[2];
+ out[1] = x*m[3] + y*m[4] + z*m[5];
+ out[2] = x*m[6] + y*m[7] + z*m[8];
+}
+
+void Math::tmul33(float* m, float* v, float* out)
+{
+ float x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[3] + z*m[6];
+ out[1] = x*m[1] + y*m[4] + z*m[7];
+ out[2] = x*m[2] + y*m[5] + z*m[8];
+}
+
+void Math::invert33(float* m, float* out)
+{
+ // Compute the inverse as the adjoint matrix times 1/(det M).
+ // A, B ... I are the cofactors of a b c
+ // d e f
+ // g h i
+ float a=m[0], b=m[1], c=m[2];
+ float d=m[3], e=m[4], f=m[5];
+ float g=m[6], h=m[7], i=m[8];
+
+ float A = (e*i - h*f);
+ float B = -(d*i - g*f);
+ float C = (d*h - g*e);
+ float D = -(b*i - h*c);
+ float E = (a*i - g*c);
+ float F = -(a*h - g*b);
+ float G = (b*f - e*c);
+ float H = -(a*f - d*c);
+ float I = (a*e - d*b);
+
+ float id = 1/(a*A + b*B + c*C);
+
+ out[0] = id*A; out[1] = id*D; out[2] = id*G;
+ out[3] = id*B; out[4] = id*E; out[5] = id*H;
+ out[6] = id*C; out[7] = id*F; out[8] = id*I;
+}
+
+void Math::trans33(float* m, float* out)
+{
+ // 0 1 2 Elements 0, 4, and 8 are the same
+ // 3 4 5 Swap elements 1/3, 2/6, and 5/7
+ // 6 7 8
+ out[0] = m[0];
+ out[4] = m[4];
+ out[8] = m[8];
+
+ float tmp = m[1];
+ out[1] = m[3];
+ out[3] = tmp;
+
+ tmp = m[2];
+ out[2] = m[6];
+ out[6] = tmp;
+
+ tmp = m[5];
+ out[5] = m[7];
+ out[7] = tmp;
+}
+
+void Math::ortho33(float* x, float* y,
+ float* xOut, float* yOut, float* zOut)
+{
+ float x0[3], y0[3];
+ set3(x, x0);
+ set3(y, y0);
+
+ unit3(x0, xOut);
+ cross3(xOut, y0, zOut);
+ unit3(zOut, zOut);
+ cross3(zOut, xOut, yOut);
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _MATH_HPP
+#define _MATH_HPP
+
+namespace yasim {
+
+class Math
+{
+public:
+ // Simple wrappers around library routines
+ static float abs(float f);
+ static float sqrt(float f);
+ static float pow(float base, float exp);
+ static float ceil(float f);
+ static float sin(float f);
+ static float cos(float f);
+ static float tan(float f);
+ static float atan2(float y, float x);
+
+ // double variants of the above
+ static double abs(double f);
+ static double sqrt(double f);
+ static double pow(double base, double exp);
+ static double ceil(double f);
+ static double sin(double f);
+ static double cos(double f);
+ static double tan(double f);
+ static double atan2(double y, double x);
+
+ // Some 3D vector stuff. In all cases, it is permissible for the
+ // "out" vector to be the same as one of the inputs.
+ static void set3(float* v, float* out);
+ static float dot3(float* a, float* b);
+ static void cross3(float* a, float* b, float* out);
+ static void mul3(float scalar, float* v, float* out);
+ static void add3(float* a, float* b, float* out);
+ static void sub3(float* a, float* b, float* out);
+ static float mag3(float* v);
+ static void unit3(float* v, float* out);
+
+ // Matrix array convention: 0 1 2
+ // 3 4 5
+ // 6 7 8
+
+ // Multiply two matrices
+ static void mmul33(float* a, float* b, float* out);
+
+ // Multiply by vector
+ static void vmul33(float* m, float* v, float* out);
+
+ // Multiply the vector by the matrix transpose. Or pre-multiply the
+ // matrix by v as a row vector. Same thing.
+ static void tmul33(float* m, float* v, float* out);
+
+ // Invert matrix
+ static void invert33(float* m, float* out);
+
+ // Transpose matrix (for an orthonormal orientation matrix, this
+ // is the same as the inverse).
+ static void trans33(float* m, float* out);
+
+ // Generates an orthonormal basis:
+ // xOut becomes the unit vector in the direction of x
+ // yOut is perpendicular to xOut in the x/y plane
+ // zOut becomes the unit vector: (xOut cross yOut)
+ static void ortho33(float* x, float* y,
+ float* xOut, float* yOut, float* zOut);
+};
+
+}; // namespace yasim
+#endif // _MATH_HPP
--- /dev/null
+#include <stdio.h>
+
+#include "Atmosphere.hpp"
+#include "Thruster.hpp"
+#include "Math.hpp"
+#include "RigidBody.hpp"
+#include "Integrator.hpp"
+#include "Propeller.hpp"
+#include "PistonEngine.hpp"
+#include "Gear.hpp"
+#include "Surface.hpp"
+#include "Glue.hpp"
+
+#include "Model.hpp"
+namespace yasim {
+
+void printState(State* s)
+{
+ State tmp = *s;
+ Math::vmul33(tmp.orient, tmp.v, tmp.v);
+ Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
+ Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
+ Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
+
+ printf("\nNEW STATE (LOCAL COORDS)\n");
+ printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
+ printf("o: ");
+ for(int i=0; i<3; i++) {
+ if(i != 0) printf(" ");
+ printf("%6.2f %6.2f %6.2f\n",
+ tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
+ }
+ printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
+ printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
+ printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
+ printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
+}
+
+Model::Model()
+{
+ for(int i=0; i<3; i++) _wind[i] = 0;
+
+ _integrator.setBody(&_body);
+ _integrator.setEnvironment(this);
+
+ // Default value of 30 Hz
+ _integrator.setInterval(1.0/30.0);
+}
+
+Model::~Model()
+{
+ // FIXME: who owns these things? Need a policy
+}
+
+void Model::getThrust(float* out)
+{
+ float tmp[3];
+ out[0] = out[1] = out[2] = 0;
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = (Thruster*)_thrusters.get(i);
+ t->getThrust(tmp);
+ Math::add3(tmp, out, out);
+ }
+}
+
+void Model::initIteration()
+{
+ // Precompute torque and angular momentum for the thrusters
+ for(int i=0; i<3; i++)
+ _gyro[i] = _torque[i] = 0;
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = (Thruster*)_thrusters.get(i);
+
+ // Get the wind velocity at the thruster location
+ float pos[3], v[3];
+ t->getPosition(pos);
+ localWind(pos, _s, v);
+
+ t->setWind(v);
+ t->setDensity(_rho);
+ t->integrate(_integrator.getInterval());
+
+ t->getTorque(v);
+ Math::add3(v, _torque, _torque);
+
+ t->getGyro(v);
+ Math::add3(v, _gyro, _gyro);
+ }
+}
+
+void Model::iterate()
+{
+ initIteration();
+ _body.recalc(); // FIXME: amortize this, somehow
+ _integrator.calcNewInterval();
+}
+
+State* Model::getState()
+{
+ return _s;
+}
+
+void Model::setState(State* s)
+{
+ _integrator.setState(s);
+ _s = _integrator.getState();
+}
+
+RigidBody* Model::getBody()
+{
+ return &_body;
+}
+
+Integrator* Model::getIntegrator()
+{
+ return &_integrator;
+}
+
+Surface* Model::getSurface(int handle)
+{
+ return (Surface*)_surfaces.get(handle);
+}
+
+int Model::addThruster(Thruster* t)
+{
+ return _thrusters.add(t);
+}
+
+Thruster* Model::getThruster(int handle)
+{
+ return (Thruster*)_thrusters.get(handle);
+}
+
+void Model::setThruster(int handle, Thruster* t)
+{
+ _thrusters.set(handle, t);
+}
+
+int Model::addSurface(Surface* surf)
+{
+ return _surfaces.add(surf);
+}
+
+int Model::addGear(Gear* gear)
+{
+ return _gears.add(gear);
+}
+
+// The first three elements are a unit vector pointing from the global
+// origin to the plane, the final element is the distance from the
+// origin (the radius of the earth, in most implementations). So
+// (v dot _ground)-_ground[3] gives the distance AGL.
+void Model::setGroundPlane(double* planeNormal, double fromOrigin)
+{
+ for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
+ _ground[3] = fromOrigin;
+}
+
+void Model::setAirDensity(float rho)
+{
+ _rho = rho;
+}
+
+void Model::setAir(float pressure, float temp)
+{
+ _rho = Atmosphere::calcDensity(pressure, temp);
+}
+
+void Model::setWind(float* wind)
+{
+ Math::set3(wind, _wind);
+}
+
+void Model::calcForces(State* s)
+{
+ // Add in the pre-computed stuff. These values aren't part of the
+ // Runge-Kutta integration (they don't depend on position or
+ // velocity), and are therefore constant across the four calls to
+ // calcForces. They get computed before we begin the integration
+ // step.
+ _body.setGyro(_gyro);
+ _body.addTorque(_torque);
+ for(int i=0; i<_thrusters.size(); i++) {
+ Thruster* t = (Thruster*)_thrusters.get(i);
+ float thrust[3], pos[3];
+ t->getThrust(thrust);
+ t->getPosition(pos);
+ _body.addForce(pos, thrust);
+ }
+
+ // Gravity, convert to a force, then to local coordinates
+ float grav[3];
+ Glue::geodUp(s->pos, grav);
+ Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
+ Math::vmul33(s->orient, grav, grav);
+ _body.addForce(grav);
+
+ // Do each surface, remembering that the local velocity at each
+ // point is different due to rotation.
+ for(int i=0; i<_surfaces.size(); i++) {
+ Surface* sf = (Surface*)_surfaces.get(i);
+
+ // Vsurf = wind - velocity + (rot cross (cg - pos))
+ float vs[3], pos[3];
+ sf->getPosition(pos);
+ localWind(pos, s, vs);
+
+ float force[3], torque[3];
+ sf->calcForce(vs, _rho, force, torque);
+ _body.addForce(pos, force);
+ _body.addTorque(torque);
+ }
+
+ // Get a ground plane in local coordinates. The first three
+ // elements are the normal vector, the final one is the distance
+ // from the local origin along that vector to the ground plane
+ // (negative for objects "above" the ground)
+ float ground[4];
+ ground[3] = localGround(s, ground);
+
+ // Convert the velocity and rotation vectors to local coordinates
+ float lrot[3], lv[3];
+ Math::vmul33(s->orient, s->rot, lrot);
+ Math::vmul33(s->orient, s->v, lv);
+
+ // The landing gear
+ for(int i=0; i<_gears.size(); i++) {
+ float force[3], contact[3];
+ Gear* g = (Gear*)_gears.get(i);
+ g->calcForce(&_body, lv, lrot, ground);
+ g->getForce(force, contact);
+ _body.addForce(contact, force);
+ }
+}
+
+void Model::newState(State* s)
+{
+ _s = s;
+
+ //printState(s);
+
+ // Some simple collision detection
+ float ground[4], pos[3], cmpr[3];
+ ground[3] = localGround(s, ground);
+ for(int i=0; i<_gears.size(); i++) {
+ Gear* g = (Gear*)_gears.get(i);
+ g->getPosition(pos);
+ g->getCompression(cmpr);
+ Math::add3(cmpr, pos, pos);
+ float dist = ground[3] - Math::dot3(pos, ground);
+ if(dist < 0) {
+ printf("CRASH: gear %d\n", i);
+ *(int*)0=0;
+ }
+ }
+}
+
+// Returns a unit "down" vector for the ground in out, and the
+// distance from the local origin to the ground as the return value.
+// So for a given position V, "dist - (V dot out)" will be the height
+// AGL.
+float Model::localGround(State* s, float* out)
+{
+ // Get the ground's "down" vector, this can be in floats, because
+ // we don't need positioning accuracy. The direction has plenty
+ // of accuracy after truncation.
+ out[0] = -(float)_ground[0];
+ out[1] = -(float)_ground[1];
+ out[2] = -(float)_ground[2];
+ Math::vmul33(s->orient, out, out);
+
+ // The distance from the ground to the Aircraft's origin:
+ double dist = (s->pos[0]*_ground[0]
+ + s->pos[1]*_ground[1]
+ + s->pos[2]*_ground[2] - _ground[3]);
+
+ return (float)dist;
+}
+
+// Calculates the airflow direction at the given point and for the
+// specified aircraft velocity.
+void Model::localWind(float* pos, State* s, float* out)
+{
+ // Most of the input is in global coordinates. Fix that.
+ float lwind[3], lrot[3], lv[3];
+ Math::vmul33(s->orient, _wind, lwind);
+ Math::vmul33(s->orient, s->rot, lrot);
+ Math::vmul33(s->orient, s->v, lv);
+
+ _body.pointVelocity(pos, lrot, out); // rotational velocity
+ Math::mul3(-1, out, out); // (negated)
+ Math::add3(lwind, out, out); // + wind
+ Math::sub3(out, lv, out); // - velocity
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _MODEL_HPP
+#define _MODEL_HPP
+
+#include "Integrator.hpp"
+#include "RigidBody.hpp"
+#include "BodyEnvironment.hpp"
+#include "util/Vector.hpp"
+
+namespace yasim {
+
+// Declare the types whose pointers get passed around here
+class Integrator;
+class Thruster;
+class Surface;
+class Gear;
+
+class Model : public BodyEnvironment {
+public:
+ Model();
+ virtual ~Model();
+
+ RigidBody* getBody();
+ Integrator* getIntegrator();
+
+ State* getState();
+ void setState(State* s);
+
+ void iterate();
+
+ // Externally-managed subcomponents
+ int addThruster(Thruster* t);
+ int addSurface(Surface* surf);
+ int addGear(Gear* gear);
+ Surface* getSurface(int handle);
+ Gear* getGear(int handle);
+
+ // Semi-private methods for use by the Airplane solver.
+ Thruster* getThruster(int handle);
+ void setThruster(int handle, Thruster* t);
+ void initIteration();
+ void getThrust(float* out);
+
+ //
+ // Per-iteration settables
+ //
+ void setGroundPlane(double* planeNormal, double fromOrigin);
+ void setWind(float* wind);
+ void setAirDensity(float rho);
+ void setAir(float pressure, float temp);
+
+ // BodyEnvironment callbacks
+ virtual void calcForces(State* s);
+ virtual void newState(State* s);
+
+private:
+ void calcGearForce(Gear* g, float* v, float* rot, float* ground);
+ float gearFriction(float wgt, float v, Gear* g);
+ float localGround(State* s, float* out);
+ void localWind(float* pos, State* s, float* out);
+
+ Integrator _integrator;
+ RigidBody _body;
+
+ Vector _thrusters;
+ Vector _surfaces;
+ Vector _gears;
+
+ double _ground[4];
+ float _rho;
+ float _wind[3];
+
+ // Accumulators for the total internal gyro and engine torque
+ float _gyro[3];
+ float _torque[3];
+
+ State* _s;
+};
+
+}; // namespace yasim
+#endif // _MODEL_HPP
--- /dev/null
+#include "PistonEngine.hpp"
+namespace yasim {
+
+PistonEngine::PistonEngine(float power, float speed)
+{
+ // Presume a BSFC (in lb/hour per HP) of 0.45. In SI that becomes
+ // (2.2 lb/kg, 745.7 W/hp, 3600 sec/hour) 3.69e-07 kg/Ws.
+ _f0 = power * 3.69e-07;
+
+ _P0 = power;
+ _omega0 = speed;
+
+ // We must be at sea level under standard conditions
+ _rho0 = 1.225; // kg/m^3
+
+ // Further presume that takeoff is (duh) full throttle and
+ // peak-power, that means that by our efficiency function, we are
+ // at 11/8 of "ideal" fuel flow.
+ float realFlow = _f0 * (11.0/8.0);
+ _mixCoeff = realFlow * 1.1 / _omega0;
+}
+
+void PistonEngine::setThrottle(float t)
+{
+ _throttle = t;
+}
+
+void PistonEngine::setMixture(float m)
+{
+ _mixture = m;
+}
+
+void PistonEngine::calc(float density, float speed,
+ float* torqueOut, float* fuelFlowOut)
+{
+ // The actual fuel flow
+ float fuel = _mixture * _mixCoeff * speed;
+
+ // manifold air density
+ float rho = density * _throttle;
+
+ // How much fuel could be burned with ideal (i.e. uncorrected!)
+ // combustion.
+ float burnable = _f0 * (rho/_rho0) * (speed/_omega0);
+
+ // Calculate the fuel that actually burns to produce work. The
+ // idea is that less than 5/8 of ideal, we get complete
+ // combustion. We use up all the oxygen at 1 3/8 of ideal (that
+ // is, you need to waste fuel to use all your O2). In between,
+ // interpolate. This vaguely matches a curve I copied out of a
+ // book for a single engine. Shrug.
+ float burned;
+ float r = fuel/burnable;
+ if (burnable == 0) burned = 0;
+ else if(r < .625) burned = fuel;
+ else if(r > 1.375) burned = burnable;
+ else burned = fuel + (burnable-fuel)*(r-.625)*(4.0/3.0);
+
+ // And finally the power is just the reference power scaled by the
+ // amount of fuel burned.
+ float power = _P0 * burned/_f0;
+
+ *torqueOut = power/speed;
+ *fuelFlowOut = fuel;
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _PISTONENGINE_HPP
+#define _PISTONENGINE_HPP
+
+namespace yasim {
+
+class PistonEngine {
+public:
+ // Initializes an engine from known "takeoff" parameters.
+ PistonEngine(float power, float spd);
+
+ void setThrottle(float throttle);
+ void setMixture(float mixture);
+
+ // Calculates power output and fuel flow, based on a given
+ // throttle setting (0-1 corresponding to the fraction of
+ // "available" manifold pressure), mixture (fuel flux per rpm,
+ // 0-1, where 1 is "max rich", or a little bit more than needed
+ // for rated power at sea level)
+ void calc(float density, float speed,
+ float* powerOut, float* fuelFlowOut);
+
+private:
+ float _P0; // reference power setting
+ float _omega0; // " engine speed
+ float _rho0; // " manifold air density
+ float _f0; // "ideal" fuel flow at P0/omega0
+ float _mixCoeff; // fuel flow per omega at full mixture
+
+ // Runtime settables:
+ float _throttle;
+ float _mixture;
+};
+
+}; // namespace yasim
+#endif // _PISTONENGINE_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Propeller.hpp"
+#include "PistonEngine.hpp"
+#include "PropEngine.hpp"
+namespace yasim {
+
+PropEngine::PropEngine(Propeller* prop, PistonEngine* eng, float moment)
+{
+ // Start off at 500rpm, because the start code doesn't exist yet
+ _omega = 52.3;
+ _dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
+
+ _prop = prop;
+ _eng = eng;
+ _moment = moment;
+}
+
+PropEngine::~PropEngine()
+{
+ delete _prop;
+ delete _eng;
+}
+
+float PropEngine::getOmega()
+{
+ return _omega;
+}
+
+Thruster* PropEngine::clone()
+{
+ // FIXME: bloody mess
+ PropEngine* p = new PropEngine(_prop, _eng, _moment);
+ cloneInto(p);
+ p->_prop = new Propeller(*_prop);
+ p->_eng = new PistonEngine(*_eng);
+ return p;
+}
+
+void PropEngine::getThrust(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _thrust[i];
+}
+
+void PropEngine::getTorque(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _torque[i];
+}
+
+void PropEngine::getGyro(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _gyro[i];
+}
+
+float PropEngine::getFuelFlow()
+{
+ return _fuelFlow;
+}
+
+void PropEngine::integrate(float dt)
+{
+ float speed = -Math::dot3(_wind, _dir);
+
+ float propTorque, engTorque, thrust;
+
+ _eng->setThrottle(_throttle);
+ _eng->setMixture(_mixture);
+
+ _prop->calc(_rho, speed, _omega,
+ &thrust, &propTorque);
+ _eng->calc(_rho, _omega, &engTorque, &_fuelFlow);
+
+ // Turn the thrust into a vector and save it
+ Math::mul3(thrust, _dir, _thrust);
+
+ // Euler-integrate the RPM. This doesn't need the full-on
+ // Runge-Kutta stuff.
+ _omega += dt*(engTorque-propTorque)/Math::abs(_moment);
+
+ // FIXME: Integrate the propeller governor here, when that gets
+ // implemented...
+
+ // Store the total angular momentum into _gyro
+ Math::mul3(_omega*_moment, _dir, _gyro);
+
+ // Accumulate the engine torque, it acta on the body as a whole.
+ // (Note: engine torque, not propeller torque. They can be
+ // different, but the difference goes to accelerating the
+ // rotation. It is the engine torque that is felt at the shaft
+ // and works on the body.)
+ float tau = _moment < 0 ? engTorque : -engTorque;
+ Math::mul3(tau, _dir, _torque);
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _PROPENGINE_HPP
+#define _PROPENGINE_HPP
+
+#include "Thruster.hpp"
+
+namespace yasim {
+
+class Propeller;
+class PistonEngine;
+
+class PropEngine : public Thruster {
+public:
+ PropEngine(Propeller* prop, PistonEngine* eng, float moment);
+ virtual ~PropEngine();
+
+ virtual Thruster* clone();
+
+ // Dynamic output
+ virtual void getThrust(float* out);
+ virtual void getTorque(float* out);
+ virtual void getGyro(float* out);
+ virtual float getFuelFlow();
+
+ // Runtime instructions
+ virtual void integrate(float dt);
+
+ float getOmega();
+
+private:
+ float _moment;
+ Propeller* _prop;
+ PistonEngine* _eng;
+
+ float _omega; // RPM, in radians/sec
+ float _thrust[3];
+ float _torque[3];
+ float _gyro[3];
+ float _fuelFlow;
+};
+
+}; // namespace yasim
+#endif // _PROPENGINE_HPP
--- /dev/null
+#include "Atmosphere.hpp"
+#include "Math.hpp"
+#include "Propeller.hpp"
+namespace yasim {
+
+Propeller::Propeller(float radius, float v, float omega,
+ float rho, float power, float omega0,
+ float power0)
+{
+ // Initialize numeric constants:
+ _lambdaPeak = Math::pow(9.0, -1.0/8.0);
+ _beta = 1.0/(Math::pow(9.0, -1.0/8.0) - Math::pow(9.0, -9.0/8.0));
+
+ _r = radius;
+ _etaC = 0.85; // make this settable?
+
+ _J0 = v/(omega*_lambdaPeak);
+
+ float V2 = v*v + (_r*omega)*(_r*omega);
+ _F0 = 2*_etaC*power/(rho*v*V2);
+
+ float stallAngle = 0.25;
+ _lambdaS = _r*(_J0/_r - stallAngle) / _J0;
+
+ // Now compute a correction for zero forward speed to make the
+ // takeoff performance correct.
+ float torque0 = power0/omega0;
+ float thrust, torque;
+ _takeoffCoef = 1;
+ calc(Atmosphere::getStdDensity(0), 0, omega0, &thrust, &torque);
+ _takeoffCoef = torque/torque0;
+}
+
+void Propeller::calc(float density, float v, float omega,
+ float* thrustOut, float* torqueOut)
+{
+ float tipspd = _r*omega;
+ float V2 = v*v + tipspd*tipspd;
+
+ // Clamp v (forward velocity) to zero, now that we've used it to
+ // calculate V (propeller "speed")
+ if(v < 0) v = 0;
+
+ float J = v/omega;
+ float lambda = J/_J0;
+
+ float torque = 0;
+ if(lambda > 1) {
+ lambda = 1.0/lambda;
+ torque = (density*V2*_F0*_J0)/(8*_etaC*_beta*(1-_lambdaPeak));
+ }
+
+ // There's an undefined point at 1. Just offset by a tiny bit to
+ // fix (note: the discontinuity is at EXACTLY one, this is about
+ // the only time in history you'll see me use == on a floating
+ // point number!)
+ if(lambda == 1) lambda = 0.9999;
+
+ // Compute thrust, remembering to clamp lambda to the stall point
+ float lambda2 = lambda < _lambdaS ? _lambdaS : lambda;
+ float thrust = (0.5*density*V2*_F0/(1-_lambdaPeak))*(1-lambda2);
+
+ // Calculate lambda^8
+ float l8 = lambda*lambda; l8 = l8*l8; l8 = l8*l8;
+
+ // thrust/torque ratio
+ float gamma = (_etaC*_beta/_J0)*(1-l8);
+
+ // Correct slow speeds to get the takeoff parameters correct
+ if(lambda < _lambdaPeak) {
+ // This will interpolate takeoffCoef along a descending from 1
+ // at lambda==0 to 0 at the peak, fairing smoothly into the
+ // flat peak.
+ float frac = (lambda - _lambdaPeak)/_lambdaPeak;
+ gamma *= 1 + (_takeoffCoef - 1)*frac*frac;
+ }
+
+ if(torque > 0) {
+ torque -= thrust/gamma;
+ thrust = -thrust;
+ } else {
+ torque = thrust/gamma;
+ }
+
+ *thrustOut = thrust;
+ *torqueOut = torque;
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _PROPELLER_HPP
+#define _PROPELLER_HPP
+
+namespace yasim {
+
+// A generic propeller model. See the TeX documentation for
+// implementation details, this is too hairy to explain in code
+// comments.
+class Propeller
+{
+public:
+ // Initializes a propeller with the specified "cruise" numbers
+ // for airspeed, RPM, power and air density, and two "takeoff"
+ // numbers for RPM and power (with air speed and density being
+ // zero and sea level). RPM values are in radians per second, of
+ // course.
+ Propeller(float radius, float v, float omega, float rho, float power,
+ float omega0, float power0);
+
+ void calc(float density, float v, float omega,
+ float* thrustOut, float* torqueOut);
+
+private:
+ float _r; // characteristic radius
+ float _J0; // zero-thrust advance ratio
+ float _lambdaS; // "propeller stall" normalized advance ratio
+ float _F0; // thrust coefficient
+ float _etaC; // Peak efficiency
+ float _lambdaPeak; // constant, ~0.759835;
+ float _beta; // constant, ~1.48058;
+ float _takeoffCoef; // correction to get the zero-speed torque right
+};
+
+}; // namespace yasim
+#endif // _PROPELLER_HPP
--- /dev/null
+#include "Math.hpp"
+#include "RigidBody.hpp"
+namespace yasim {
+
+RigidBody::RigidBody()
+{
+ // Allocate space for 16 masses initially. More space will be
+ // allocated dynamically.
+ _nMasses = 0;
+ _massesAlloced = 16;
+ _masses = new Mass[_massesAlloced];
+ _gyro[0] = _gyro[1] = _gyro[2] = 0;
+ _spin[0] = _spin[1] = _spin[2] = 0;
+}
+
+RigidBody::~RigidBody()
+{
+ delete[] _masses;
+}
+
+int RigidBody::addMass(float mass, float* pos)
+{
+ // If out of space, reallocate twice as much
+ if(_nMasses == _massesAlloced) {
+ _massesAlloced *= 2;
+ Mass *m2 = new Mass[_massesAlloced];
+ for(int i=0; i<_nMasses; i++)
+ m2[i] = _masses[i];
+ delete[] _masses;
+ _masses = m2;
+ }
+
+ _masses[_nMasses].m = mass;
+ Math::set3(pos, _masses[_nMasses].p);
+ return _nMasses++;
+}
+
+void RigidBody::setMass(int handle, float mass)
+{
+ _masses[handle].m = mass;
+}
+
+void RigidBody::setMass(int handle, float mass, float* pos)
+{
+ _masses[handle].m = mass;
+ Math::set3(pos, _masses[handle].p);
+}
+
+int RigidBody::numMasses()
+{
+ return _nMasses;
+}
+
+float RigidBody::getMass(int handle)
+{
+ return _masses[handle].m;
+}
+
+void RigidBody::getMassPosition(int handle, float* out)
+{
+ out[0] = _masses[handle].p[0];
+ out[1] = _masses[handle].p[1];
+ out[2] = _masses[handle].p[2];
+}
+
+float RigidBody::getTotalMass()
+{
+ return _totalMass;
+}
+
+// Calcualtes the rotational velocity of a particular point. All
+// coordinates are local!
+void RigidBody::pointVelocity(float* pos, float* rot, float* out)
+{
+ Math::sub3(pos, _cg, out); // out = pos-cg
+ Math::cross3(rot, out, out); // = rot cross (pos-cg)
+}
+
+void RigidBody::setGyro(float* angularMomentum)
+{
+ Math::set3(angularMomentum, _gyro);
+}
+
+void RigidBody::recalc()
+{
+ // Calculate the c.g and total mass:
+ _totalMass = 0;
+ _cg[0] = _cg[1] = _cg[2] = 0;
+ for(int i=0; i<_nMasses; i++) {
+ float m = _masses[i].m;
+ _totalMass += m;
+ _cg[0] += m * _masses[i].p[0];
+ _cg[1] += m * _masses[i].p[1];
+ _cg[2] += m * _masses[i].p[2];
+ }
+ Math::mul3(1/_totalMass, _cg, _cg);
+
+ // Now the inertia tensor:
+ for(int i=0; i<9; i++)
+ _I[i] = 0;
+
+ for(int i=0; i<_nMasses; i++) {
+ float m = _masses[i].m;
+
+ float x = _masses[i].p[0] - _cg[0];
+ float y = _masses[i].p[1] - _cg[1];
+ float z = _masses[i].p[2] - _cg[2];
+
+ float xy = m*x*y; float yz = m*y*z; float zx = m*z*x;
+ float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z;
+
+ _I[0] += y2+z2; _I[1] -= xy; _I[2] -= zx;
+ _I[3] -= xy; _I[4] += x2+z2; _I[5] -= yz;
+ _I[6] -= zx; _I[7] -= yz; _I[8] += x2+y2;
+ }
+
+ // And its inverse
+ Math::invert33(_I, _invI);
+}
+
+void RigidBody::reset()
+{
+ _torque[0] = _torque[1] = _torque[2] = 0;
+ _force[0] = _force[1] = _force[2] = 0;
+}
+
+void RigidBody::addForce(float* force)
+{
+ Math::add3(_force, force, _force);
+}
+
+void RigidBody::addTorque(float* torque)
+{
+ Math::add3(_torque, torque, _torque);
+}
+
+void RigidBody::addForce(float* pos, float* force)
+{
+ addForce(force);
+
+ // For a force F at position X, the torque about the c.g C is:
+ // torque = F cross (C - X)
+ float v[3], t[3];
+ Math::sub3(_cg, pos, v);
+ Math::cross3(force, v, t);
+ addTorque(t);
+}
+
+void RigidBody::setBodySpin(float* rotation)
+{
+ Math::set3(rotation, _spin);
+}
+
+void RigidBody::getCG(float* cgOut)
+{
+ Math::set3(_cg, cgOut);
+}
+
+void RigidBody::getAccel(float* accelOut)
+{
+ Math::mul3(1/_totalMass, _force, accelOut);
+}
+
+void RigidBody::getAccel(float* pos, float* accelOut)
+{
+ getAccel(accelOut);
+
+ // Turn the "spin" vector into a normalized spin axis "a" and a
+ // radians/sec scalar "rate".
+ float a[3];
+ float rate = Math::mag3(_spin);
+ Math::set3(_spin, a);
+ Math::mul3(1/rate, a, a);
+
+ float v[3];
+ Math::sub3(_cg, pos, v); // v = cg - pos
+ Math::mul3(Math::dot3(v, a), a, a); // a = a * (v dot a)
+ Math::add3(v, a, v); // v = v + a
+
+ // Now v contains the vector from pos to the rotation axis.
+ // Multiply by the square of the rotation rate to get the linear
+ // acceleration.
+ Math::mul3(rate*rate, v, v);
+ Math::add3(v, accelOut, accelOut);
+}
+
+void RigidBody::getAngularAccel(float* accelOut)
+{
+ // Compute "tau" as the externally applied torque, plus the
+ // counter-torque due to the internal gyro.
+ float tau[3]; // torque
+ Math::cross3(_gyro, _spin, tau);
+ Math::add3(_torque, tau, tau);
+
+ // Now work the equation of motion. Use "v" as a notational
+ // shorthand, as the value isn't an acceleration until the end.
+ float *v = accelOut;
+ Math::vmul33(_I, _spin, v); // v = I*omega
+ Math::cross3(_spin, v, v); // v = omega X I*omega
+ Math::add3(tau, v, v); // v = tau + (omega X I*omega)
+ Math::vmul33(_invI, v, v); // v = invI*(tau + (omega X I*omega))
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _RIGIDBODY_HPP
+#define _RIGIDBODY_HPP
+
+namespace yasim {
+
+//
+// A RigidBody object maintains all "internal" state about an object,
+// accumulates force and torque information from external sources, and
+// calculates the resulting accelerations.
+//
+//
+// Units note: obviously, the choice of mass, time and distance units
+// is up to the user. If you provide mass in kilograms, forces in
+// newtons, and torques in newton-meters, you'll get your
+// accelerations back in m/s^2. The angular units, however, are
+// UNIFORMLY radians. Angular velocities are radians per <time unit>,
+// the angular acceleration you get back is radians per <time unit>^2,
+// and the angular momenta supplied to setGyro must be in radians,
+// too. Radians, not degrees. Don't forget.
+//
+class RigidBody
+{
+public:
+ RigidBody();
+ ~RigidBody();
+
+ // Adds a point mass to the system. Returns a handle so the gyro
+ // can be later modified via setMass().
+ int addMass(float mass, float* pos);
+
+ // Modifies a previously-added point mass (fuel tank running dry,
+ // gear going up, swing wing swinging, pilot bailing out, etc...)
+ void setMass(int handle, float mass);
+ void setMass(int handle, float mass, float* pos);
+
+ int numMasses();
+ float getMass(int handle);
+ void getMassPosition(int handle, float* out);
+ float getTotalMass();
+
+ // The velocity, in local coordinates, of the specified point on a
+ // body rotating about its c.g. with velocity rot.
+ void pointVelocity(float* pos, float* rot, float* out);
+
+ // Sets the "gyroscope" for the body. This is the total
+ // "intrinsic" angular momentum of the body; that is, rotations of
+ // sub-objects, NOT rotation of the whole body within the global
+ // frame. Because angular momentum is additive in this way, we
+ // don't need to specify specific gyro objects; just add all their
+ // momenta together and set it here.
+ void setGyro(float* angularMomentum);
+
+
+ // When masses are moved or changed, this object needs to
+ // regenerate its internal tables. This step is expensive, so
+ // it's exposed to the client who can amortize the call across
+ // multiple changes.
+ void recalc();
+
+ // Resets the current force/torque parameters to zero.
+ void reset();
+
+
+ // Applies a force at the specified position.
+ void addForce(float* pos, float* force);
+
+ // Applies a force at the center of gravity.
+ void addForce(float* force);
+
+ // Adds a torque with the specified axis and magnitude
+ void addTorque(float* torque);
+
+ // Sets the rotation rate of the body (about its c.g.) within the
+ // surrounding environment. This is needed to compute torque on
+ // the body due to the centripetal forces involved in the
+ // rotation. NOTE: the rotation vector, like all other
+ // coordinates used here, is specified IN THE LOCAL COORDINATE
+ // SYSTEM.
+ void setBodySpin(float* rotation);
+
+
+
+ // Returns the center of gravity of the masses, in the body
+ // coordinate system.
+ void getCG(float* cgOut);
+
+ // Returns the acceleration of the body's c.g. relative to the
+ // rest of the world, specified in local coordinates.
+ void getAccel(float* accelOut);
+
+ // Returns the acceleration of a specific location in local
+ // coordinates. If the body is rotating, this will be different
+ // from the c.g. acceleration due to the centripetal accelerations
+ // of points not on the rotation axis.
+ void getAccel(float* pos, float* accelOut);
+
+ // Returns the instantaneous rate of change of the angular
+ // velocity, as a vector in local coordinates.
+ void getAngularAccel(float* accelOut);
+
+private:
+ struct Mass { float m; float p[3]; };
+
+ // Internal "rotational structure"
+ Mass* _masses;
+ int _nMasses;
+ int _massesAlloced;
+ float _totalMass;
+ float _cg[3];
+ float _gyro[3];
+
+ // Inertia tensor, and its inverse. Computed from the above.
+ float _I[9];
+ float _invI[9];
+
+ // Externally determined quantities
+ float _force[3];
+ float _torque[3];
+ float _spin[3];
+};
+
+}; // namespace yasim
+#endif // _RIGIDBODY_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Surface.hpp"
+namespace yasim {
+
+Surface::Surface()
+{
+ // Start in a "sane" mode, so unset stuff doesn't freak us out
+ _c0 = 1;
+ _cx = _cy = _cz = 1;
+ _cz0 = 0;
+ _peaks[0] = _peaks[1] = 1;
+ for(int i=0; i<4; i++)
+ _stalls[i] = _widths[i] = 0;
+ _orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
+ _orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
+ _orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
+
+ _incidence = 0;
+ _slatPos = _spoilerPos = _flapPos = 0;
+ _slatDrag = _spoilerDrag = _flapDrag = 1;
+ _flapLift = 0;
+ _slatAlpha = 0;
+ _spoilerLift = 1;
+}
+
+void Surface::setPosition(float* p)
+{
+ for(int i=0; i<3; i++) _pos[i] = p[i];
+}
+
+void Surface::getPosition(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _pos[i];
+}
+
+void Surface::setChord(float chord)
+{
+ _chord = chord;
+}
+
+void Surface::setTotalDrag(float c0)
+{
+ _c0 = c0;
+}
+
+float Surface::getTotalDrag()
+{
+ return _c0;
+}
+
+void Surface::setXDrag(float cx)
+{
+ _cx = cx;
+}
+
+void Surface::setYDrag(float cy)
+{
+ _cy = cy;
+}
+
+void Surface::setZDrag(float cz)
+{
+ _cz = cz;
+}
+
+void Surface::setBaseZDrag(float cz0)
+{
+ _cz0 = cz0;
+}
+
+void Surface::setStallPeak(int i, float peak)
+{
+ _peaks[i] = peak;
+}
+
+void Surface::setStall(int i, float alpha)
+{
+ _stalls[i] = alpha;
+}
+
+void Surface::setStallWidth(int i, float width)
+{
+ _widths[i] = width;
+}
+
+void Surface::setOrientation(float* o)
+{
+ for(int i=0; i<9; i++)
+ _orient[i] = o[i];
+}
+
+void Surface::setIncidence(float angle)
+{
+ _incidence = angle;
+}
+
+void Surface::setSlatParams(float stallDelta, float dragPenalty)
+{
+ _slatAlpha = stallDelta;
+ _slatDrag = dragPenalty;
+}
+
+void Surface::setFlapParams(float liftAdd, float dragPenalty)
+{
+ _flapLift = liftAdd;
+ _flapDrag = dragPenalty;
+}
+
+void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
+{
+ _spoilerLift = liftPenalty;
+ _spoilerDrag = dragPenalty;
+}
+
+void Surface::setFlap(float pos)
+{
+ _flapPos = pos;
+}
+
+void Surface::setSlat(float pos)
+{
+ _slatPos = pos;
+}
+
+void Surface::setSpoiler(float pos)
+{
+ _spoilerPos = pos;
+}
+
+// Calculate the aerodynamic force given a wind vector v (in the
+// aircraft's "local" coordinates) and an air density rho. Returns a
+// torque about the Y axis, too.
+void Surface::calcForce(float* v, float rho, float* out, float* torque)
+{
+ // Split v into magnitude and direction:
+ float vel = Math::mag3(v);
+
+ // Handle the blowup condition. Zero velocity means zero force by
+ // definition.
+ if(vel == 0) {
+ for(int i=0; i<3; i++) out[i] = torque[i] = 0;
+ return;
+ }
+
+ Math::mul3(1/vel, v, out);
+
+ // Convert to the surface's coordinates
+ Math::vmul33(_orient, out, out);
+
+ // "Rotate" by the incidence angle. Assume small angles, so we
+ // need to diddle only the Z component, X is relatively unchanged
+ // by small rotations.
+ out[2] += _incidence * out[0]; // z' = z + incidence * x
+
+ // Diddle the Z force according to our configuration
+ float stallMul = stallFunc(out);
+ stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
+ float stallLift = (stallMul - 1) * _cz * out[2];
+ float flapLift = _cz * _flapPos * (_flapLift-1);
+
+ out[2] *= _cz; // scaling factor
+ out[2] += _cz*_cz0; // zero-alpha lift
+ out[2] += stallLift;
+ out[2] += flapLift;
+
+ // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
+ // torque) around the Y axis, while flap lift pushes down. Both
+ // forces are considered to act at one third chord from the
+ // center. Convert to local (i.e. airplane) coordiantes and store
+ // into "torque".
+ torque[0] = 0;
+ torque[1] = 0.33 * _chord * (flapLift - (_cz*_cz0 + stallLift));
+ torque[2] = 0;
+ Math::tmul33(_orient, torque, torque);
+
+ // Diddle X (drag) and Y (side force) in the same manner
+ out[0] *= _cx * controlDrag();
+ out[1] *= _cy;
+
+ // Reverse the incidence rotation to get back to surface
+ // coordinates.
+ out[2] -= _incidence * out[0];
+
+ // Convert back to external coordinates
+ Math::tmul33(_orient, out, out);
+
+ // Add in the units to make a real force:
+ float scale = 0.5*rho*vel*vel*_c0;
+ Math::mul3(scale, out, out);
+ Math::mul3(scale, torque, torque);
+}
+
+// Returns a multiplier for the "plain" force equations that
+// approximates an airfoil's lift/stall curve.
+float Surface::stallFunc(float* v)
+{
+ // Sanity check to treat FPU psychopathology
+ if(v[0] == 0) return 1;
+
+ float alpha = Math::abs(v[2]/v[0]);
+
+ // Wacky use of indexing, see setStall*() methods.
+ int fwdBak = v[0] > 0; // set if this is "backward motion"
+ int posNeg = v[2] < 0; // set if the lift is toward -z
+ int i = (fwdBak<<1) | posNeg;
+
+ float stallAlpha = _stalls[i];
+ if(stallAlpha == 0)
+ return 1;
+
+ if(i == 0)
+ stallAlpha += _slatAlpha;
+
+ // Beyond the stall
+ if(alpha > stallAlpha+_widths[i])
+ return 1;
+
+ // (note mask: we want to use the "positive" stall angle here)
+ float scale = 0.5*_peaks[fwdBak]/_stalls[i&2];
+
+ // Before the stall
+ if(alpha <= stallAlpha)
+ return scale;
+
+ // Inside the stall. Compute a cubic interpolation between the
+ // pre-stall "scale" value and the post-stall unity.
+ float frac = (alpha - stallAlpha) / _widths[i];
+ frac = frac*frac*(3-2*frac);
+
+ return scale*(1-frac) + frac;
+}
+
+float Surface::controlDrag()
+{
+ float d = 1;
+ d *= 1 + _spoilerPos * (_spoilerDrag - 1);
+ d *= 1 + _slatPos * (_slatDrag - 1);
+
+ // FIXME: flaps should REDUCE drag when the reduce lift!
+ d *= 1 + Math::abs(_flapPos) * (_flapDrag - 1);
+
+ return d;
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _SURFACE_HPP
+#define _SURFACE_HPP
+
+namespace yasim {
+
+// FIXME: need a "chord" member for calculating moments. Generic
+// forces act at the center, but "pre-stall" lift acts towards the
+// front, and flaps act (in both lift and drag) toward the back.
+class Surface
+{
+public:
+ Surface();
+
+ // Position of this surface in local coords
+ void setPosition(float* p);
+ void getPosition(float* out);
+
+ // Distance scale along the X axis
+ void setChord(float chord);
+
+ // Slats act to move the stall peak by the specified angle, and
+ // increase drag by the multiplier specified.
+ void setSlatParams(float stallDelta, float dragPenalty);
+
+ // Flaps add to lift coefficient, and multiply drag.
+ void setFlapParams(float liftAdd, float dragPenalty);
+
+ // Spoilers reduce the pre-stall lift, and multiply drag.
+ void setSpoilerParams(float liftPenalty, float dragPenalty);
+
+ // Positions for the controls, in the range [0:1]. [-1:1] for
+ // flaps, with positive meaning "force goes towards positive Z"
+ void setFlap(float pos);
+ void setSlat(float pos);
+ void setSpoiler(float pos);
+
+ // local -> Surface coords
+ void setOrientation(float* o);
+
+ // For variable-incidence control surfaces. The angle is a
+ // negative rotation about the surface's Y axis, in radians, so
+ // positive is "up" (i.e. "positive AoA")
+ void setIncidence(float angle);
+
+ void setTotalDrag(float c0);
+ float getTotalDrag();
+
+ void setXDrag(float cx);
+ void setYDrag(float cy);
+ void setZDrag(float cz);
+
+ // zero-alpha Z drag ("camber") specified as a fraction of cz
+ void setBaseZDrag(float cz0);
+
+ // i: 0 == forward, 1 == backwards
+ void setStallPeak(int i, float peak);
+
+ // i: 0 == fwd/+z, 1 == fwd/-z, 2 == rev/+z, 3 == rev/-z
+ void setStall(int i, float alpha);
+ void setStallWidth(int i, float width);
+
+ void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
+
+private:
+ float stallFunc(float* v);
+ float controlDrag();
+
+ float _chord; // X-axis size
+ float _c0; // total force coefficient
+ float _cx; // X-axis force coefficient
+ float _cy; // Y-axis force coefficient
+ float _cz; // Z-axis force coefficient
+ float _cz0; // Z-axis force offset
+ float _peaks[2]; // Stall peak coefficients (fwd, back)
+ float _stalls[4]; // Stall angles (fwd/back, pos/neg)
+ float _widths[4]; // Stall widths " "
+ float _pos[3]; // position in local coords
+ float _orient[9]; // local->surface orthonormal matrix
+
+ float _slatAlpha;
+ float _slatDrag;
+ float _flapLift;
+ float _flapDrag;
+ float _spoilerLift;
+ float _spoilerDrag;
+
+ float _slatPos;
+ float _flapPos;
+ float _spoilerPos;
+ float _incidence;
+};
+
+}; // namespace yasim
+#endif // _SURFACE_HPP
--- /dev/null
+#include "Math.hpp"
+#include "Thruster.hpp"
+namespace yasim {
+
+Thruster::Thruster()
+{
+ _dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
+ for(int i=0; i<3; i++) _pos[i] = _wind[i] = 0;
+ _throttle = 0;
+ _mixture = 0;
+ _propAdvance = 0;
+ _rho = 0;
+}
+
+Thruster::~Thruster()
+{
+}
+
+void Thruster::getPosition(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _pos[i];
+}
+
+void Thruster::setPosition(float* pos)
+{
+ for(int i=0; i<3; i++) _pos[i] = pos[i];
+}
+
+void Thruster::getDirection(float* out)
+{
+ for(int i=0; i<3; i++) out[i] = _dir[i];
+}
+
+void Thruster::setDirection(float* dir)
+{
+ Math::unit3(dir, _dir);
+}
+
+void Thruster::setThrottle(float throttle)
+{
+ _throttle = throttle;
+}
+
+void Thruster::setMixture(float mixture)
+{
+ _mixture = mixture;
+}
+
+void Thruster::setPropAdvance(float propAdvance)
+{
+ _propAdvance = propAdvance;
+}
+
+void Thruster::setWind(float* wind)
+{
+ for(int i=0; i<3; i++) _wind[i] = wind[i];
+}
+
+void Thruster::setDensity(float rho)
+{
+ _rho = rho;
+}
+
+void Thruster::cloneInto(Thruster* out)
+{
+ for(int i=0; i<3; i++) {
+ out->_pos[i] = _pos[i];
+ out->_dir[i] = _dir[i];
+ }
+ out->_throttle = _throttle;
+ out->_mixture = _mixture;
+ out->_propAdvance = _propAdvance;
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _THRUSTER_HPP
+#define _THRUSTER_HPP
+
+namespace yasim {
+
+class Thruster {
+public:
+ Thruster();
+ virtual ~Thruster();
+
+ // Static data
+ void getPosition(float* out);
+ void setPosition(float* pos);
+ void getDirection(float* out);
+ void setDirection(float* dir);
+
+ virtual Thruster* clone()=0;
+
+ // Controls
+ void setThrottle(float throttle);
+ void setMixture(float mixture);
+ void setPropAdvance(float advance);
+
+ // Dynamic output
+ virtual void getThrust(float* out)=0;
+ virtual void getTorque(float* out)=0;
+ virtual void getGyro(float* out)=0;
+ virtual float getFuelFlow()=0;
+
+ // Runtime instructions
+ void setWind(float* wind);
+ void setDensity(float rho);
+ virtual void integrate(float dt)=0;
+
+protected:
+ void cloneInto(Thruster* out);
+
+ float _pos[3];
+ float _dir[3];
+ float _throttle;
+ float _mixture;
+ float _propAdvance;
+
+ float _wind[3];
+ float _rho;
+};
+
+}; // namespace yasim
+#endif // _THRUSTER_HPP
+
--- /dev/null
+#include "Math.hpp"
+#include "Surface.hpp"
+#include "Wing.hpp"
+namespace yasim {
+
+Wing::Wing()
+{
+ _mirror = false;
+ _base[0] = _base[1] = _base[2] = 0;
+ _length = 0;
+ _chord = 0;
+ _taper = 0;
+ _sweep = 0;
+ _dihedral = 0;
+ _stall = 0;
+ _stallWidth = 0;
+ _stallPeak = 0;
+ _camber = 0;
+ _incidence = 0;
+ _dragScale = 1;
+ _liftRatio = 1;
+ _flap0Start = 0;
+ _flap0End = 0;
+ _flap0Lift = 0;
+ _flap0Drag = 0;
+ _flap1Start = 0;
+ _flap1End = 0;
+ _flap1Lift = 0;
+ _flap1Drag = 0;
+ _spoilerStart = 0;
+ _spoilerEnd = 0;
+ _spoilerLift = 0;
+ _spoilerDrag = 0;
+ _slatStart = 0;
+ _slatEnd = 0;
+ _slatAoA = 0;
+ _slatDrag = 0;
+}
+
+Wing::~Wing()
+{
+ for(int i=0; i<_surfs.size(); i++) {
+ SurfRec* s = (SurfRec*)_surfs.get(i);
+ delete s->surface;
+ delete s;
+ }
+}
+
+int Wing::numSurfaces()
+{
+ return _surfs.size();
+}
+
+Surface* Wing::getSurface(int n)
+{
+ return ((SurfRec*)_surfs.get(n))->surface;
+}
+
+float Wing::getSurfaceWeight(int n)
+{
+ return ((SurfRec*)_surfs.get(n))->weight;
+}
+
+void Wing::setMirror(bool mirror)
+{
+ _mirror = mirror;
+}
+
+void Wing::setBase(float* base)
+{
+ for(int i=0; i<3; i++) _base[i] = base[i];
+}
+
+void Wing::setLength(float length)
+{
+ _length = length;
+}
+
+void Wing::setChord(float chord)
+{
+ _chord = chord;
+}
+
+void Wing::setTaper(float taper)
+{
+ _taper = taper;
+}
+
+void Wing::setSweep(float sweep)
+{
+ _sweep = sweep;
+}
+
+void Wing::setDihedral(float dihedral)
+{
+ _dihedral = dihedral;
+}
+
+void Wing::setStall(float aoa)
+{
+ _stall = aoa;
+}
+
+void Wing::setStallWidth(float angle)
+{
+ _stallWidth = angle;
+}
+
+void Wing::setStallPeak(float fraction)
+{
+ _stallPeak = fraction;
+}
+
+void Wing::setCamber(float camber)
+{
+ _camber = camber;
+}
+
+void Wing::setIncidence(float incidence)
+{
+ _incidence = incidence;
+ for(int i=0; i<_surfs.size(); i++)
+ ((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
+}
+
+void Wing::setFlap0(float start, float end, float lift, float drag)
+{
+ _flap0Start = start;
+ _flap0End = end;
+ _flap0Lift = lift;
+ _flap0Drag = drag;
+}
+
+void Wing::setFlap1(float start, float end, float lift, float drag)
+{
+ _flap1Start = start;
+ _flap1End = end;
+ _flap1Lift = lift;
+ _flap1Drag = drag;
+}
+
+void Wing::setSlat(float start, float end, float aoa, float drag)
+{
+ _slatStart = start;
+ _slatEnd = end;
+ _slatAoA = aoa;
+ _slatDrag = drag;
+}
+
+void Wing::setSpoiler(float start, float end, float lift, float drag)
+{
+ _spoilerStart = start;
+ _spoilerEnd = end;
+ _spoilerLift = lift;
+ _spoilerDrag = drag;
+}
+
+void Wing::setFlap0(float lval, float rval)
+{
+ for(int i=0; i<_flap0Surfs.size(); i++) {
+ ((Surface*)_flap0Surfs.get(i))->setFlap(lval);
+ if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
+ }
+}
+
+void Wing::setFlap1(float lval, float rval)
+{
+ for(int i=0; i<_flap1Surfs.size(); i++) {
+ ((Surface*)_flap1Surfs.get(i))->setFlap(lval);
+ if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
+ }
+}
+
+void Wing::setSpoiler(float lval, float rval)
+{
+ for(int i=0; i<_spoilerSurfs.size(); i++) {
+ ((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
+ if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
+ }
+}
+
+void Wing::setSlat(float val)
+{
+ for(int i=0; i<_slatSurfs.size(); i++)
+ ((Surface*)_slatSurfs.get(i))->setSlat(val);
+}
+
+void Wing::compile()
+{
+ // Have we already been compiled?
+ if(_surfs.size() != 0) return;
+
+ // Assemble the start/end coordinates into an array, sort them,
+ // and remove duplicates. This gives us the boundaries of our
+ // segments.
+ float bounds[8];
+ bounds[0] = _flap0Start; bounds[1] = _flap0End;
+ bounds[2] = _flap1Start; bounds[3] = _flap1End;
+ bounds[4] = _spoilerStart; bounds[5] = _spoilerEnd;
+ bounds[6] = _slatStart; bounds[7] = _slatEnd;
+
+ // Sort in increasing order
+ for(int i=0; i<8; i++) {
+ int minIdx = i;
+ float minVal = bounds[i];
+ for(int j=i+1; j<8; j++) {
+ if(bounds[j] < minVal) {
+ minIdx = j;
+ minVal = bounds[j];
+ }
+ }
+ float tmp = bounds[i];
+ bounds[i] = minVal; bounds[minIdx] = tmp;
+ }
+
+ // Uniqify
+ float last = bounds[0];
+ int nbounds = 1;
+ for(int i=1; i<8; i++) {
+ if(bounds[i] != last)
+ bounds[nbounds++] = bounds[i];
+ last = bounds[i];
+ }
+
+ // Calculate a "nominal" segment length equal to an average chord,
+ // normalized to lie within 0-1 over the length of the wing.
+ float segLen = _chord * (0.5*(_taper+1)) / _length;
+
+ // Generating a unit vector pointing out the left wing.
+ float left[3];
+ left[0] = -Math::tan(_sweep);
+ left[1] = Math::cos(_dihedral);
+ left[2] = Math::sin(_dihedral);
+ Math::unit3(left, left);
+
+ // Calculate coordinates for the root and tip of the wing
+ float root[3], tip[3];
+ Math::set3(_base, root);
+ Math::set3(left, tip);
+ Math::mul3(_length, tip, tip);
+ Math::add3(root, tip, tip);
+
+ // The wing's Y axis will be the "left" vector. The Z axis will
+ // be perpendicular to this and the local (!) X axis, because we
+ // want motion along the local X axis to be zero AoA (i.e. in the
+ // wing's XY plane) by definition. Then the local X coordinate is
+ // just Y cross Z.
+ float orient[9], rightOrient[9];
+ float *x = orient, *y = orient+3, *z = orient+6;
+ x[0] = 1; x[1] = 0; x[2] = 0;
+ Math::set3(left, y);
+ Math::cross3(x, y, z);
+ Math::unit3(z, z);
+ Math::cross3(y, z, x);
+
+ if(_mirror) {
+ // Derive the right side orientation matrix from this one.
+ for(int i=0; i<9; i++) rightOrient[i] = orient[i];
+
+ // Negate all Y coordinates, this gets us a valid basis, but
+ // it's left handed! So...
+ for(int i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
+
+ // Change the direction of the Y axis to get back to a
+ // right-handed system.
+ for(int i=3; i<6; i++) rightOrient[i] = -rightOrient[i];
+ }
+
+ // Now go through each boundary and make segments
+ for(int i=0; i<(nbounds-1); i++) {
+ float start = bounds[i];
+ float end = bounds[i+1];
+ float mid = (start+end)/2;
+
+ bool flap0=0, flap1=0, slat=0, spoiler=0;
+ if(_flap0Start < mid && mid < _flap0End) flap0 = 1;
+ if(_flap1Start < mid && mid < _flap1End) flap1 = 1;
+ if(_slatStart < mid && mid < _slatEnd) slat = 1;
+ if(_spoilerStart < mid && mid < _spoilerEnd) spoiler = 1;
+
+ // FIXME: Should probably detect an error here if both flap0
+ // and flap1 are set. Right now flap1 overrides.
+
+ int nSegs = (int)Math::ceil((end-start)/segLen);
+ float segWid = _length * (end - start)/nSegs;
+
+ for(int j=0; j<nSegs; j++) {
+ float frac = start + (j+0.5) * (end-start)/nSegs;
+ float pos[3];
+ interp(root, tip, frac, pos);
+
+ float chord = _chord * (1 - (1-_taper)*frac);
+
+ Surface *s = newSurface(pos, orient, chord,
+ flap0, flap1, slat, spoiler);
+
+ SurfRec *sr = new SurfRec();
+ sr->surface = s;
+ sr->weight = chord * segWid;
+ s->setTotalDrag(sr->weight);
+ _surfs.add(sr);
+
+ if(_mirror) {
+ pos[1] = -pos[1];
+ s = newSurface(pos, rightOrient, chord,
+ flap0, flap1, slat, spoiler);
+ sr = new SurfRec();
+ sr->surface = s;
+ sr->weight = chord * segWid;
+ s->setTotalDrag(sr->weight);
+ _surfs.add(sr);
+ }
+ }
+ }
+}
+
+float Wing::getDragScale()
+{
+ return _dragScale;
+}
+
+void Wing::setDragScale(float scale)
+{
+ _dragScale = scale;
+ for(int i=0; i<_surfs.size(); i++) {
+ SurfRec* s = (SurfRec*)_surfs.get(i);
+ s->surface->setTotalDrag(scale * s->weight);
+ }
+}
+
+void Wing::setLiftRatio(float ratio)
+{
+ _liftRatio = ratio;
+ for(int i=0; i<_surfs.size(); i++)
+ ((SurfRec*)_surfs.get(i))->surface->setZDrag(ratio);
+}
+
+float Wing::getLiftRatio()
+{
+ return _liftRatio;
+}
+
+Surface* Wing::newSurface(float* pos, float* orient, float chord,
+ bool flap0, bool flap1, bool slat, bool spoiler)
+{
+ Surface* s = new Surface();
+
+ s->setPosition(pos);
+ s->setOrientation(orient);
+ s->setChord(chord);
+
+ // Camber is expressed as a fraction of stall peak, so convert.
+ s->setBaseZDrag(_camber*_stallPeak);
+
+ // The "main" (i.e. normal) stall angle
+ float stallAoA = _stall - _stallWidth/4;
+ s->setStall(0, stallAoA);
+ s->setStallWidth(0, _stallWidth);
+ s->setStallPeak(0, _stallPeak);
+
+ // The negative AoA stall is the same if we're using an uncambered
+ // airfoil, otherwise a "little badder".
+ if(_camber > 0) {
+ s->setStall(1, stallAoA * 0.8);
+ s->setStallWidth(1, _stallWidth * 0.5);
+ } else {
+ s->setStall(1, stallAoA);
+ s->setStall(1, _stallWidth);
+ }
+
+ // The "reverse" stalls are unmeasurable junk. Just use 13deg and
+ // "sharp".
+ s->setStallPeak(1, 1);
+ for(int i=2; i<4; i++) {
+ s->setStall(i, 0.2267);
+ s->setStallWidth(i, 1);
+ }
+
+ if(flap0) s->setFlapParams(_flap0Lift, _flap0Drag);
+ if(flap1) s->setFlapParams(_flap1Lift, _flap1Drag);
+ if(slat) s->setSlatParams(_slatAoA, _slatDrag);
+ if(spoiler) s->setSpoilerParams(_spoilerLift, _spoilerDrag);
+
+ if(flap0) _flap0Surfs.add(s);
+ if(flap1) _flap1Surfs.add(s);
+ if(slat) _slatSurfs.add(s);
+ if(spoiler) _spoilerSurfs.add(s);
+
+ return s;
+}
+
+void Wing::interp(float* v1, float* v2, float frac, float* out)
+{
+ out[0] = v1[0] + frac*(v2[0]-v1[0]);
+ out[1] = v1[1] + frac*(v2[1]-v1[1]);
+ out[2] = v1[2] + frac*(v2[2]-v1[2]);
+}
+
+}; // namespace yasim
--- /dev/null
+#ifndef _WING_HPP
+#define _WING_HPP
+
+#include "util/Vector.hpp"
+
+namespace yasim {
+
+class Surface;
+
+// FIXME: need to handle "inverted" controls for mirrored wings.
+class Wing {
+public:
+ Wing();
+ ~Wing();
+
+ // Do we mirror ourselves about the XZ plane?
+ void setMirror(bool mirror);
+
+ // Wing geometry:
+ void setBase(float* base); // in local coordinates
+ void setLength(float length); // dist. ALONG wing (not span!)
+ void setChord(float chord); // at base, measured along X axis
+ void setTaper(float taper); // fraction, 0-1
+ void setSweep(float sweep); // radians
+ void setDihedral(float dihedral); // radians, positive is "up"
+
+ void setStall(float aoa);
+ void setStallWidth(float angle);
+ void setStallPeak(float fraction);
+ void setCamber(float camber);
+ void setIncidence(float incidence);
+
+ void setFlap0(float start, float end, float lift, float drag);
+ void setFlap1(float start, float end, float lift, float drag);
+ void setSpoiler(float start, float end, float lift, float drag);
+ void setSlat(float start, float end, float aoa, float drag);
+
+ // Set the control axes for the sub-surfaces
+ void setFlap0(float lval, float rval);
+ void setFlap1(float lval, float rval);
+ void setSpoiler(float lval, float rval);
+ void setSlat(float val);
+
+ // Compile the thing into a bunch of Surface objects
+ void compile();
+
+ // Query the list of Surface objects
+ int numSurfaces();
+ Surface* getSurface(int n);
+ float getSurfaceWeight(int n);
+
+ // The overall drag coefficient for the wing as a whole. Units are
+ // arbitrary.
+ void setDragScale(float scale);
+ float getDragScale();
+
+ // The ratio of force along the Z (lift) direction of each wing
+ // segment to that along the X (drag) direction.
+ void setLiftRatio(float ratio);
+ float getLiftRatio();
+
+private:
+ void interp(float* v1, float* v2, float frac, float* out);
+ Surface* newSurface(float* pos, float* orient, float chord,
+ bool flap0, bool flap1, bool slat, bool spoiler);
+
+ struct SurfRec { Surface * surface; float weight; };
+
+ Vector _surfs;
+ Vector _flap0Surfs;
+ Vector _flap1Surfs;
+ Vector _slatSurfs;
+ Vector _spoilerSurfs;
+
+ bool _mirror;
+
+ float _base[3];
+ float _length;
+ float _chord;
+ float _taper;
+ float _sweep;
+ float _dihedral;
+
+ float _stall;
+ float _stallWidth;
+ float _stallPeak;
+ float _camber;
+ float _incidence;
+
+ float _dragScale;
+ float _liftRatio;
+
+ float _flap0Start;
+ float _flap0End;
+ float _flap0Lift;
+ float _flap0Drag;
+
+ float _flap1Start;
+ float _flap1End;
+ float _flap1Lift;
+ float _flap1Drag;
+
+ float _spoilerStart;
+ float _spoilerEnd;
+ float _spoilerLift;
+ float _spoilerDrag;
+
+ float _slatStart;
+ float _slatEnd;
+ float _slatAoA;
+ float _slatDrag;
+};
+
+}; // namespace yasim
+#endif // _WING_HPP
--- /dev/null
+#include <stdio.h>
+
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/debug/logstream.hxx>
+#include <simgear/easyxml.hxx>
+#include <Main/globals.hxx>
+#include <Main/fg_props.hxx>
+#include <Scenery/scenery.hxx>
+
+#include "FGFDM.hpp"
+#include "Atmosphere.hpp"
+#include "Math.hpp"
+#include "Airplane.hpp"
+#include "Model.hpp"
+#include "Integrator.hpp"
+#include "Glue.hpp"
+#include "Gear.hpp"
+#include "PropEngine.hpp"
+
+#include "YASim.hxx"
+
+using namespace yasim;
+
+static const float RAD2DEG = 180/3.14159265358979323846;
+static const float M2FT = 3.2808399;
+static const float FT2M = 0.3048;
+static const float MPS2KTS = 3600.0/1852.0;
+static const float CM2GALS = 264.172037284; // gallons/cubic meter
+
+void YASim::printDEBUG()
+{
+ static int debugCount = 0;
+
+ debugCount++;
+ if(debugCount >= 3) {
+ debugCount = 0;
+
+// printf("RPM %.1f FF %.1f\n",
+// fgGetFloat("/engines/engine[0]/rpm"),
+// fgGetFloat("/engines/engine[0]/fuel-flow-gph"));
+
+// printf("gear: %f\n", fgGetFloat("/controls/gear-down"));
+
+// printf("alpha %5.1f beta %5.1f\n", get_Alpha()*57.3, get_Beta()*57.3);
+
+// printf("pilot: %f %f %f\n",
+// fgGetDouble("/sim/view/pilot/x-offset-m"),
+// fgGetDouble("/sim/view/pilot/y-offset-m"),
+// fgGetDouble("/sim/view/pilot/z-offset-m"));
+ }
+}
+
+YASim::YASim(double dt)
+{
+ set_delta_t(dt);
+ _fdm = new FGFDM();
+
+ // Because the integration method is via fourth-order Runge-Kutta,
+ // we only get an "output" state for every 4 times the internal
+ // forces are calculated. So divide dt by four to account for
+ // this, and only run an iteration every fourth time through
+ // update.
+ _dt = dt * 4;
+ _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
+ _updateCount = 0;
+}
+
+void YASim::report()
+{
+ Airplane* a = _fdm->getAirplane();
+
+ float aoa = a->getCruiseAoA() * RAD2DEG;
+ float tail = -1 * a->getTailIncidence() * RAD2DEG;
+ float drag = 1000 * a->getDragCoefficient();
+
+ SG_LOG(SG_FLIGHT,SG_INFO,"YASim solution results:");
+ SG_LOG(SG_FLIGHT,SG_INFO," Iterations: "<<a->getSolutionIterations());
+ SG_LOG(SG_FLIGHT,SG_INFO,"Drag Coefficient: "<< drag);
+ SG_LOG(SG_FLIGHT,SG_INFO," Lift Ratio: "<<a->getLiftRatio());
+ SG_LOG(SG_FLIGHT,SG_INFO," Cruise AoA: "<< aoa);
+ SG_LOG(SG_FLIGHT,SG_INFO," Tail Incidence: "<< tail);
+
+ float cg[3];
+ char buf[256];
+ a->getModel()->getBody()->getCG(cg);
+ sprintf(buf, " CG: %.1f, %.1f, %.1f", cg[0], cg[1], cg[2]);
+ SG_LOG(SG_FLIGHT, SG_INFO, buf);
+
+ if(a->getFailureMsg()) {
+ SG_LOG(SG_FLIGHT, SG_ALERT, "YASim SOLUTION FAILURE:");
+ SG_LOG(SG_FLIGHT, SG_ALERT, a->getFailureMsg());
+ exit(1);
+ }
+}
+
+void YASim::init()
+{
+ Airplane* a = _fdm->getAirplane();
+ Model* m = a->getModel();
+
+ // Superclass hook
+ common_init();
+
+ // Build a filename and parse it
+ SGPath f(globals->get_fg_root());
+ f.append("Aircraft");
+ f.append(fgGetString("/sim/aircraft"));
+ f.concat(".xml");
+ readXML(f.str(), *_fdm);
+
+ // Compile it into a real airplane, and tell the user what they got
+ a->compile();
+ report();
+
+ _fdm->init();
+
+ // Lift the plane up so the gear clear the ground
+ float minGearZ = 1e18;
+ for(int i=0; i<a->numGear(); i++) {
+ Gear* g = a->getGear(i);
+ float pos[3];
+ g->getPosition(pos);
+ if(pos[2] < minGearZ)
+ minGearZ = pos[2];
+ }
+ _set_Altitude(get_Altitude() - minGearZ*M2FT);
+
+ // The pilot's eyepoint
+ float pilot[3];
+ a->getPilotPos(pilot);
+ fgSetFloat("/sim/view/pilot/x-offset-m", -pilot[0]);
+ fgSetFloat("/sim/view/pilot/y-offset-m", -pilot[1]);
+ fgSetFloat("/sim/view/pilot/z-offset-m", pilot[2]);
+
+ // Blank the state, and copy in ours
+ State s;
+ m->setState(&s);
+
+ copyToYASim(true);
+ set_inited(true);
+}
+
+bool YASim::update(int iterations)
+{
+ for(int i=0; i<iterations; i++) {
+ // Remember, update only every 4th call
+ _updateCount++;
+ if(_updateCount >= 4) {
+
+ copyToYASim(false);
+ _fdm->iterate(_dt);
+ copyFromYASim();
+
+ printDEBUG();
+
+ _updateCount = 0;
+ }
+ }
+
+ return true; // what does this mean?
+}
+
+void YASim::copyToYASim(bool copyState)
+{
+ // Physical state
+ float lat = get_Latitude();
+ float lon = get_Longitude();
+ float alt = get_Altitude() * FT2M;
+ float roll = get_Phi();
+ float pitch = get_Theta();
+ float hdg = get_Psi();
+
+ // Environment
+ float wind[3];
+ wind[0] = get_V_north_airmass() * FT2M;
+ wind[1] = get_V_east_airmass() * FT2M;
+ wind[2] = get_V_down_airmass() * FT2M;
+ double ground = get_Runway_altitude() * FT2M;
+
+ // You'd this this would work, but it doesn't. These values are
+ // always zero. Use a "standard" pressure intstead.
+ //
+ // float pressure = get_Static_pressure();
+ // float temp = get_Static_temperature();
+ float pressure = Atmosphere::getStdPressure(alt);
+ float temp = Atmosphere::getStdTemperature(alt);
+
+ // Convert and set:
+ Model* model = _fdm->getAirplane()->getModel();
+ State s;
+ float xyz2ned[9];
+ Glue::xyz2nedMat(lat, lon, xyz2ned);
+
+ // position
+ Glue::geod2xyz(lat, lon, alt, s.pos);
+
+ // orientation
+ Glue::euler2orient(roll, pitch, hdg, s.orient);
+ Math::mmul33(s.orient, xyz2ned, s.orient);
+
+ // Copy in the existing velocity for now...
+ Math::set3(model->getState()->v, s.v);
+
+ if(copyState)
+ model->setState(&s);
+
+ // wind
+ Math::tmul33(xyz2ned, wind, wind);
+ model->setWind(wind);
+
+ // ground. Calculate a cartesian coordinate for the ground under
+ // us, find the (geodetic) up vector normal to the ground, then
+ // use that to find the final (radius) term of the plane equation.
+ double xyz[3], gplane[3]; float up[3];
+ Glue::geod2xyz(lat, lon, ground, xyz);
+ Glue::geodUp(xyz, up); // FIXME, needless reverse computation...
+ for(int i=0; i<3; i++) gplane[i] = up[i];
+ double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
+ model->setGroundPlane(gplane, rad);
+
+ // air
+ model->setAir(pressure, temp);
+}
+
+// All the settables:
+//
+// These are set below:
+// _set_Accels_Local
+// _set_Accels_Body
+// _set_Accels_CG_Body
+// _set_Accels_Pilot_Body
+// _set_Accels_CG_Body_N
+// _set_Velocities_Local
+// _set_Velocities_Ground
+// _set_Velocities_Wind_Body
+// _set_Omega_Body
+// _set_Euler_Rates
+// _set_Euler_Angles
+// _set_V_rel_wind
+// _set_V_ground_speed
+// _set_V_equiv_kts
+// _set_V_calibrated_kts
+// _set_Alpha
+// _set_Beta
+// _set_Mach_number
+// _set_Climb_Rate
+// _set_Tank1Fuel
+// _set_Tank2Fuel
+// _set_Altitude_AGL
+// _set_Geodetic_Position
+// _set_Runway_altitude
+
+// Ignoring these, because they're unused:
+// _set_Geocentric_Position
+// _set_Geocentric_Rates
+// _set_Cos_phi
+// _set_Cos_theta
+// _set_Earth_position_angle (WTF?)
+// _set_Gamma_vert_rad
+// _set_Inertias
+// _set_T_Local_to_Body
+// _set_CG_Position
+// _set_Sea_Level_Radius
+
+// Externally set via the weather code:
+// _set_Velocities_Local_Airmass
+// _set_Density
+// _set_Static_pressure
+// _set_Static_temperature
+void YASim::copyFromYASim()
+{
+ Model* model = _fdm->getAirplane()->getModel();
+ State* s = model->getState();
+
+ // position
+ double lat, lon, alt;
+ Glue::xyz2geod(s->pos, &lat, &lon, &alt);
+ _set_Geodetic_Position(lat, lon, alt*M2FT);
+
+ // UNUSED
+ //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
+
+ // FIXME: there's a normal vector available too, use it.
+ float groundMSL = scenery.get_cur_elev();
+ _set_Runway_altitude(groundMSL*M2FT);
+ _set_Altitude_AGL((alt - groundMSL)*M2FT);
+
+ // useful conversion matrix
+ float xyz2ned[9];
+ Glue::xyz2nedMat(lat, lon, xyz2ned);
+
+ // velocity
+ float v[3];
+ Math::vmul33(xyz2ned, s->v, v);
+ _set_Velocities_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
+ _set_V_ground_speed(Math::sqrt(M2FT*v[0]*M2FT*v[0] +
+ M2FT*v[1]*M2FT*v[1]));
+ _set_Climb_Rate(-M2FT*v[2]);
+
+ // The HUD uses this, but inverts down (?!)
+ _set_Velocities_Ground(M2FT*v[0], M2FT*v[1], -M2FT*v[2]);
+
+ // _set_Geocentric_Rates(M2FT*v[0], M2FT*v[1], M2FT*v[2]); // UNUSED
+
+ Math::vmul33(s->orient, s->v, v);
+ _set_Velocities_Wind_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT);
+ _set_V_rel_wind(Math::mag3(v)*M2FT); // units?
+
+ // These don't work, use a dummy based on altitude
+ // float P = get_Static_pressure();
+ // float T = get_Static_temperature();
+ float P = Atmosphere::getStdPressure(alt);
+ float T = Atmosphere::getStdTemperature(alt);
+ _set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T)*MPS2KTS);
+ _set_V_calibrated_kts(Atmosphere::calcVCAS(v[0], P, T)*MPS2KTS);
+ _set_Mach_number(Atmosphere::calcMach(v[0], T));
+
+ // acceleration
+ Math::vmul33(xyz2ned, s->acc, v);
+ _set_Accels_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
+
+ Math::vmul33(s->orient, s->acc, v);
+ _set_Accels_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
+ _set_Accels_CG_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
+
+ _fdm->getAirplane()->getPilotAccel(v);
+ _set_Accels_Pilot_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
+
+ // The one appears (!) to want inverted pilot acceleration
+ // numbers, in G's...
+ Math::mul3(1.0/9.8, v, v);
+ _set_Accels_CG_Body_N(v[0], -v[1], -v[2]);
+
+ // orientation
+ float alpha, beta;
+ Glue::calcAlphaBeta(s, &alpha, &beta);
+ _set_Alpha(alpha);
+ _set_Beta(beta);
+
+ float tmp[9];
+ Math::trans33(xyz2ned, tmp);
+ Math::mmul33(s->orient, tmp, tmp);
+ float roll, pitch, hdg;
+ Glue::orient2euler(tmp, &roll, &pitch, &hdg);
+ _set_Euler_Angles(roll, pitch, hdg);
+
+ // rotation
+ Math::vmul33(s->orient, s->rot, v);
+ _set_Omega_Body(v[0], -v[1], -v[2]);
+
+ Glue::calcEulerRates(s, &roll, &pitch, &hdg);
+ _set_Euler_Rates(roll, pitch, hdg);
+}
--- /dev/null
+#ifndef _YASIM_HXX
+#define _YASIM_HXX
+
+#include <FDM/flight.hxx>
+
+namespace yasim { class FGFDM; };
+
+class YASim : public FGInterface {
+public:
+ YASim(double dt);
+
+ // Load externally set stuff into the FDM
+ virtual void init();
+
+ // Run an iteration
+ virtual bool update(int iterations);
+
+ private:
+ void report();
+ void copyFromYASim();
+ void copyToYASim(bool copyState);
+
+ void printDEBUG();
+
+ yasim::FGFDM* _fdm;
+ int _updateCount;
+ float _dt;
+};
+
+#endif // _YASIM_HXX
projects / flightgear.git / commitdiff