1 #include "Atmosphere.hpp"
2 #include "Thruster.hpp"
4 #include "RigidBody.hpp"
5 #include "Integrator.hpp"
6 #include "Propeller.hpp"
7 #include "PistonEngine.hpp"
10 #include "Launchbar.hpp"
11 #include "Surface.hpp"
13 #include "Rotorpart.hpp"
14 #include "Rotorblade.hpp"
22 void printState(State* s)
25 Math::vmul33(tmp.orient, tmp.v, tmp.v);
26 Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
27 Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
28 Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
30 printf("\nNEW STATE (LOCAL COORDS)\n");
31 printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
35 if(i != 0) printf(" ");
36 printf("%6.2f %6.2f %6.2f\n",
37 tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
39 printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
40 printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
41 printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
42 printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
49 for(i=0; i<3; i++) _wind[i] = 0;
51 _integrator.setBody(&_body);
52 _integrator.setEnvironment(this);
54 // Default value of 30 Hz
55 _integrator.setInterval(1.0f/30.0f);
60 _ground_cb = new Ground();
64 _groundEffectSpan = 0;
66 for(i=0; i<3; i++) _wingCenter[i] = 0;
68 _global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
69 _global_ground[3] = -100000;
75 // FIXME: who owns these things? Need a policy
81 void Model::getThrust(float* out)
84 out[0] = out[1] = out[2] = 0;
86 for(i=0; i<_thrusters.size(); i++) {
87 Thruster* t = (Thruster*)_thrusters.get(i);
89 Math::add3(tmp, out, out);
93 void Model::initIteration()
95 // Precompute torque and angular momentum for the thrusters
98 _gyro[i] = _torque[i] = 0;
100 // Need a local altitude for the wind calculation
102 _s->planeGlobalToLocal(_global_ground, lground);
103 float alt = Math::abs(lground[3]);
105 for(i=0; i<_thrusters.size(); i++) {
106 Thruster* t = (Thruster*)_thrusters.get(i);
108 // Get the wind velocity at the thruster location
111 localWind(pos, _s, v, alt);
114 t->setAir(_pressure, _temp, _rho);
115 t->integrate(_integrator.getInterval());
118 Math::add3(v, _torque, _torque);
121 Math::add3(v, _gyro, _gyro);
124 // Displace the turbulence coordinates according to the local wind.
127 Math::mul3(_integrator.getInterval(), _wind, toff);
134 // FIXME: This method looks to me like it's doing *integration*, not
135 // initialization. Integration code should ideally go into
136 // calcForces. Only very "unstiff" problems can be solved well like
137 // this (see the engine code for an example); I don't know if rotor
138 // dynamics qualify or not.
140 void Model::initRotorIteration()
143 float dt = _integrator.getInterval();
145 Math::vmul33(_s->orient, _s->rot, lrot);
146 Math::mul3(dt,lrot,lrot);
147 for(i=0; i<_rotors.size(); i++) {
148 Rotor* r = (Rotor*)_rotors.get(i);
149 r->inititeration(dt);
151 for(i=0; i<_rotorparts.size(); i++) {
152 Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
153 rp->inititeration(dt,lrot);
155 for(i=0; i<_rotorblades.size(); i++) {
156 Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
157 rp->inititeration(dt,lrot);
161 void Model::iterate()
164 initRotorIteration();
165 _body.recalc(); // FIXME: amortize this, somehow
166 _integrator.calcNewInterval();
169 bool Model::isCrashed()
174 void Model::setCrashed(bool crashed)
179 float Model::getAGL()
184 State* Model::getState()
189 void Model::setState(State* s)
191 _integrator.setState(s);
192 _s = _integrator.getState();
195 RigidBody* Model::getBody()
200 Integrator* Model::getIntegrator()
205 Surface* Model::getSurface(int handle)
207 return (Surface*)_surfaces.get(handle);
210 Rotorpart* Model::getRotorpart(int handle)
212 return (Rotorpart*)_rotorparts.get(handle);
214 Rotorblade* Model::getRotorblade(int handle)
216 return (Rotorblade*)_rotorblades.get(handle);
218 Rotor* Model::getRotor(int handle)
220 return (Rotor*)_rotors.get(handle);
223 int Model::addThruster(Thruster* t)
225 return _thrusters.add(t);
228 Hook* Model::getHook(void)
233 Launchbar* Model::getLaunchbar(void)
238 int Model::numThrusters()
240 return _thrusters.size();
243 Thruster* Model::getThruster(int handle)
245 return (Thruster*)_thrusters.get(handle);
248 void Model::setThruster(int handle, Thruster* t)
250 _thrusters.set(handle, t);
253 int Model::addSurface(Surface* surf)
255 return _surfaces.add(surf);
258 int Model::addRotorpart(Rotorpart* rpart)
260 return _rotorparts.add(rpart);
262 int Model::addRotorblade(Rotorblade* rblade)
264 return _rotorblades.add(rblade);
266 int Model::addRotor(Rotor* r)
268 return _rotors.add(r);
271 int Model::addGear(Gear* gear)
273 return _gears.add(gear);
276 void Model::addHook(Hook* hook)
281 void Model::addLaunchbar(Launchbar* launchbar)
283 _launchbar = launchbar;
286 void Model::setGroundCallback(Ground* ground_cb)
289 _ground_cb = ground_cb;
292 Ground* Model::getGroundCallback(void)
297 void Model::setGroundEffect(float* pos, float span, float mul)
299 Math::set3(pos, _wingCenter);
300 _groundEffectSpan = span;
304 void Model::setAir(float pressure, float temp, float density)
306 _pressure = pressure;
311 void Model::setWind(float* wind)
313 Math::set3(wind, _wind);
316 void Model::updateGround(State* s)
319 _ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
323 for(i=0; i<_gears.size(); i++) {
324 Gear* g = (Gear*)_gears.get(i);
326 // Get the point of ground contact
327 float pos[3], cmpr[3];
329 g->getCompression(cmpr);
331 Math::mul3(g->getCompressFraction(), cmpr, cmpr);
332 Math::add3(cmpr, pos, pos);
333 // Transform the local coordinates of the contact point to
334 // global coordinates.
336 s->posLocalToGlobal(pos, pt);
338 // Ask for the ground plane in the global coordinate system
339 double global_ground[4];
341 _ground_cb->getGroundPlane(pt, global_ground, global_vel);
342 g->setGlobalGround(global_ground, global_vel);
348 _hook->getTipGlobalPosition(s, pt);
349 double global_ground[4];
350 _ground_cb->getGroundPlane(pt, global_ground, dummy);
351 _hook->setGlobalGround(global_ground);
354 // The launchbar/holdback
357 _launchbar->getTipGlobalPosition(s, pt);
358 double global_ground[4];
359 _ground_cb->getGroundPlane(pt, global_ground, dummy);
360 _launchbar->setGlobalGround(global_ground);
364 void Model::calcForces(State* s)
366 // Add in the pre-computed stuff. These values aren't part of the
367 // Runge-Kutta integration (they don't depend on position or
368 // velocity), and are therefore constant across the four calls to
369 // calcForces. They get computed before we begin the integration
371 _body.setGyro(_gyro);
372 _body.addTorque(_torque);
374 for(i=0; i<_thrusters.size(); i++) {
375 Thruster* t = (Thruster*)_thrusters.get(i);
376 float thrust[3], pos[3];
377 t->getThrust(thrust);
379 _body.addForce(pos, thrust);
382 // Get a ground plane in local coordinates. The first three
383 // elements are the normal vector, the final one is the distance
384 // from the local origin along that vector to the ground plane
385 // (negative for objects "above" the ground)
387 s->planeGlobalToLocal(_global_ground, ground);
388 float alt = Math::abs(ground[3]);
390 // Gravity, convert to a force, then to local coordinates
392 Glue::geodUp(s->pos, grav);
393 Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
394 Math::vmul33(s->orient, grav, grav);
395 _body.addForce(grav);
397 // Do each surface, remembering that the local velocity at each
398 // point is different due to rotation.
400 faero[0] = faero[1] = faero[2] = 0;
401 for(i=0; i<_surfaces.size(); i++) {
402 Surface* sf = (Surface*)_surfaces.get(i);
404 // Vsurf = wind - velocity + (rot cross (cg - pos))
406 sf->getPosition(pos);
407 localWind(pos, s, vs, alt);
409 float force[3], torque[3];
410 sf->calcForce(vs, _rho, force, torque);
411 Math::add3(faero, force, faero);
413 _body.addForce(pos, force);
414 _body.addTorque(torque);
416 for(i=0; i<_rotorparts.size(); i++) {
417 Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
419 // Vsurf = wind - velocity + (rot cross (cg - pos))
421 sf->getPosition(pos);
422 localWind(pos, s, vs, alt);
424 float force[3], torque[3];
425 sf->calcForce(vs, _rho, force, torque);
426 //Math::add3(faero, force, faero);
428 sf->getPositionForceAttac(pos);
430 _body.addForce(pos, force);
431 _body.addTorque(torque);
433 for(i=0; i<_rotorblades.size(); i++) {
434 Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
436 // Vsurf = wind - velocity + (rot cross (cg - pos))
438 sf->getPosition(pos);
439 localWind(pos, s, vs, alt);
441 float force[3], torque[3];
442 sf->calcForce(vs, _rho, force, torque);
443 //Math::add3(faero, force, faero);
445 sf->getPositionForceAttac(pos);
447 _body.addForce(pos, force);
448 _body.addTorque(torque);
451 // Account for ground effect by multiplying the vertical force
452 // component by an amount linear with the fraction of the wingspan
454 float dist = ground[3] - Math::dot3(ground, _wingCenter);
455 if(dist > 0 && dist < _groundEffectSpan) {
456 float fz = Math::dot3(faero, ground);
457 fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
459 Math::mul3(fz, ground, faero);
460 _body.addForce(faero);
463 // Convert the velocity and rotation vectors to local coordinates
464 float lrot[3], lv[3];
465 Math::vmul33(s->orient, s->rot, lrot);
466 Math::vmul33(s->orient, s->v, lv);
469 for(i=0; i<_gears.size(); i++) {
470 float force[3], contact[3];
471 Gear* g = (Gear*)_gears.get(i);
473 g->calcForce(&_body, s, lv, lrot);
474 g->getForce(force, contact);
475 _body.addForce(contact, force);
480 float v[3], rot[3], glvel[3], ground[3];
481 _hook->calcForce(_ground_cb, &_body, s, lv, lrot);
482 float force[3], contact[3];
483 _hook->getForce(force, contact);
484 _body.addForce(contact, force);
487 // The launchbar/holdback
489 float v[3], rot[3], glvel[3], ground[3];
490 _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
491 float forcelb[3], contactlb[3], forcehb[3], contacthb[3];
492 _launchbar->getForce(forcelb, contactlb, forcehb, contacthb);
493 _body.addForce(contactlb, forcelb);
494 _body.addForce(contacthb, forcehb);
498 void Model::newState(State* s)
502 // Some simple collision detection
505 for(i=0; i<_gears.size(); i++) {
506 Gear* g = (Gear*)_gears.get(i);
508 // Get the point of ground contact
509 float pos[3], cmpr[3];
511 g->getCompression(cmpr);
512 Math::mul3(g->getCompressFraction(), cmpr, cmpr);
513 Math::add3(cmpr, pos, pos);
515 // The plane transformed to local coordinates.
516 double global_ground[4];
517 g->getGlobalGround(global_ground);
519 s->planeGlobalToLocal(global_ground, ground);
520 float dist = ground[3] - Math::dot3(pos, ground);
522 // Find the lowest one
527 if(_agl < -1) // Allow for some integration slop
531 // Calculates the airflow direction at the given point and for the
532 // specified aircraft velocity.
533 void Model::localWind(float* pos, State* s, float* out, float alt)
535 float tmp[3], lwind[3], lrot[3], lv[3];
537 // Get a global coordinate for our local position, and calculate
540 double gpos[3]; float up[3];
541 Math::tmul33(s->orient, pos, tmp);
542 for(int i=0; i<3; i++) {
543 gpos[i] = s->pos[i] + tmp[i];
545 Glue::geodUp(gpos, up);
546 _turb->getTurbulence(gpos, alt, up, lwind);
547 Math::add3(_wind, lwind, lwind);
549 Math::set3(_wind, lwind);
552 // Convert to local coordinates
553 Math::vmul33(s->orient, lwind, lwind);
554 Math::vmul33(s->orient, s->rot, lrot);
555 Math::vmul33(s->orient, s->v, lv);
557 _body.pointVelocity(pos, lrot, out); // rotational velocity
558 Math::mul3(-1, out, out); // (negated)
559 Math::add3(lwind, out, out); // + wind
560 Math::sub3(out, lv, out); // - velocity
563 }; // namespace yasim