src/FDM/YASim/Model.cpp

   1 #include "Atmosphere.hpp"
   2 #include "Thruster.hpp"
   3 #include "Math.hpp"
   4 #include "RigidBody.hpp"
   5 #include "Integrator.hpp"
   6 #include "Propeller.hpp"
   7 #include "PistonEngine.hpp"
   8 #include "Gear.hpp"
   9 #include "Surface.hpp"
  10 #include "Rotor.hpp"
  11 #include "Rotorpart.hpp"
  12 #include "Rotorblade.hpp"
  13 #include "Glue.hpp"
  14
  15 #include "Model.hpp"
  16 namespace yasim {
  17
  18 #if 0
  19 void printState(State* s)
  20 {
  21     State tmp = *s;
  22     Math::vmul33(tmp.orient, tmp.v, tmp.v);
  23     Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
  24     Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
  25     Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
  26
  27     printf("\nNEW STATE (LOCAL COORDS)\n");
  28     printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
  29     printf("o:   ");
  30     int i;
  31     for(i=0; i<3; i++) {
  32         if(i != 0) printf("     ");
  33         printf("%6.2f %6.2f %6.2f\n",
  34                tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
  35     }
  36     printf("v:   %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
  37     printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
  38     printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
  39     printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
  40 }
  41 #endif
  42
  43 Model::Model()
  44 {
  45     int i;
  46     for(i=0; i<3; i++) _wind[i] = 0;
  47
  48     _integrator.setBody(&_body);
  49     _integrator.setEnvironment(this);
  50
  51     // Default value of 30 Hz
  52     _integrator.setInterval(1.0f/30.0f);
  53
  54     _agl = 0;
  55     _crashed = false;
  56     _turb = 0;
  57 }
  58
  59 Model::~Model()
  60 {
  61     // FIXME: who owns these things?  Need a policy
  62 }
  63
  64 void Model::getThrust(float* out)
  65 {
  66     float tmp[3];
  67     out[0] = out[1] = out[2] = 0;
  68     int i;
  69     for(i=0; i<_thrusters.size(); i++) {
  70         Thruster* t = (Thruster*)_thrusters.get(i);
  71         t->getThrust(tmp);
  72         Math::add3(tmp, out, out);
  73     }
  74 }
  75
  76 void Model::initIteration()
  77 {
  78     // Precompute torque and angular momentum for the thrusters
  79     int i;
  80     for(i=0; i<3; i++)
  81         _gyro[i] = _torque[i] = 0;
  82
  83     // Need a local altitude for the wind calculation
  84     float dummy[3];
  85     float alt = Math::abs(localGround(_s, dummy));
  86
  87     for(i=0; i<_thrusters.size(); i++) {
  88         Thruster* t = (Thruster*)_thrusters.get(i);
  89
  90         // Get the wind velocity at the thruster location
  91         float pos[3], v[3];
  92         t->getPosition(pos);
  93         localWind(pos, _s, v, alt);
  94
  95         t->setWind(v);
  96         t->setAir(_pressure, _temp, _rho);
  97         t->integrate(_integrator.getInterval());
  98
  99         t->getTorque(v);
 100         Math::add3(v, _torque, _torque);
 101
 102         t->getGyro(v);
 103         Math::add3(v, _gyro, _gyro);
 104     }
 105
 106     // Displace the turbulence coordinates according to the local wind.
 107     if(_turb) {
 108         float toff[3];
 109         Math::mul3(_integrator.getInterval(), _wind, toff);
 110         _turb->offset(toff);
 111     }
 112 }
 113
 114 // FIXME: This method looks to me like it's doing *integration*, not
 115 // initialization.  Integration code should ideally go into
 116 // calcForces.  Only very "unstiff" problems can be solved well like
 117 // this (see the engine code for an example); I don't know if rotor
 118 // dynamics qualify or not.
 119 // -Andy
 120 void Model::initRotorIteration()
 121 {
 122     int i;
 123     float dt = _integrator.getInterval();
 124     float lrot[3];
 125     Math::vmul33(_s->orient, _s->rot, lrot);
 126     Math::mul3(dt,lrot,lrot);
 127     for(i=0; i<_rotors.size(); i++) {
 128         Rotor* r = (Rotor*)_rotors.get(i);
 129         r->inititeration(dt);
 130     }
 131     for(i=0; i<_rotorparts.size(); i++) {
 132         Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
 133         rp->inititeration(dt,lrot);
 134     }
 135     for(i=0; i<_rotorblades.size(); i++) {
 136         Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
 137         rp->inititeration(dt,lrot);
 138     }
 139 }
 140
 141 void Model::iterate()
 142 {
 143     initIteration();
 144     initRotorIteration();
 145     _body.recalc(); // FIXME: amortize this, somehow
 146     _integrator.calcNewInterval();
 147 }
 148
 149 bool Model::isCrashed()
 150 {
 151     return _crashed;
 152 }
 153
 154 void Model::setCrashed(bool crashed)
 155 {
 156     _crashed = crashed;
 157 }
 158
 159 float Model::getAGL()
 160 {
 161     return _agl;
 162 }
 163
 164 State* Model::getState()
 165 {
 166     return _s;
 167 }
 168
 169 void Model::setState(State* s)
 170 {
 171     _integrator.setState(s);
 172     _s = _integrator.getState();
 173 }
 174
 175 RigidBody* Model::getBody()
 176 {
 177     return &_body;
 178 }
 179
 180 Integrator* Model::getIntegrator()
 181 {
 182     return &_integrator;
 183 }
 184
 185 Surface* Model::getSurface(int handle)
 186 {
 187     return (Surface*)_surfaces.get(handle);
 188 }
 189
 190 Rotorpart* Model::getRotorpart(int handle)
 191 {
 192     return (Rotorpart*)_rotorparts.get(handle);
 193 }
 194 Rotorblade* Model::getRotorblade(int handle)
 195 {
 196     return (Rotorblade*)_rotorblades.get(handle);
 197 }
 198 Rotor* Model::getRotor(int handle)
 199 {
 200     return (Rotor*)_rotors.get(handle);
 201 }
 202
 203 int Model::addThruster(Thruster* t)
 204 {
 205     return _thrusters.add(t);
 206 }
 207
 208 int Model::numThrusters()
 209 {
 210     return _thrusters.size();
 211 }
 212
 213 Thruster* Model::getThruster(int handle)
 214 {
 215     return (Thruster*)_thrusters.get(handle);
 216 }
 217
 218 void Model::setThruster(int handle, Thruster* t)
 219 {
 220     _thrusters.set(handle, t);
 221 }
 222
 223 int Model::addSurface(Surface* surf)
 224 {
 225     return _surfaces.add(surf);
 226 }
 227
 228 int Model::addRotorpart(Rotorpart* rpart)
 229 {
 230     return _rotorparts.add(rpart);
 231 }
 232 int Model::addRotorblade(Rotorblade* rblade)
 233 {
 234     return _rotorblades.add(rblade);
 235 }
 236 int Model::addRotor(Rotor* r)
 237 {
 238     return _rotors.add(r);
 239 }
 240
 241 int Model::addGear(Gear* gear)
 242 {
 243     return _gears.add(gear);
 244 }
 245
 246 void Model::setGroundEffect(float* pos, float span, float mul)
 247 {
 248     Math::set3(pos, _wingCenter);
 249     _groundEffectSpan = span;
 250     _groundEffect = mul;
 251 }
 252
 253 // The first three elements are a unit vector pointing from the global
 254 // origin to the plane, the final element is the distance from the
 255 // origin (the radius of the earth, in most implementations).  So
 256 // (v dot _ground)-_ground[3] gives the distance AGL.
 257 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
 258 {
 259     for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
 260     _ground[3] = fromOrigin;
 261 }
 262
 263 void Model::setAir(float pressure, float temp, float density)
 264 {
 265     _pressure = pressure;
 266     _temp = temp;
 267     _rho = density;
 268 }
 269
 270 void Model::setWind(float* wind)
 271 {
 272     Math::set3(wind, _wind);
 273 }
 274
 275 void Model::calcForces(State* s)
 276 {
 277     // Add in the pre-computed stuff.  These values aren't part of the
 278     // Runge-Kutta integration (they don't depend on position or
 279     // velocity), and are therefore constant across the four calls to
 280     // calcForces.  They get computed before we begin the integration
 281     // step.
 282     _body.setGyro(_gyro);
 283     _body.addTorque(_torque);
 284     int i;
 285     for(i=0; i<_thrusters.size(); i++) {
 286         Thruster* t = (Thruster*)_thrusters.get(i);
 287         float thrust[3], pos[3];
 288         t->getThrust(thrust);
 289         t->getPosition(pos);
 290         _body.addForce(pos, thrust);
 291     }
 292
 293     // Get a ground plane in local coordinates.  The first three
 294     // elements are the normal vector, the final one is the distance
 295     // from the local origin along that vector to the ground plane
 296     // (negative for objects "above" the ground)
 297     float ground[4];
 298     ground[3] = localGround(s, ground);
 299     float alt = Math::abs(ground[3]);
 300
 301     // Gravity, convert to a force, then to local coordinates
 302     float grav[3];
 303     Glue::geodUp(s->pos, grav);
 304     Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
 305     Math::vmul33(s->orient, grav, grav);
 306     _body.addForce(grav);
 307
 308     // Do each surface, remembering that the local velocity at each
 309     // point is different due to rotation.
 310     float faero[3];
 311     faero[0] = faero[1] = faero[2] = 0;
 312     for(i=0; i<_surfaces.size(); i++) {
 313         Surface* sf = (Surface*)_surfaces.get(i);
 314
 315         // Vsurf = wind - velocity + (rot cross (cg - pos))
 316         float vs[3], pos[3];
 317         sf->getPosition(pos);
 318         localWind(pos, s, vs, alt);
 319
 320         float force[3], torque[3];
 321         sf->calcForce(vs, _rho, force, torque);
 322         Math::add3(faero, force, faero);
 323
 324         _body.addForce(pos, force);
 325         _body.addTorque(torque);
 326     }
 327     for(i=0; i<_rotorparts.size(); i++) {
 328         Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
 329
 330         // Vsurf = wind - velocity + (rot cross (cg - pos))
 331         float vs[3], pos[3];
 332         sf->getPosition(pos);
 333         localWind(pos, s, vs, alt);
 334
 335         float force[3], torque[3];
 336         sf->calcForce(vs, _rho, force, torque);
 337         //Math::add3(faero, force, faero);
 338
 339         sf->getPositionForceAttac(pos);
 340
 341         _body.addForce(pos, force);
 342         _body.addTorque(torque);
 343     }
 344     for(i=0; i<_rotorblades.size(); i++) {
 345         Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
 346
 347         // Vsurf = wind - velocity + (rot cross (cg - pos))
 348         float vs[3], pos[3];
 349         sf->getPosition(pos);
 350         localWind(pos, s, vs, alt);
 351
 352         float force[3], torque[3];
 353         sf->calcForce(vs, _rho, force, torque);
 354         //Math::add3(faero, force, faero);
 355
 356         sf->getPositionForceAttac(pos);
 357
 358         _body.addForce(pos, force);
 359         _body.addTorque(torque);
 360     }
 361
 362     // Account for ground effect by multiplying the vertical force
 363     // component by an amount linear with the fraction of the wingspan
 364     // above the ground.
 365     float dist = ground[3] - Math::dot3(ground, _wingCenter);
 366     if(dist > 0 && dist < _groundEffectSpan) {
 367         float fz = Math::dot3(faero, ground);
 368         fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
 369         fz *= _groundEffect;
 370         Math::mul3(fz, ground, faero);
 371         _body.addForce(faero);
 372     }
 373
 374     // Convert the velocity and rotation vectors to local coordinates
 375     float lrot[3], lv[3];
 376     Math::vmul33(s->orient, s->rot, lrot);
 377     Math::vmul33(s->orient, s->v, lv);
 378
 379     // The landing gear
 380     for(i=0; i<_gears.size(); i++) {
 381         float force[3], contact[3];
 382         Gear* g = (Gear*)_gears.get(i);
 383         g->calcForce(&_body, lv, lrot, ground);
 384         g->getForce(force, contact);
 385         _body.addForce(contact, force);
 386     }
 387 }
 388
 389 void Model::newState(State* s)
 390 {
 391     _s = s;
 392
 393     // Some simple collision detection
 394     float min = 1e8;
 395     float ground[4], pos[3], cmpr[3];
 396     ground[3] = localGround(s, ground);
 397     int i;
 398     for(i=0; i<_gears.size(); i++) {
 399         Gear* g = (Gear*)_gears.get(i);
 400
 401         // Get the point of ground contact
 402         g->getPosition(pos);
 403         g->getCompression(cmpr);
 404         Math::mul3(g->getCompressFraction(), cmpr, cmpr);
 405         Math::add3(cmpr, pos, pos);
 406         float dist = ground[3] - Math::dot3(pos, ground);
 407
 408         // Find the lowest one
 409         if(dist < min)
 410             min = dist;
 411     }
 412     _agl = min;
 413     if(_agl < -1) // Allow for some integration slop
 414         _crashed = true;
 415 }
 416
 417 // Returns a unit "down" vector for the ground in out, and the
 418 // distance from the local origin to the ground as the return value.
 419 // So for a given position V, "dist - (V dot out)" will be the height
 420 // AGL.
 421 float Model::localGround(State* s, float* out)
 422 {
 423     // Get the ground's "down" vector, this can be in floats, because
 424     // we don't need positioning accuracy.  The direction has plenty
 425     // of accuracy after truncation.
 426     out[0] = -(float)_ground[0];
 427     out[1] = -(float)_ground[1];
 428     out[2] = -(float)_ground[2];
 429     Math::vmul33(s->orient, out, out);
 430
 431     // The distance from the ground to the Aircraft's origin:
 432     double dist = (s->pos[0]*_ground[0]
 433                    + s->pos[1]*_ground[1]
 434                    + s->pos[2]*_ground[2] - _ground[3]);
 435
 436     return (float)dist;
 437 }
 438
 439 // Calculates the airflow direction at the given point and for the
 440 // specified aircraft velocity.
 441 void Model::localWind(float* pos, State* s, float* out, float alt)
 442 {
 443     float tmp[3], lwind[3], lrot[3], lv[3];
 444
 445     // Get a global coordinate for our local position, and calculate
 446     // turbulence.
 447     if(_turb) {
 448         double gpos[3]; float up[3];
 449         Math::tmul33(s->orient, pos, tmp);
 450         for(int i=0; i<3; i++) {
 451             gpos[i] = s->pos[i] + tmp[i];
 452             up[i] = _ground[i];
 453         }
 454         _turb->getTurbulence(gpos, alt, up, lwind);
 455         Math::add3(_wind, lwind, lwind);
 456     } else {
 457         Math::set3(_wind, lwind);
 458     }
 459
 460     // Convert to local coordinates
 461     Math::vmul33(s->orient, lwind, lwind);
 462     Math::vmul33(s->orient, s->rot, lrot);
 463     Math::vmul33(s->orient, s->v, lv);
 464
 465     _body.pointVelocity(pos, lrot, out); // rotational velocity
 466     Math::mul3(-1, out, out);      //  (negated)
 467     Math::add3(lwind, out, out);    //  + wind
 468     Math::sub3(out, lv, out);       //  - velocity
 469 }
 470
 471 }; // namespace yasim