src/FDM/YASim/Model.cpp

   1 #include <stdio.h>
   2
   3 #include "Atmosphere.hpp"
   4 #include "Thruster.hpp"
   5 #include "Math.hpp"
   6 #include "RigidBody.hpp"
   7 #include "Integrator.hpp"
   8 #include "Propeller.hpp"
   9 #include "PistonEngine.hpp"
  10 #include "Gear.hpp"
  11 #include "Surface.hpp"
  12 #include "Glue.hpp"
  13
  14 #include "Model.hpp"
  15 namespace yasim {
  16
  17 void printState(State* s)
  18 {
  19     State tmp = *s;
  20     Math::vmul33(tmp.orient, tmp.v, tmp.v);
  21     Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
  22     Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
  23     Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
  24
  25     printf("\nNEW STATE (LOCAL COORDS)\n");
  26     printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
  27     printf("o:   ");
  28     int i;
  29     for(i=0; i<3; i++) {
  30         if(i != 0) printf("     ");
  31         printf("%6.2f %6.2f %6.2f\n",
  32                tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
  33     }
  34     printf("v:   %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
  35     printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
  36     printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
  37     printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
  38 }
  39
  40 Model::Model()
  41 {
  42     int i;
  43     for(i=0; i<3; i++) _wind[i] = 0;
  44
  45     _integrator.setBody(&_body);
  46     _integrator.setEnvironment(this);
  47
  48     // Default value of 30 Hz
  49     _integrator.setInterval(1.0f/30.0f);
  50
  51     _agl = 0;
  52     _crashed = false;
  53 }
  54
  55 Model::~Model()
  56 {
  57     // FIXME: who owns these things?  Need a policy
  58 }
  59
  60 void Model::getThrust(float* out)
  61 {
  62     float tmp[3];
  63     out[0] = out[1] = out[2] = 0;
  64     int i;
  65     for(i=0; i<_thrusters.size(); i++) {
  66         Thruster* t = (Thruster*)_thrusters.get(i);
  67         t->getThrust(tmp);
  68         Math::add3(tmp, out, out);
  69     }
  70 }
  71
  72 void Model::initIteration()
  73 {
  74     // Precompute torque and angular momentum for the thrusters
  75     int i;
  76     for(i=0; i<3; i++)
  77         _gyro[i] = _torque[i] = 0;
  78     for(i=0; i<_thrusters.size(); i++) {
  79         Thruster* t = (Thruster*)_thrusters.get(i);
  80
  81         // Get the wind velocity at the thruster location
  82         float pos[3], v[3];
  83         t->getPosition(pos);
  84         localWind(pos, _s, v);
  85
  86         t->setWind(v);
  87         t->setAir(_pressure, _temp, _rho);
  88         t->integrate(_integrator.getInterval());
  89
  90         t->getTorque(v);
  91         Math::add3(v, _torque, _torque);
  92
  93         t->getGyro(v);
  94         Math::add3(v, _gyro, _gyro);
  95     }
  96 }
  97
  98 void Model::iterate()
  99 {
 100     initIteration();
 101     _body.recalc(); // FIXME: amortize this, somehow
 102     _integrator.calcNewInterval();
 103 }
 104
 105 bool Model::isCrashed()
 106 {
 107     return _crashed;
 108 }
 109
 110 void Model::setCrashed(bool crashed)
 111 {
 112     _crashed = crashed;
 113 }
 114
 115 float Model::getAGL()
 116 {
 117     return _agl;
 118 }
 119
 120 State* Model::getState()
 121 {
 122     return _s;
 123 }
 124
 125 void Model::setState(State* s)
 126 {
 127     _integrator.setState(s);
 128     _s = _integrator.getState();
 129 }
 130
 131 RigidBody* Model::getBody()
 132 {
 133     return &_body;
 134 }
 135
 136 Integrator* Model::getIntegrator()
 137 {
 138     return &_integrator;
 139 }
 140
 141 Surface* Model::getSurface(int handle)
 142 {
 143     return (Surface*)_surfaces.get(handle);
 144 }
 145
 146 int Model::addThruster(Thruster* t)
 147 {
 148     return _thrusters.add(t);
 149 }
 150
 151 int Model::numThrusters()
 152 {
 153     return _thrusters.size();
 154 }
 155
 156 Thruster* Model::getThruster(int handle)
 157 {
 158     return (Thruster*)_thrusters.get(handle);
 159 }
 160
 161 void Model::setThruster(int handle, Thruster* t)
 162 {
 163     _thrusters.set(handle, t);
 164 }
 165
 166 int Model::addSurface(Surface* surf)
 167 {
 168     return _surfaces.add(surf);
 169 }
 170
 171 int Model::addGear(Gear* gear)
 172 {
 173     return _gears.add(gear);
 174 }
 175
 176 void Model::setGroundEffect(float* pos, float span, float mul)
 177 {
 178     Math::set3(pos, _wingCenter);
 179     _groundEffectSpan = span;
 180     _groundEffect = mul;
 181 }
 182
 183 // The first three elements are a unit vector pointing from the global
 184 // origin to the plane, the final element is the distance from the
 185 // origin (the radius of the earth, in most implementations).  So
 186 // (v dot _ground)-_ground[3] gives the distance AGL.
 187 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
 188 {
 189     int i;
 190     for(i=0; i<3; i++) _ground[i] = planeNormal[i];
 191     _ground[3] = fromOrigin;
 192 }
 193
 194 void Model::setAir(float pressure, float temp, float density)
 195 {
 196     _pressure = pressure;
 197     _temp = temp;
 198     _rho = density;
 199 }
 200
 201 void Model::setWind(float* wind)
 202 {
 203     Math::set3(wind, _wind);
 204 }
 205
 206 void Model::calcForces(State* s)
 207 {
 208     // Add in the pre-computed stuff.  These values aren't part of the
 209     // Runge-Kutta integration (they don't depend on position or
 210     // velocity), and are therefore constant across the four calls to
 211     // calcForces.  They get computed before we begin the integration
 212     // step.
 213     _body.setGyro(_gyro);
 214     _body.addTorque(_torque);
 215     int i;
 216     for(i=0; i<_thrusters.size(); i++) {
 217         Thruster* t = (Thruster*)_thrusters.get(i);
 218         float thrust[3], pos[3];
 219         t->getThrust(thrust);
 220         t->getPosition(pos);
 221         _body.addForce(pos, thrust);
 222     }
 223
 224     // Gravity, convert to a force, then to local coordinates
 225     float grav[3];
 226     Glue::geodUp(s->pos, grav);
 227     Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
 228     Math::vmul33(s->orient, grav, grav);
 229     _body.addForce(grav);
 230
 231     // Do each surface, remembering that the local velocity at each
 232     // point is different due to rotation.
 233     float faero[3];
 234     faero[0] = faero[1] = faero[2] = 0;
 235     for(i=0; i<_surfaces.size(); i++) {
 236         Surface* sf = (Surface*)_surfaces.get(i);
 237
 238         // Vsurf = wind - velocity + (rot cross (cg - pos))
 239         float vs[3], pos[3];
 240         sf->getPosition(pos);
 241         localWind(pos, s, vs);
 242
 243         float force[3], torque[3];
 244         sf->calcForce(vs, _rho, force, torque);
 245         Math::add3(faero, force, faero);
 246         _body.addForce(pos, force);
 247         _body.addTorque(torque);
 248     }
 249
 250     // Get a ground plane in local coordinates.  The first three
 251     // elements are the normal vector, the final one is the distance
 252     // from the local origin along that vector to the ground plane
 253     // (negative for objects "above" the ground)
 254     float ground[4];
 255     ground[3] = localGround(s, ground);
 256
 257     // Account for ground effect by multiplying the vertical force
 258     // component by an amount linear with the fraction of the wingspan
 259     // above the ground.
 260     float dist = ground[3] - Math::dot3(ground, _wingCenter);
 261     if(dist > 0 && dist < _groundEffectSpan) {
 262         float fz = Math::dot3(faero, ground);
 263         fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
 264         fz *= _groundEffect;
 265         Math::mul3(fz, ground, faero);
 266         _body.addForce(faero);
 267     }
 268
 269     // Convert the velocity and rotation vectors to local coordinates
 270     float lrot[3], lv[3];
 271     Math::vmul33(s->orient, s->rot, lrot);
 272     Math::vmul33(s->orient, s->v, lv);
 273
 274     // The landing gear
 275     for(i=0; i<_gears.size(); i++) {
 276         float force[3], contact[3];
 277         Gear* g = (Gear*)_gears.get(i);
 278         g->calcForce(&_body, lv, lrot, ground);
 279         g->getForce(force, contact);
 280         _body.addForce(contact, force);
 281     }
 282 }
 283
 284 void Model::newState(State* s)
 285 {
 286     _s = s;
 287
 288     //printState(s);
 289
 290     // Some simple collision detection
 291     float min = 1e8;
 292     float ground[4], pos[3], cmpr[3];
 293     ground[3] = localGround(s, ground);
 294     int i;
 295     for(i=0; i<_gears.size(); i++) {
 296         Gear* g = (Gear*)_gears.get(i);
 297
 298         // Get the point of ground contact
 299         g->getPosition(pos);
 300         g->getCompression(cmpr);
 301         Math::mul3(g->getCompressFraction(), cmpr, cmpr);
 302         Math::add3(cmpr, pos, pos);
 303         float dist = ground[3] - Math::dot3(pos, ground);
 304
 305         // Find the lowest one
 306         if(dist < min)
 307             min = dist;
 308     }
 309     _agl = min;
 310     if(_agl < -1) // Allow for some integration slop
 311         _crashed = true;
 312 }
 313
 314 // Returns a unit "down" vector for the ground in out, and the
 315 // distance from the local origin to the ground as the return value.
 316 // So for a given position V, "dist - (V dot out)" will be the height
 317 // AGL.
 318 float Model::localGround(State* s, float* out)
 319 {
 320     // Get the ground's "down" vector, this can be in floats, because
 321     // we don't need positioning accuracy.  The direction has plenty
 322     // of accuracy after truncation.
 323     out[0] = -(float)_ground[0];
 324     out[1] = -(float)_ground[1];
 325     out[2] = -(float)_ground[2];
 326     Math::vmul33(s->orient, out, out);
 327
 328     // The distance from the ground to the Aircraft's origin:
 329     double dist = (s->pos[0]*_ground[0]
 330                    + s->pos[1]*_ground[1]
 331                    + s->pos[2]*_ground[2] - _ground[3]);
 332
 333     return (float)dist;
 334 }
 335
 336 // Calculates the airflow direction at the given point and for the
 337 // specified aircraft velocity.
 338 void Model::localWind(float* pos, State* s, float* out)
 339 {
 340     // Most of the input is in global coordinates.  Fix that.
 341     float lwind[3], lrot[3], lv[3];
 342     Math::vmul33(s->orient, _wind, lwind);
 343     Math::vmul33(s->orient, s->rot, lrot);
 344     Math::vmul33(s->orient, s->v, lv);
 345
 346     _body.pointVelocity(pos, lrot, out); // rotational velocity
 347     Math::mul3(-1, out, out);      //  (negated)
 348     Math::add3(lwind, out, out);    //  + wind
 349     Math::sub3(out, lv, out);       //  - velocity
 350 }
 351
 352 }; // namespace yasim