1 #include "Atmosphere.hpp"
2 #include "Thruster.hpp"
4 #include "RigidBody.hpp"
5 #include "Integrator.hpp"
6 #include "Propeller.hpp"
7 #include "PistonEngine.hpp"
11 #include "Rotorpart.hpp"
12 #include "Rotorblade.hpp"
19 void printState(State* 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);
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]);
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]);
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]);
46 for(i=0; i<3; i++) _wind[i] = 0;
48 _integrator.setBody(&_body);
49 _integrator.setEnvironment(this);
51 // Default value of 30 Hz
52 _integrator.setInterval(1.0f/30.0f);
61 // FIXME: who owns these things? Need a policy
64 void Model::getThrust(float* out)
67 out[0] = out[1] = out[2] = 0;
69 for(i=0; i<_thrusters.size(); i++) {
70 Thruster* t = (Thruster*)_thrusters.get(i);
72 Math::add3(tmp, out, out);
76 void Model::initIteration()
78 // Precompute torque and angular momentum for the thrusters
81 _gyro[i] = _torque[i] = 0;
83 // Need a local altitude for the wind calculation
85 float alt = Math::abs(localGround(_s, dummy));
87 for(i=0; i<_thrusters.size(); i++) {
88 Thruster* t = (Thruster*)_thrusters.get(i);
90 // Get the wind velocity at the thruster location
93 localWind(pos, _s, v, alt);
96 t->setAir(_pressure, _temp, _rho);
97 t->integrate(_integrator.getInterval());
100 Math::add3(v, _torque, _torque);
103 Math::add3(v, _gyro, _gyro);
106 // Displace the turbulence coordinates according to the local wind.
109 Math::mul3(_integrator.getInterval(), _wind, toff);
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.
120 void Model::initRotorIteration()
123 float dt = _integrator.getInterval();
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);
131 for(i=0; i<_rotorparts.size(); i++) {
132 Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
133 rp->inititeration(dt,lrot);
135 for(i=0; i<_rotorblades.size(); i++) {
136 Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
137 rp->inititeration(dt,lrot);
141 void Model::iterate()
144 initRotorIteration();
145 _body.recalc(); // FIXME: amortize this, somehow
146 _integrator.calcNewInterval();
149 bool Model::isCrashed()
154 void Model::setCrashed(bool crashed)
159 float Model::getAGL()
164 State* Model::getState()
169 void Model::setState(State* s)
171 _integrator.setState(s);
172 _s = _integrator.getState();
175 RigidBody* Model::getBody()
180 Integrator* Model::getIntegrator()
185 Surface* Model::getSurface(int handle)
187 return (Surface*)_surfaces.get(handle);
190 Rotorpart* Model::getRotorpart(int handle)
192 return (Rotorpart*)_rotorparts.get(handle);
194 Rotorblade* Model::getRotorblade(int handle)
196 return (Rotorblade*)_rotorblades.get(handle);
198 Rotor* Model::getRotor(int handle)
200 return (Rotor*)_rotors.get(handle);
203 int Model::addThruster(Thruster* t)
205 return _thrusters.add(t);
208 int Model::numThrusters()
210 return _thrusters.size();
213 Thruster* Model::getThruster(int handle)
215 return (Thruster*)_thrusters.get(handle);
218 void Model::setThruster(int handle, Thruster* t)
220 _thrusters.set(handle, t);
223 int Model::addSurface(Surface* surf)
225 return _surfaces.add(surf);
228 int Model::addRotorpart(Rotorpart* rpart)
230 return _rotorparts.add(rpart);
232 int Model::addRotorblade(Rotorblade* rblade)
234 return _rotorblades.add(rblade);
236 int Model::addRotor(Rotor* r)
238 return _rotors.add(r);
241 int Model::addGear(Gear* gear)
243 return _gears.add(gear);
246 void Model::setGroundEffect(float* pos, float span, float mul)
248 Math::set3(pos, _wingCenter);
249 _groundEffectSpan = span;
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)
259 for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
260 _ground[3] = fromOrigin;
263 void Model::setAir(float pressure, float temp, float density)
265 _pressure = pressure;
270 void Model::setWind(float* wind)
272 Math::set3(wind, _wind);
275 void Model::calcForces(State* s)
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
282 _body.setGyro(_gyro);
283 _body.addTorque(_torque);
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);
290 _body.addForce(pos, thrust);
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)
298 ground[3] = localGround(s, ground);
299 float alt = Math::abs(ground[3]);
301 // Gravity, convert to a force, then to local coordinates
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);
308 // Do each surface, remembering that the local velocity at each
309 // point is different due to rotation.
311 faero[0] = faero[1] = faero[2] = 0;
312 for(i=0; i<_surfaces.size(); i++) {
313 Surface* sf = (Surface*)_surfaces.get(i);
315 // Vsurf = wind - velocity + (rot cross (cg - pos))
317 sf->getPosition(pos);
318 localWind(pos, s, vs, alt);
320 float force[3], torque[3];
321 sf->calcForce(vs, _rho, force, torque);
322 Math::add3(faero, force, faero);
324 _body.addForce(pos, force);
325 _body.addTorque(torque);
327 for(i=0; i<_rotorparts.size(); i++) {
328 Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
330 // Vsurf = wind - velocity + (rot cross (cg - pos))
332 sf->getPosition(pos);
333 localWind(pos, s, vs, alt);
335 float force[3], torque[3];
336 sf->calcForce(vs, _rho, force, torque);
337 //Math::add3(faero, force, faero);
339 sf->getPositionForceAttac(pos);
341 _body.addForce(pos, force);
342 _body.addTorque(torque);
344 for(i=0; i<_rotorblades.size(); i++) {
345 Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
347 // Vsurf = wind - velocity + (rot cross (cg - pos))
349 sf->getPosition(pos);
350 localWind(pos, s, vs, alt);
352 float force[3], torque[3];
353 sf->calcForce(vs, _rho, force, torque);
354 //Math::add3(faero, force, faero);
356 sf->getPositionForceAttac(pos);
358 _body.addForce(pos, force);
359 _body.addTorque(torque);
362 // Account for ground effect by multiplying the vertical force
363 // component by an amount linear with the fraction of the wingspan
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;
370 Math::mul3(fz, ground, faero);
371 _body.addForce(faero);
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);
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);
389 void Model::newState(State* s)
393 // Some simple collision detection
395 float ground[4], pos[3], cmpr[3];
396 ground[3] = localGround(s, ground);
398 for(i=0; i<_gears.size(); i++) {
399 Gear* g = (Gear*)_gears.get(i);
401 // Get the point of ground contact
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);
408 // Find the lowest one
413 if(_agl < -1) // Allow for some integration slop
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
421 float Model::localGround(State* s, float* out)
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);
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]);
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)
443 float tmp[3], lwind[3], lrot[3], lv[3];
445 // Get a global coordinate for our local position, and calculate
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];
454 _turb->getTurbulence(gpos, alt, up, lwind);
455 Math::add3(_wind, lwind, lwind);
457 Math::set3(_wind, lwind);
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);
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
471 }; // namespace yasim