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     for(int i=0; i<3; i++) {
  29         if(i != 0) printf("     ");
  30         printf("%6.2f %6.2f %6.2f\n",
  31                tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
  32     }
  33     printf("v:   %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
  34     printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
  35     printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
  36     printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
  37 }
  38
  39 Model::Model()
  40 {
  41     for(int i=0; i<3; i++) _wind[i] = 0;
  42
  43     _integrator.setBody(&_body);
  44     _integrator.setEnvironment(this);
  45
  46     // Default value of 30 Hz
  47     _integrator.setInterval(1.0/30.0);
  48 }
  49
  50 Model::~Model()
  51 {
  52     // FIXME: who owns these things?  Need a policy
  53 }
  54
  55 void Model::getThrust(float* out)
  56 {
  57     float tmp[3];
  58     out[0] = out[1] = out[2] = 0;
  59     for(int i=0; i<_thrusters.size(); i++) {
  60         Thruster* t = (Thruster*)_thrusters.get(i);
  61         t->getThrust(tmp);
  62         Math::add3(tmp, out, out);
  63     }
  64 }
  65
  66 void Model::initIteration()
  67 {
  68     // Precompute torque and angular momentum for the thrusters
  69     for(int i=0; i<3; i++)
  70         _gyro[i] = _torque[i] = 0;
  71     for(int i=0; i<_thrusters.size(); i++) {
  72         Thruster* t = (Thruster*)_thrusters.get(i);
  73
  74         // Get the wind velocity at the thruster location
  75         float pos[3], v[3];
  76         t->getPosition(pos);
  77         localWind(pos, _s, v);
  78
  79         t->setWind(v);
  80         t->setDensity(_rho);
  81         t->integrate(_integrator.getInterval());
  82
  83         t->getTorque(v);
  84         Math::add3(v, _torque, _torque);
  85
  86         t->getGyro(v);
  87         Math::add3(v, _gyro, _gyro);
  88     }
  89 }
  90
  91 void Model::iterate()
  92 {
  93     initIteration();
  94     _body.recalc(); // FIXME: amortize this, somehow
  95     _integrator.calcNewInterval();
  96 }
  97
  98 State* Model::getState()
  99 {
 100     return _s;
 101 }
 102
 103 void Model::setState(State* s)
 104 {
 105     _integrator.setState(s);
 106     _s = _integrator.getState();
 107 }
 108
 109 RigidBody* Model::getBody()
 110 {
 111     return &_body;
 112 }
 113
 114 Integrator* Model::getIntegrator()
 115 {
 116     return &_integrator;
 117 }
 118
 119 Surface* Model::getSurface(int handle)
 120 {
 121     return (Surface*)_surfaces.get(handle);
 122 }
 123
 124 int Model::addThruster(Thruster* t)
 125 {
 126     return _thrusters.add(t);
 127 }
 128
 129 Thruster* Model::getThruster(int handle)
 130 {
 131     return (Thruster*)_thrusters.get(handle);
 132 }
 133
 134 void Model::setThruster(int handle, Thruster* t)
 135 {
 136     _thrusters.set(handle, t);
 137 }
 138
 139 int Model::addSurface(Surface* surf)
 140 {
 141     return _surfaces.add(surf);
 142 }
 143
 144 int Model::addGear(Gear* gear)
 145 {
 146     return _gears.add(gear);
 147 }
 148
 149 // The first three elements are a unit vector pointing from the global
 150 // origin to the plane, the final element is the distance from the
 151 // origin (the radius of the earth, in most implementations).  So
 152 // (v dot _ground)-_ground[3] gives the distance AGL.
 153 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
 154 {
 155     for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
 156     _ground[3] = fromOrigin;
 157 }
 158
 159 void Model::setAirDensity(float rho)
 160 {
 161     _rho = rho;
 162 }
 163
 164 void Model::setAir(float pressure, float temp)
 165 {
 166     _rho = Atmosphere::calcDensity(pressure, temp);
 167 }
 168
 169 void Model::setWind(float* wind)
 170 {
 171     Math::set3(wind, _wind);
 172 }
 173
 174 void Model::calcForces(State* s)
 175 {
 176     // Add in the pre-computed stuff.  These values aren't part of the
 177     // Runge-Kutta integration (they don't depend on position or
 178     // velocity), and are therefore constant across the four calls to
 179     // calcForces.  They get computed before we begin the integration
 180     // step.
 181     _body.setGyro(_gyro);
 182     _body.addTorque(_torque);
 183     for(int i=0; i<_thrusters.size(); i++) {
 184         Thruster* t = (Thruster*)_thrusters.get(i);
 185         float thrust[3], pos[3];
 186         t->getThrust(thrust);
 187         t->getPosition(pos);
 188         _body.addForce(pos, thrust);
 189     }
 190
 191     // Gravity, convert to a force, then to local coordinates
 192     float grav[3];
 193     Glue::geodUp(s->pos, grav);
 194     Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
 195     Math::vmul33(s->orient, grav, grav);
 196     _body.addForce(grav);
 197
 198     // Do each surface, remembering that the local velocity at each
 199     // point is different due to rotation.
 200     for(int i=0; i<_surfaces.size(); i++) {
 201         Surface* sf = (Surface*)_surfaces.get(i);
 202
 203         // Vsurf = wind - velocity + (rot cross (cg - pos))
 204         float vs[3], pos[3];
 205         sf->getPosition(pos);
 206         localWind(pos, s, vs);
 207
 208         float force[3], torque[3];
 209         sf->calcForce(vs, _rho, force, torque);
 210         _body.addForce(pos, force);
 211         _body.addTorque(torque);
 212     }
 213
 214     // Get a ground plane in local coordinates.  The first three
 215     // elements are the normal vector, the final one is the distance
 216     // from the local origin along that vector to the ground plane
 217     // (negative for objects "above" the ground)
 218     float ground[4];
 219     ground[3] = localGround(s, ground);
 220
 221     // Convert the velocity and rotation vectors to local coordinates
 222     float lrot[3], lv[3];
 223     Math::vmul33(s->orient, s->rot, lrot);
 224     Math::vmul33(s->orient, s->v, lv);
 225
 226     // The landing gear
 227     for(int i=0; i<_gears.size(); i++) {
 228         float force[3], contact[3];
 229         Gear* g = (Gear*)_gears.get(i);
 230         g->calcForce(&_body, lv, lrot, ground);
 231         g->getForce(force, contact);
 232         _body.addForce(contact, force);
 233     }
 234 }
 235
 236 void Model::newState(State* s)
 237 {
 238     _s = s;
 239
 240     //printState(s);
 241
 242     // Some simple collision detection
 243     float ground[4], pos[3], cmpr[3];
 244     ground[3] = localGround(s, ground);
 245     for(int i=0; i<_gears.size(); i++) {
 246         Gear* g = (Gear*)_gears.get(i);
 247         g->getPosition(pos);
 248         g->getCompression(cmpr);
 249         Math::add3(cmpr, pos, pos);
 250         float dist = ground[3] - Math::dot3(pos, ground);
 251         if(dist < 0) {
 252             printf("CRASH: gear %d\n", i);
 253             *(int*)0=0;
 254         }
 255     }
 256 }
 257
 258 // Returns a unit "down" vector for the ground in out, and the
 259 // distance from the local origin to the ground as the return value.
 260 // So for a given position V, "dist - (V dot out)" will be the height
 261 // AGL.
 262 float Model::localGround(State* s, float* out)
 263 {
 264     // Get the ground's "down" vector, this can be in floats, because
 265     // we don't need positioning accuracy.  The direction has plenty
 266     // of accuracy after truncation.
 267     out[0] = -(float)_ground[0];
 268     out[1] = -(float)_ground[1];
 269     out[2] = -(float)_ground[2];
 270     Math::vmul33(s->orient, out, out);
 271
 272     // The distance from the ground to the Aircraft's origin:
 273     double dist = (s->pos[0]*_ground[0]
 274                    + s->pos[1]*_ground[1]
 275                    + s->pos[2]*_ground[2] - _ground[3]);
 276
 277     return (float)dist;
 278 }
 279
 280 // Calculates the airflow direction at the given point and for the
 281 // specified aircraft velocity.
 282 void Model::localWind(float* pos, State* s, float* out)
 283 {
 284     // Most of the input is in global coordinates.  Fix that.
 285     float lwind[3], lrot[3], lv[3];
 286     Math::vmul33(s->orient, _wind, lwind);
 287     Math::vmul33(s->orient, s->rot, lrot);
 288     Math::vmul33(s->orient, s->v, lv);
 289
 290     _body.pointVelocity(pos, lrot, out); // rotational velocity
 291     Math::mul3(-1, out, out);      //  (negated)
 292     Math::add3(lwind, out, out);    //  + wind
 293     Math::sub3(out, lv, out);       //  - velocity
 294 }
 295
 296 }; // namespace yasim