3 #include "Atmosphere.hpp"
4 #include "Thruster.hpp"
6 #include "RigidBody.hpp"
7 #include "Integrator.hpp"
8 #include "Propeller.hpp"
9 #include "PistonEngine.hpp"
11 #include "Surface.hpp"
13 #include "Rotorpart.hpp"
14 #include "Rotorblade.hpp"
20 void printState(State* 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);
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]);
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]);
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]);
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);
60 // FIXME: who owns these things? Need a policy
63 void Model::getThrust(float* out)
66 out[0] = out[1] = out[2] = 0;
68 for(i=0; i<_thrusters.size(); i++) {
69 Thruster* t = (Thruster*)_thrusters.get(i);
71 Math::add3(tmp, out, out);
75 void Model::initIteration(float dt)
77 // Precompute torque and angular momentum for the thrusters
80 _gyro[i] = _torque[i] = 0;
81 for(i=0; i<_thrusters.size(); i++) {
82 Thruster* t = (Thruster*)_thrusters.get(i);
84 // Get the wind velocity at the thruster location
87 localWind(pos, _s, v);
90 t->setAir(_pressure, _temp, _rho);
91 t->integrate(_integrator.getInterval());
94 Math::add3(v, _torque, _torque);
97 Math::add3(v, _gyro, _gyro);
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);
110 for(i=0; i<_rotorparts.size(); i++) {
111 Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
112 rp->inititeration(dt,lrot);
114 for(i=0; i<_rotorblades.size(); i++) {
115 Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
116 rp->inititeration(dt,lrot);
121 void Model::iterate(float dt)
124 _body.recalc(); // FIXME: amortize this, somehow
125 _integrator.calcNewInterval();
128 bool Model::isCrashed()
133 void Model::setCrashed(bool crashed)
138 float Model::getAGL()
143 State* Model::getState()
148 void Model::resetState()
154 void Model::setState(State* s)
156 _integrator.setState(s);
157 _s = _integrator.getState();
160 RigidBody* Model::getBody()
165 Integrator* Model::getIntegrator()
170 Surface* Model::getSurface(int handle)
172 return (Surface*)_surfaces.get(handle);
175 Rotorpart* Model::getRotorpart(int handle)
177 return (Rotorpart*)_rotorparts.get(handle);
179 Rotorblade* Model::getRotorblade(int handle)
181 return (Rotorblade*)_rotorblades.get(handle);
183 Rotor* Model::getRotor(int handle)
185 return (Rotor*)_rotors.get(handle);
188 int Model::addThruster(Thruster* t)
190 return _thrusters.add(t);
193 int Model::numThrusters()
195 return _thrusters.size();
198 Thruster* Model::getThruster(int handle)
200 return (Thruster*)_thrusters.get(handle);
203 void Model::setThruster(int handle, Thruster* t)
205 _thrusters.set(handle, t);
208 int Model::addSurface(Surface* surf)
210 return _surfaces.add(surf);
213 int Model::addRotorpart(Rotorpart* rpart)
215 return _rotorparts.add(rpart);
217 int Model::addRotorblade(Rotorblade* rblade)
219 return _rotorblades.add(rblade);
221 int Model::addRotor(Rotor* r)
223 return _rotors.add(r);
226 int Model::addGear(Gear* gear)
228 return _gears.add(gear);
231 void Model::setGroundEffect(float* pos, float span, float mul)
233 Math::set3(pos, _wingCenter);
234 _groundEffectSpan = span;
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)
245 for(i=0; i<3; i++) _ground[i] = planeNormal[i];
246 _ground[3] = fromOrigin;
249 void Model::setAir(float pressure, float temp, float density)
251 _pressure = pressure;
256 void Model::setWind(float* wind)
258 Math::set3(wind, _wind);
261 void Model::calcForces(State* s)
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
269 _body.setGyro(_gyro);
270 _body.addTorque(_torque);
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);
277 _body.addForce(pos, thrust);
280 // Gravity, convert to a force, then to local coordinates
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);
287 // Do each surface, remembering that the local velocity at each
288 // point is different due to rotation.
290 faero[0] = faero[1] = faero[2] = 0;
291 for(i=0; i<_surfaces.size(); i++) {
292 Surface* sf = (Surface*)_surfaces.get(i);
294 // Vsurf = wind - velocity + (rot cross (cg - pos))
296 sf->getPosition(pos);
297 localWind(pos, s, vs);
299 float force[3], torque[3];
300 sf->calcForce(vs, _rho, force, torque);
301 Math::add3(faero, force, faero);
305 _body.addForce(pos, force);
306 _body.addTorque(torque);
308 for(i=0; i<_rotorparts.size(); i++) {
309 Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
311 // Vsurf = wind - velocity + (rot cross (cg - pos))
313 sf->getPosition(pos);
314 localWind(pos, s, vs);
316 float force[3], torque[3];
317 sf->calcForce(vs, _rho, force, torque);
318 //Math::add3(faero, force, faero);
320 sf->getPositionForceAttac(pos);
322 _body.addForce(pos, force);
323 _body.addTorque(torque);
325 for(i=0; i<_rotorblades.size(); i++) {
326 Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
328 // Vsurf = wind - velocity + (rot cross (cg - pos))
330 sf->getPosition(pos);
331 localWind(pos, s, vs);
333 float force[3], torque[3];
334 sf->calcForce(vs, _rho, force, torque);
335 //Math::add3(faero, force, faero);
337 sf->getPositionForceAttac(pos);
339 _body.addForce(pos, force);
340 _body.addTorque(torque);
346 //printf("cg: %5.3lf %5.3lf %5.3lf ",cg[0],cg[1],cg[2]);
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)
355 ground[3] = localGround(s, ground);
357 // Account for ground effect by multiplying the vertical force
358 // component by an amount linear with the fraction of the wingspan
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;
365 Math::mul3(fz, ground, faero);
366 _body.addForce(faero);
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);
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);
384 void Model::newState(State* s)
390 // Some simple collision detection
392 float ground[4], pos[3], cmpr[3];
393 ground[3] = localGround(s, ground);
395 for(i=0; i<_gears.size(); i++) {
396 Gear* g = (Gear*)_gears.get(i);
398 // Get the point of ground contact
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);
405 // Find the lowest one
410 if(_agl < -1) // Allow for some integration slop
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
418 float Model::localGround(State* s, float* out)
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);
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]);
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)
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);
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
452 }; // namespace yasim