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