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