6 #include "Atmosphere.hpp"
7 #include "Thruster.hpp"
9 #include "RigidBody.hpp"
10 #include "Integrator.hpp"
11 #include "Propeller.hpp"
12 #include "PistonEngine.hpp"
15 #include "Launchbar.hpp"
16 #include "Surface.hpp"
18 #include "Rotorpart.hpp"
27 void printState(State* s)
30 Math::vmul33(tmp.orient, tmp.v, tmp.v);
31 Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
32 Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
33 Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
35 printf("\nNEW STATE (LOCAL COORDS)\n");
36 printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
40 if(i != 0) printf(" ");
41 printf("%6.2f %6.2f %6.2f\n",
42 tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
44 printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
45 printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
46 printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
47 printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
54 for(i=0; i<3; i++) _wind[i] = 0;
56 _integrator.setBody(&_body);
57 _integrator.setEnvironment(this);
59 // Default value of 30 Hz
60 _integrator.setInterval(1.0f/30.0f);
65 _ground_cb = new Ground();
69 _groundEffectSpan = 0;
71 for(i=0; i<3; i++) _wingCenter[i] = 0;
73 _global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
74 _global_ground[3] = -100000;
80 // FIXME: who owns these things? Need a policy
84 for(int i=0; i<_hitches.size();i++)
85 delete (Hitch*)_hitches.get(i);
89 void Model::getThrust(float* out)
92 out[0] = out[1] = out[2] = 0;
94 for(i=0; i<_thrusters.size(); i++) {
95 Thruster* t = (Thruster*)_thrusters.get(i);
97 Math::add3(tmp, out, out);
101 void Model::initIteration()
103 // Precompute torque and angular momentum for the thrusters
106 _gyro[i] = _torque[i] = 0;
108 // Need a local altitude for the wind calculation
110 _s->planeGlobalToLocal(_global_ground, lground);
111 float alt = Math::abs(lground[3]);
113 for(i=0; i<_thrusters.size(); i++) {
114 Thruster* t = (Thruster*)_thrusters.get(i);
116 // Get the wind velocity at the thruster location
119 localWind(pos, _s, v, alt);
122 t->setAir(_pressure, _temp, _rho);
123 t->integrate(_integrator.getInterval());
126 Math::add3(v, _torque, _torque);
129 Math::add3(v, _gyro, _gyro);
132 // Displace the turbulence coordinates according to the local wind.
135 Math::mul3(_integrator.getInterval(), _wind, toff);
139 for(i=0; i<_hitches.size(); i++) {
140 Hitch* h = (Hitch*)_hitches.get(i);
141 h->integrate(_integrator.getInterval());
147 // This function initializes some variables for the rotor calculation
148 // Furthermore it integrates in "void Rotorpart::inititeration
149 // (float dt,float *rot)" the "rotor orientation" by omega*dt for the
150 // 3D-visualization of the heli only. and it compensates the rotation
151 // of the fuselage. The rotor does not follow the rotation of the fuselage.
152 // Therefore its rotation must be subtracted from the orientation of the
155 void Model::initRotorIteration()
157 float dt = _integrator.getInterval();
159 if (!_rotorgear.isInUse()) return;
160 Math::vmul33(_s->orient, _s->rot, lrot);
161 Math::mul3(dt,lrot,lrot);
162 _rotorgear.initRotorIteration(lrot,dt);
165 void Model::iterate()
168 initRotorIteration();
169 _body.recalc(); // FIXME: amortize this, somehow
170 _integrator.calcNewInterval();
173 bool Model::isCrashed()
178 void Model::setCrashed(bool crashed)
183 float Model::getAGL()
188 State* Model::getState()
193 void Model::setState(State* s)
195 _integrator.setState(s);
196 _s = _integrator.getState();
199 RigidBody* Model::getBody()
204 Integrator* Model::getIntegrator()
209 Surface* Model::getSurface(int handle)
211 return (Surface*)_surfaces.get(handle);
214 Rotorgear* Model::getRotorgear(void)
219 int Model::addThruster(Thruster* t)
221 return _thrusters.add(t);
224 Hook* Model::getHook(void)
229 Launchbar* Model::getLaunchbar(void)
234 int Model::numThrusters()
236 return _thrusters.size();
239 Thruster* Model::getThruster(int handle)
241 return (Thruster*)_thrusters.get(handle);
244 void Model::setThruster(int handle, Thruster* t)
246 _thrusters.set(handle, t);
249 int Model::addSurface(Surface* surf)
251 return _surfaces.add(surf);
254 int Model::addGear(Gear* gear)
256 return _gears.add(gear);
259 void Model::addHook(Hook* hook)
264 void Model::addLaunchbar(Launchbar* launchbar)
266 _launchbar = launchbar;
269 int Model::addHitch(Hitch* hitch)
271 return _hitches.add(hitch);
274 void Model::setGroundCallback(Ground* ground_cb)
277 _ground_cb = ground_cb;
280 Ground* Model::getGroundCallback(void)
285 void Model::setGroundEffect(float* pos, float span, float mul)
287 Math::set3(pos, _wingCenter);
288 _groundEffectSpan = span;
292 void Model::setAir(float pressure, float temp, float density)
294 _pressure = pressure;
299 void Model::setWind(float* wind)
301 Math::set3(wind, _wind);
304 void Model::updateGround(State* s)
307 _ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
311 for(i=0; i<_gears.size(); i++) {
312 Gear* g = (Gear*)_gears.get(i);
314 // Get the point of ground contact
315 float pos[3], cmpr[3];
317 g->getCompression(cmpr);
319 Math::mul3(g->getCompressFraction(), cmpr, cmpr);
320 Math::add3(cmpr, pos, pos);
321 // Transform the local coordinates of the contact point to
322 // global coordinates.
324 s->posLocalToGlobal(pos, pt);
326 // Ask for the ground plane in the global coordinate system
327 double global_ground[4];
329 const SGMaterial* material;
330 _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material);
331 g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material);
334 for(i=0; i<_hitches.size(); i++) {
335 Hitch* h = (Hitch*)_hitches.get(i);
337 // Get the point of interest
341 // Transform the local coordinates of the contact point to
342 // global coordinates.
344 s->posLocalToGlobal(pos, pt);
346 // Ask for the ground plane in the global coordinate system
347 double global_ground[4];
349 _ground_cb->getGroundPlane(pt, global_ground, global_vel);
350 h->setGlobalGround(global_ground, global_vel);
353 for(i=0; i<_rotorgear.getRotors()->size(); i++) {
354 Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
355 r->findGroundEffectAltitude(_ground_cb,s);
361 _hook->getTipGlobalPosition(s, pt);
362 double global_ground[4];
363 _ground_cb->getGroundPlane(pt, global_ground, dummy);
364 _hook->setGlobalGround(global_ground);
367 // The launchbar/holdback
370 _launchbar->getTipGlobalPosition(s, pt);
371 double global_ground[4];
372 _ground_cb->getGroundPlane(pt, global_ground, dummy);
373 _launchbar->setGlobalGround(global_ground);
377 void Model::calcForces(State* s)
379 // Add in the pre-computed stuff. These values aren't part of the
380 // Runge-Kutta integration (they don't depend on position or
381 // velocity), and are therefore constant across the four calls to
382 // calcForces. They get computed before we begin the integration
384 _body.setGyro(_gyro);
385 _body.addTorque(_torque);
387 for(i=0; i<_thrusters.size(); i++) {
388 Thruster* t = (Thruster*)_thrusters.get(i);
389 float thrust[3], pos[3];
390 t->getThrust(thrust);
392 _body.addForce(pos, thrust);
395 // Get a ground plane in local coordinates. The first three
396 // elements are the normal vector, the final one is the distance
397 // from the local origin along that vector to the ground plane
398 // (negative for objects "above" the ground)
400 s->planeGlobalToLocal(_global_ground, ground);
401 float alt = Math::abs(ground[3]);
403 // Gravity, convert to a force, then to local coordinates
405 Glue::geodUp(s->pos, grav);
406 Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
407 Math::vmul33(s->orient, grav, grav);
408 _body.addForce(grav);
410 // Do each surface, remembering that the local velocity at each
411 // point is different due to rotation.
413 faero[0] = faero[1] = faero[2] = 0;
414 for(i=0; i<_surfaces.size(); i++) {
415 Surface* sf = (Surface*)_surfaces.get(i);
417 // Vsurf = wind - velocity + (rot cross (cg - pos))
419 sf->getPosition(pos);
420 localWind(pos, s, vs, alt);
422 float force[3], torque[3];
423 sf->calcForce(vs, _rho, force, torque);
424 Math::add3(faero, force, faero);
426 _body.addForce(pos, force);
427 _body.addTorque(torque);
429 for (j=0; j<_rotorgear.getRotors()->size();j++)
431 Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
434 localWind(pos, s, vs, alt);
435 r->calcLiftFactor(vs, _rho,s);
437 // total torque of rotor (scalar) for calculating new rotor rpm
439 for(i=0; i<r->_rotorparts.size(); i++) {
440 float torque_scalar=0;
441 Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
443 // Vsurf = wind - velocity + (rot cross (cg - pos))
445 rp->getPosition(pos);
446 localWind(pos, s, vs, alt,true);
448 float force[3], torque[3];
449 rp->calcForce(vs, _rho, force, torque, &torque_scalar);
451 rp->getPositionForceAttac(pos);
453 _body.addForce(pos, force);
454 _body.addTorque(torque);
458 if (_rotorgear.isInUse())
461 _rotorgear.calcForces(torque);
462 _body.addTorque(torque);
465 // Account for ground effect by multiplying the vertical force
466 // component by an amount linear with the fraction of the wingspan
468 if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
470 float dist = ground[3] - Math::dot3(ground, _wingCenter);
471 if(dist > 0 && dist < _groundEffectSpan) {
472 float fz = Math::dot3(faero, ground);
473 fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
475 Math::mul3(fz, ground, faero);
476 _body.addForce(faero);
480 // Convert the velocity and rotation vectors to local coordinates
481 float lrot[3], lv[3];
482 Math::vmul33(s->orient, s->rot, lrot);
483 Math::vmul33(s->orient, s->v, lv);
486 for(i=0; i<_gears.size(); i++) {
487 float force[3], contact[3];
488 Gear* g = (Gear*)_gears.get(i);
490 g->calcForce(&_body, s, lv, lrot);
491 g->getForce(force, contact);
492 _body.addForce(contact, force);
497 _hook->calcForce(_ground_cb, &_body, s, lv, lrot);
498 float force[3], contact[3];
499 _hook->getForce(force, contact);
500 _body.addForce(contact, force);
503 // The launchbar/holdback
505 _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
506 float forcelb[3], contactlb[3], forcehb[3], contacthb[3];
507 _launchbar->getForce(forcelb, contactlb, forcehb, contacthb);
508 _body.addForce(contactlb, forcelb);
509 _body.addForce(contacthb, forcehb);
513 for(i=0; i<_hitches.size(); i++) {
514 float force[3], contact[3];
515 Hitch* h = (Hitch*)_hitches.get(i);
516 h->calcForce(_ground_cb,&_body, s);
517 h->getForce(force, contact);
518 _body.addForce(contact, force);
521 void Model::newState(State* s)
525 // Some simple collision detection
528 for(i=0; i<_gears.size(); i++) {
529 Gear* g = (Gear*)_gears.get(i);
531 if (!g->getSubmergable())
533 // Get the point of ground contact
534 float pos[3], cmpr[3];
536 g->getCompression(cmpr);
537 Math::mul3(g->getCompressFraction(), cmpr, cmpr);
538 Math::add3(cmpr, pos, pos);
540 // The plane transformed to local coordinates.
541 double global_ground[4];
542 g->getGlobalGround(global_ground);
544 s->planeGlobalToLocal(global_ground, ground);
545 float dist = ground[3] - Math::dot3(pos, ground);
547 // Find the lowest one
553 if(_agl < -1) // Allow for some integration slop
557 // Calculates the airflow direction at the given point and for the
558 // specified aircraft velocity.
559 void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
561 float tmp[3], lwind[3], lrot[3], lv[3];
563 // Get a global coordinate for our local position, and calculate
566 double gpos[3]; float up[3];
567 Math::tmul33(s->orient, pos, tmp);
568 for(int i=0; i<3; i++) {
569 gpos[i] = s->pos[i] + tmp[i];
571 Glue::geodUp(gpos, up);
572 _turb->getTurbulence(gpos, alt, up, lwind);
573 Math::add3(_wind, lwind, lwind);
575 Math::set3(_wind, lwind);
578 // Convert to local coordinates
579 Math::vmul33(s->orient, lwind, lwind);
580 Math::vmul33(s->orient, s->rot, lrot);
581 Math::vmul33(s->orient, s->v, lv);
583 _body.pointVelocity(pos, lrot, out); // rotational velocity
584 Math::mul3(-1, out, out); // (negated)
585 Math::add3(lwind, out, out); // + wind
586 Math::sub3(out, lv, out); // - velocity
588 //add the downwash of the rotors if it is not self a rotor
589 if (_rotorgear.isInUse()&&!is_rotor)
591 _rotorgear.getDownWash(pos,lv,tmp);
592 Math::add3(out,tmp, out); // + downwash
598 }; // namespace yasim